Repository: coal-library/coal Branch: devel Commit: 60b3e498ca40 Files: 500 Total size: 4.3 MB Directory structure: gitextract_n08c_ker/ ├── .clang-format ├── .gersemirc ├── .git-blame-ignore-revs ├── .gitattributes ├── .github/ │ ├── dependabot.yml │ └── workflows/ │ ├── check-changelog.yml │ ├── dockgen.yml │ ├── macos-linux-pip.yml │ ├── macos-linux-windows-pixi.yml │ ├── nix.yml │ ├── ros_ci.yml │ ├── update-flake-lock.yml │ └── update_pixi_lockfile.yml ├── .gitignore ├── .gitlab-ci.yml ├── .gitmodules ├── .pre-commit-config.yaml ├── CHANGELOG.md ├── CITATION.bib ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE ├── README.md ├── colcon.pkg ├── development/ │ ├── build.md │ ├── release.md │ └── scripts/ │ └── pixi/ │ ├── activation.bat │ └── activation.sh ├── doc/ │ ├── CMakeLists.txt │ ├── Doxyfile.extra.in │ ├── generate_distance_plot.py │ ├── gjk.py │ ├── mesh-mesh-collision-call.dot │ ├── notes │ ├── python/ │ │ ├── doxygen-boost.hh │ │ ├── doxygen.hh │ │ ├── doxygen_xml_parser.py │ │ └── xml_docstring.py │ ├── shape-mesh-collision-call.dot │ └── shape-shape-collision-call.dot ├── dockgen.toml ├── flake.nix ├── hpp-fclConfig.cmake ├── include/ │ ├── coal/ │ │ ├── BV/ │ │ │ ├── AABB.h │ │ │ ├── BV.h │ │ │ ├── BV_node.h │ │ │ ├── OBB.h │ │ │ ├── OBBRSS.h │ │ │ ├── RSS.h │ │ │ ├── kDOP.h │ │ │ └── kIOS.h │ │ ├── BVH/ │ │ │ ├── BVH_front.h │ │ │ ├── BVH_internal.h │ │ │ ├── BVH_model.h │ │ │ └── BVH_utility.h │ │ ├── alloca.h │ │ ├── broadphase/ │ │ │ ├── broadphase.h │ │ │ ├── broadphase_SSaP.h │ │ │ ├── broadphase_SaP.h │ │ │ ├── broadphase_bruteforce.h │ │ │ ├── broadphase_callbacks.h │ │ │ ├── broadphase_collision_manager.h │ │ │ ├── broadphase_continuous_collision_manager-inl.h │ │ │ ├── broadphase_continuous_collision_manager.h │ │ │ ├── broadphase_dynamic_AABB_tree-inl.h │ │ │ ├── broadphase_dynamic_AABB_tree.h │ │ │ ├── broadphase_dynamic_AABB_tree_array-inl.h │ │ │ ├── broadphase_dynamic_AABB_tree_array.h │ │ │ ├── broadphase_interval_tree.h │ │ │ ├── broadphase_spatialhash-inl.h │ │ │ ├── broadphase_spatialhash.h │ │ │ ├── default_broadphase_callbacks.h │ │ │ └── detail/ │ │ │ ├── hierarchy_tree-inl.h │ │ │ ├── hierarchy_tree.h │ │ │ ├── hierarchy_tree_array-inl.h │ │ │ ├── hierarchy_tree_array.h │ │ │ ├── interval_tree.h │ │ │ ├── interval_tree_node-inl.h │ │ │ ├── interval_tree_node.h │ │ │ ├── morton-inl.h │ │ │ ├── morton.h │ │ │ ├── node_base-inl.h │ │ │ ├── node_base.h │ │ │ ├── node_base_array-inl.h │ │ │ ├── node_base_array.h │ │ │ ├── simple_hash_table-inl.h │ │ │ ├── simple_hash_table.h │ │ │ ├── simple_interval-inl.h │ │ │ ├── simple_interval.h │ │ │ ├── sparse_hash_table-inl.h │ │ │ ├── sparse_hash_table.h │ │ │ ├── spatial_hash-inl.h │ │ │ └── spatial_hash.h │ │ ├── collision.h │ │ ├── collision_data.h │ │ ├── collision_func_matrix.h │ │ ├── collision_object.h │ │ ├── collision_utility.h │ │ ├── contact_patch/ │ │ │ ├── contact_patch_simplifier.h │ │ │ ├── contact_patch_solver.h │ │ │ ├── contact_patch_solver.hxx │ │ │ └── polygon_convex_hull.h │ │ ├── contact_patch.h │ │ ├── contact_patch_func_matrix.h │ │ ├── data_types.h │ │ ├── distance.h │ │ ├── distance_func_matrix.h │ │ ├── doc.hh │ │ ├── fwd.hh │ │ ├── hfield.h │ │ ├── internal/ │ │ │ ├── BV_fitter.h │ │ │ ├── BV_splitter.h │ │ │ ├── intersect.h │ │ │ ├── intersect.hxx │ │ │ ├── shape_shape_contact_patch_func.h │ │ │ ├── shape_shape_func.h │ │ │ ├── tools.h │ │ │ ├── traversal.h │ │ │ ├── traversal_node_base.h │ │ │ ├── traversal_node_bvh_hfield.h │ │ │ ├── traversal_node_bvh_shape.h │ │ │ ├── traversal_node_bvhs.h │ │ │ ├── traversal_node_hfield_shape.h │ │ │ ├── traversal_node_octree.h │ │ │ ├── traversal_node_setup.h │ │ │ ├── traversal_node_shapes.h │ │ │ └── traversal_recurse.h │ │ ├── logging.h │ │ ├── math/ │ │ │ ├── matrix_3f.h │ │ │ ├── transform.h │ │ │ ├── types.h │ │ │ └── vec_3f.h │ │ ├── mesh_loader/ │ │ │ ├── assimp.h │ │ │ └── loader.h │ │ ├── narrowphase/ │ │ │ ├── gjk.h │ │ │ ├── minkowski_difference.h │ │ │ ├── narrowphase.h │ │ │ ├── narrowphase_defaults.h │ │ │ ├── support_data.h │ │ │ └── support_functions.h │ │ ├── octree.h │ │ ├── serialization/ │ │ │ ├── AABB.h │ │ │ ├── BVH_model.h │ │ │ ├── BV_node.h │ │ │ ├── BV_splitter.h │ │ │ ├── OBB.h │ │ │ ├── OBBRSS.h │ │ │ ├── RSS.h │ │ │ ├── archive.h │ │ │ ├── collision_data.h │ │ │ ├── collision_object.h │ │ │ ├── contact_patch.h │ │ │ ├── convex.h │ │ │ ├── eigen.h │ │ │ ├── fwd.h │ │ │ ├── geometric_shapes.h │ │ │ ├── hfield.h │ │ │ ├── kDOP.h │ │ │ ├── kIOS.h │ │ │ ├── memory.h │ │ │ ├── octree.h │ │ │ ├── quadrilateral.h │ │ │ ├── serializer.h │ │ │ ├── transform.h │ │ │ └── triangle.h │ │ ├── shape/ │ │ │ ├── convex.h │ │ │ ├── convex.hxx │ │ │ ├── geometric_shape_to_BVH_model.h │ │ │ ├── geometric_shapes.h │ │ │ ├── geometric_shapes.hxx │ │ │ ├── geometric_shapes_traits.h │ │ │ └── geometric_shapes_utility.h │ │ ├── shared_ptr_comparison.h │ │ ├── third_party/ │ │ │ └── boost/ │ │ │ └── core/ │ │ │ ├── data.hpp │ │ │ ├── detail/ │ │ │ │ └── assert.hpp │ │ │ ├── make_span.hpp │ │ │ └── span.hpp │ │ └── timings.h │ └── hpp/ │ └── fcl/ │ ├── BV/ │ │ ├── AABB.h │ │ ├── BV.h │ │ ├── BV_node.h │ │ ├── OBB.h │ │ ├── OBBRSS.h │ │ ├── RSS.h │ │ ├── kDOP.h │ │ └── kIOS.h │ ├── BVH/ │ │ ├── BVH_front.h │ │ ├── BVH_internal.h │ │ ├── BVH_model.h │ │ └── BVH_utility.h │ ├── broadphase/ │ │ ├── broadphase.h │ │ ├── broadphase_SSaP.h │ │ ├── broadphase_SaP.h │ │ ├── broadphase_bruteforce.h │ │ ├── broadphase_callbacks.h │ │ ├── broadphase_collision_manager.h │ │ ├── broadphase_continuous_collision_manager-inl.h │ │ ├── broadphase_continuous_collision_manager.h │ │ ├── broadphase_dynamic_AABB_tree-inl.h │ │ ├── broadphase_dynamic_AABB_tree.h │ │ ├── broadphase_dynamic_AABB_tree_array-inl.h │ │ ├── broadphase_dynamic_AABB_tree_array.h │ │ ├── broadphase_interval_tree.h │ │ ├── broadphase_spatialhash-inl.h │ │ ├── broadphase_spatialhash.h │ │ ├── default_broadphase_callbacks.h │ │ └── detail/ │ │ ├── hierarchy_tree-inl.h │ │ ├── hierarchy_tree.h │ │ ├── hierarchy_tree_array-inl.h │ │ ├── hierarchy_tree_array.h │ │ ├── interval_tree.h │ │ ├── interval_tree_node.h │ │ ├── morton-inl.h │ │ ├── morton.h │ │ ├── node_base-inl.h │ │ ├── node_base.h │ │ ├── node_base_array-inl.h │ │ ├── node_base_array.h │ │ ├── simple_hash_table-inl.h │ │ ├── simple_hash_table.h │ │ ├── simple_interval-inl.h │ │ ├── simple_interval.h │ │ ├── sparse_hash_table-inl.h │ │ ├── sparse_hash_table.h │ │ ├── spatial_hash-inl.h │ │ └── spatial_hash.h │ ├── coal.hpp │ ├── collision.h │ ├── collision_data.h │ ├── collision_func_matrix.h │ ├── collision_object.h │ ├── collision_utility.h │ ├── config.hh │ ├── contact_patch/ │ │ ├── contact_patch_solver.h │ │ └── contact_patch_solver.hxx │ ├── contact_patch.h │ ├── contact_patch_func_matrix.h │ ├── data_types.h │ ├── deprecated.hh │ ├── distance.h │ ├── distance_func_matrix.h │ ├── fwd.hh │ ├── hfield.h │ ├── internal/ │ │ ├── BV_fitter.h │ │ ├── BV_splitter.h │ │ ├── intersect.h │ │ ├── shape_shape_contact_patch_func.h │ │ ├── shape_shape_func.h │ │ ├── tools.h │ │ ├── traversal.h │ │ ├── traversal_node_base.h │ │ ├── traversal_node_bvh_shape.h │ │ ├── traversal_node_bvhs.h │ │ ├── traversal_node_hfield_shape.h │ │ ├── traversal_node_octree.h │ │ ├── traversal_node_setup.h │ │ ├── traversal_node_shapes.h │ │ └── traversal_recurse.h │ ├── logging.h │ ├── math/ │ │ ├── matrix_3f.h │ │ ├── transform.h │ │ ├── types.h │ │ └── vec_3f.h │ ├── mesh_loader/ │ │ ├── assimp.h │ │ └── loader.h │ ├── narrowphase/ │ │ ├── gjk.h │ │ ├── minkowski_difference.h │ │ ├── narrowphase.h │ │ ├── narrowphase_defaults.h │ │ ├── support_data.h │ │ ├── support_functions.h │ │ └── support_functions.hxx │ ├── octree.h │ ├── serialization/ │ │ ├── AABB.h │ │ ├── BVH_model.h │ │ ├── BV_node.h │ │ ├── BV_splitter.h │ │ ├── OBB.h │ │ ├── OBBRSS.h │ │ ├── RSS.h │ │ ├── archive.h │ │ ├── collision_data.h │ │ ├── collision_object.h │ │ ├── contact_patch.h │ │ ├── convex.h │ │ ├── eigen.h │ │ ├── fwd.h │ │ ├── geometric_shapes.h │ │ ├── hfield.h │ │ ├── kDOP.h │ │ ├── kIOS.h │ │ ├── memory.h │ │ ├── octree.h │ │ ├── quadrilateral.h │ │ ├── serializer.h │ │ ├── transform.h │ │ └── triangle.h │ ├── shape/ │ │ ├── convex.h │ │ ├── convex.hxx │ │ ├── geometric_shape_to_BVH_model.h │ │ ├── geometric_shapes.h │ │ ├── geometric_shapes.hxx │ │ ├── geometric_shapes_traits.h │ │ └── geometric_shapes_utility.h │ ├── timings.h │ └── warning.hh ├── package.xml ├── pixi.toml ├── python/ │ ├── CMakeLists.txt │ ├── broadphase/ │ │ ├── broadphase.cc │ │ ├── broadphase_callbacks.hh │ │ ├── broadphase_collision_manager.hh │ │ └── fwd.hh │ ├── coal/ │ │ ├── __init__.py │ │ ├── viewer.py │ │ └── windows_dll_manager.py │ ├── coal.cc │ ├── coal.hh │ ├── collision-geometries.cc │ ├── collision.cc │ ├── contact_patch.cc │ ├── deprecation.hh │ ├── distance.cc │ ├── fwd.hh │ ├── gjk.cc │ ├── hppfcl/ │ │ ├── __init__.py │ │ └── viewer.py │ ├── math.cc │ ├── octree.cc │ ├── pickle.hh │ ├── printable.hh │ ├── serializable.hh │ ├── utils/ │ │ └── std-pair.hh │ └── version.cc ├── python-nb/ │ ├── CMakeLists.txt │ ├── aabb.cc │ ├── broadphase/ │ │ ├── broadphase.cc │ │ └── broadphase_callbacks_collision_manager.hh │ ├── bvh.cc │ ├── coal/ │ │ ├── __init__.py │ │ └── windows_dll_manager.py │ ├── collision-geometries.cc │ ├── collision.cc │ ├── contact_patch.cc │ ├── distance.cc │ ├── fwd.h │ ├── gjk.cc │ ├── height_field.cc │ ├── math.cc │ ├── memory-footprint.cc │ ├── module.cc │ ├── octree.cc │ ├── pickle.hh │ ├── serializable.hh │ └── shapes.cc ├── src/ │ ├── BV/ │ │ ├── AABB.cpp │ │ ├── OBB.cpp │ │ ├── OBB.h │ │ ├── OBBRSS.cpp │ │ ├── RSS.cpp │ │ ├── kDOP.cpp │ │ └── kIOS.cpp │ ├── BVH/ │ │ ├── BVH_model.cpp │ │ ├── BVH_utility.cpp │ │ ├── BV_fitter.cpp │ │ └── BV_splitter.cpp │ ├── CMakeLists.txt │ ├── broadphase/ │ │ ├── broadphase_SSaP.cpp │ │ ├── broadphase_SaP.cpp │ │ ├── broadphase_bruteforce.cpp │ │ ├── broadphase_collision_manager.cpp │ │ ├── broadphase_dynamic_AABB_tree.cpp │ │ ├── broadphase_dynamic_AABB_tree_array.cpp │ │ ├── broadphase_interval_tree.cpp │ │ ├── default_broadphase_callbacks.cpp │ │ └── detail/ │ │ ├── interval_tree.cpp │ │ ├── interval_tree_node.cpp │ │ ├── morton-inl.h │ │ ├── morton.cpp │ │ ├── simple_interval.cpp │ │ └── spatial_hash.cpp │ ├── collision.cpp │ ├── collision_data.cpp │ ├── collision_func_matrix.cpp │ ├── collision_node.cpp │ ├── collision_node.h │ ├── collision_object.cpp │ ├── collision_utility.cpp │ ├── contact_patch/ │ │ ├── contact_patch_simplifier.cpp │ │ ├── contact_patch_solver.cpp │ │ └── polygon_convex_hull.cpp │ ├── contact_patch.cpp │ ├── contact_patch_func_matrix.cpp │ ├── distance/ │ │ ├── box_halfspace.cpp │ │ ├── box_plane.cpp │ │ ├── box_sphere.cpp │ │ ├── capsule_capsule.cpp │ │ ├── capsule_halfspace.cpp │ │ ├── capsule_plane.cpp │ │ ├── cone_halfspace.cpp │ │ ├── cone_plane.cpp │ │ ├── convex_halfspace.cpp │ │ ├── convex_plane.cpp │ │ ├── cylinder_halfspace.cpp │ │ ├── cylinder_plane.cpp │ │ ├── ellipsoid_halfspace.cpp │ │ ├── ellipsoid_plane.cpp │ │ ├── halfspace_halfspace.cpp │ │ ├── halfspace_plane.cpp │ │ ├── plane_plane.cpp │ │ ├── sphere_capsule.cpp │ │ ├── sphere_cylinder.cpp │ │ ├── sphere_halfspace.cpp │ │ ├── sphere_plane.cpp │ │ ├── sphere_sphere.cpp │ │ ├── triangle_halfspace.cpp │ │ ├── triangle_plane.cpp │ │ ├── triangle_sphere.cpp │ │ └── triangle_triangle.cpp │ ├── distance.cpp │ ├── distance_func_matrix.cpp │ ├── hfield.cpp │ ├── intersect.cpp │ ├── math/ │ │ └── transform.cpp │ ├── mesh_loader/ │ │ ├── assimp.cpp │ │ └── loader.cpp │ ├── narrowphase/ │ │ ├── details.h │ │ ├── gjk.cpp │ │ ├── minkowski_difference.cpp │ │ └── support_functions.cpp │ ├── octree.cpp │ ├── serialization/ │ │ └── serialization.cpp │ ├── shape/ │ │ ├── geometric_shapes.cpp │ │ └── geometric_shapes_utility.cpp │ ├── traits_traversal.h │ └── traversal/ │ └── traversal_recurse.cpp └── test/ ├── CMakeLists.txt ├── accelerated_gjk.cpp ├── alloca.cpp ├── benchmark/ │ └── test_fcl_gjk.output ├── benchmark.cpp ├── box_box_collision.cpp ├── box_box_distance.cpp ├── broadphase.cpp ├── broadphase_collision_1.cpp ├── broadphase_collision_2.cpp ├── broadphase_dynamic_AABB_tree.cpp ├── bvh_models.cpp ├── capsule_box_1.cpp ├── capsule_box_2.cpp ├── capsule_capsule.cpp ├── collision.cpp ├── collision_node_asserts.cpp ├── contact_patch.cpp ├── convex.cpp ├── distance.cpp ├── distance_lower_bound.cpp ├── fcl_resources/ │ ├── config.h.in │ ├── env.obj │ ├── env.octree │ ├── illformated_mesh.dae │ ├── rob.obj │ └── staircases_koroibot_hr.dae ├── frontlist.cpp ├── geometric_shapes.cpp ├── gjk-geometric-tools-benchmark ├── gjk.cpp ├── gjk_asserts.cpp ├── gjk_convergence_criterion.cpp ├── hfields.cpp ├── math.cpp ├── normal_and_nearest_points.cpp ├── obb.cpp ├── octree.cpp ├── patch_simplifier.cpp ├── pixi_build/ │ └── CMakeLists.txt ├── polygon_convex_hull.cpp ├── profiling.cpp ├── python_unit/ │ ├── CMakeLists.txt │ ├── api.py │ ├── collision.py │ ├── collision_manager.py │ ├── geometric_shapes.py │ ├── pickling.py │ └── test_case.py ├── scripts/ │ ├── collision-bench.py │ ├── collision.py │ ├── distance_lower_bound.py │ ├── geometric_shapes.py │ ├── gjk.py │ ├── obb.py │ └── octree.py ├── security_margin.cpp ├── serialization.cpp ├── shape_inflation.cpp ├── shape_mesh_consistency.cpp ├── simple.cpp ├── swept_sphere_radius.cpp ├── utility.cpp └── utility.h ================================================ FILE CONTENTS ================================================ ================================================ FILE: .clang-format ================================================ BasedOnStyle: Google SortIncludes: false ================================================ FILE: .gersemirc ================================================ definitions: [./CMakeLists.txt,./cmake,./python,./src,./test] line_length: 80 indent: 2 warn_about_unknown_commands: false ================================================ FILE: .git-blame-ignore-revs ================================================ 0067c8aa66aac548601e2a3fd029aa264cc59f2a 76b68f785df31b00e153290b45ec290a9c5f7963 # ruff --fix . (Guilhem Saurel, 2023-10-25) 02cef56abfacee590c8444fd379c8837bf007fa1 # black . (Guilhem Saurel, 2023-10-25) febfbcbe9c98cdb4e0c7bbf4554a6925b391834b # ruff --fix . (Guilhem Saurel, 2023-10-24) 58dee5ae90eded5125825a2da0fe76a5031f3334 # black . (Guilhem Saurel, 2023-10-24) 889ff8d1ca00b9e317e1da4136e233bb49a049df # pre-commit run -all (ManifoldFR, 2025-02-12) 3c3494086db02747bf858142bc3be5cbc244385f ================================================ FILE: .gitattributes ================================================ # SCM syntax highlighting pixi.lock linguist-language=YAML linguist-generated=true ================================================ FILE: .github/dependabot.yml ================================================ version: 2 updates: - package-ecosystem: github-actions directory: / target-branch: devel schedule: interval: monthly ================================================ FILE: .github/workflows/check-changelog.yml ================================================ name: Check-changelog on: pull_request: types: [assigned, opened, synchronize, reopened, labeled, unlabeled] branches: - devel jobs: check-changelog: name: Check changelog action runs-on: ubuntu-latest steps: - uses: tarides/changelog-check-action@v3 with: changelog: CHANGELOG.md ================================================ FILE: .github/workflows/dockgen.yml ================================================ name: "Publish docker image" on: push: branches: - devel tags: - "v*" jobs: dockgen: runs-on: "ubuntu-latest" permissions: contents: read packages: write attestations: write id-token: write steps: - uses: actions/checkout@v6 - uses: astral-sh/setup-uv@v7 - run: uvx dockgen - run: cat Dockerfile # https://docs.github.com/en/packages/managing-github-packages-using-github-actions-workflows/publishing-and-installing-a-package-with-github-actions - name: Log in to the Container registry uses: docker/login-action@v4 with: registry: ghcr.io username: ${{ github.actor }} password: ${{ secrets.GITHUB_TOKEN }} - name: Extract metadata (tags, labels) for Docker id: meta uses: docker/metadata-action@v6 with: images: ghcr.io/${{ github.repository }} - name: Build and push Docker image id: push uses: docker/build-push-action@v7 with: context: . push: true tags: ${{ steps.meta.outputs.tags }} labels: ${{ steps.meta.outputs.labels }} - name: Generate artifact attestation uses: actions/attest-build-provenance@v4 with: subject-name: ghcr.io/${{ github.repository }} subject-digest: ${{ steps.push.outputs.digest }} push-to-registry: true ================================================ FILE: .github/workflows/macos-linux-pip.yml ================================================ name: CI - MacOS/Linux via pip on: push: branches: - devel paths-ignore: - .gitlab-ci.yml - .gitignore - '**.md' - CITATION.* - LICENSE - colcon.pkg - .pre-commit-config.yaml pull_request: paths-ignore: - .gitlab-ci.yml - .gitignore - '**.md' - CITATION.* - LICENSE - colcon.pkg - .pre-commit-config.yaml concurrency: group: ${{ github.workflow }}-${{ github.ref }} cancel-in-progress: true env: CTEST_OUTPUT_ON_FAILURE: 1 CTEST_PARALLEL_LEVEL: 4 jobs: coal-pip: name: "CI on ${{ matrix.os }} / py ${{ matrix.python-version }} with pip" runs-on: "${{ matrix.os }}-latest" strategy: fail-fast: false matrix: os: ["ubuntu", "macos"] python-version: ["3.8", "3.9", "3.10", "3.11", "3.12", "3.13"] exclude: - os: "macos" python-version: "3.8" # Not available on arm64 steps: - uses: actions/checkout@v6 with: submodules: 'true' - uses: actions/setup-python@v6 with: python-version: ${{ matrix.python-version }} - run: python -m pip install -U pip - run: python -m pip install cmeel-assimp cmeel-octomap cmeel-qhull eigenpy[build] - run: echo "CMAKE_PREFIX_PATH=$(cmeel cmake)" >> $GITHUB_ENV - run: cmake -B build -S . -DPython_EXECUTABLE=$(which python) -DCOAL_HAS_QHULL=ON -DCOAL_PYTHON_NANOBIND=OFF - run: cmake --build build -j 4 - run: cmake --build build -t test ================================================ FILE: .github/workflows/macos-linux-windows-pixi.yml ================================================ name: CI - MacOS/Linux/Windows via Pixi on: push: branches: - devel paths-ignore: - .gitlab-ci.yml - .gitignore - '**.md' - CITATION.* - LICENSE - colcon.pkg - .pre-commit-config.yaml pull_request: paths-ignore: - .gitlab-ci.yml - .gitignore - '**.md' - CITATION.* - LICENSE - colcon.pkg - .pre-commit-config.yaml concurrency: group: ${{ github.workflow }}-${{ github.ref }} cancel-in-progress: true jobs: coal-pixi: name: Standard - ${{ matrix.os }} - Env ${{ matrix.environment }} ${{ matrix.build_type }} ${{ matrix.cxx_options }} runs-on: ${{ matrix.os }} env: CCACHE_BASEDIR: "${GITHUB_WORKSPACE}" CCACHE_DIR: "${GITHUB_WORKSPACE}/.ccache" CCACHE_COMPRESS: true CCACHE_COMPRESSLEVEL: 6 # Since pixi will install a compiler, the compiler mtime will be changed. # This can invalidate the cache (https://ccache.dev/manual/latest.html#config_compiler_check) CCACHE_COMPILERCHECK: content strategy: fail-fast: false matrix: os: [ubuntu-latest, macos-latest, macos-15-intel] environment: [all, all-python-oldest, all-boost-python] build_type: [Release, Debug] cxx_options: ['', '-mavx2'] exclude: - os: macos-latest cxx_options: '-mavx2' - os: macos-15-intel cxx_options: '-mavx2' include: - os: windows-latest environment: all cxx_options: '' build_type: Release - os: windows-latest environment: all-boost-python cxx_options: '' build_type: Release - os: windows-latest environment: all-clang-cl cxx_options: '' build_type: Release - os: windows-latest environment: default cxx_options: '' build_type: Debug steps: - uses: actions/checkout@v6 with: submodules: recursive - uses: actions/cache@v5 with: path: .ccache key: ccache-macos-linux-windows-pixi-${{ matrix.os }}-${{ matrix.build_type }}-${{ matrix.cxx_options }}-${{ matrix.environment }}-${{ github.sha }} restore-keys: ccache-macos-linux-windows-pixi-${{ matrix.os }}-${{ matrix.build_type }}-${{ matrix.cxx_options }}-${{ matrix.environment }}- - uses: prefix-dev/setup-pixi@v0.9.5 with: cache: true environments: ${{ matrix.environment }} - name: Clear ccache statistics [MacOS/Linux/Windows] run: | pixi run -e ${{ matrix.environment }} ccache -z - name: Build Coal [MacOS/Linux/Windows] env: CMAKE_BUILD_PARALLEL_LEVEL: 2 COAL_BUILD_TYPE: ${{ matrix.build_type }} COAL_CXX_FLAGS: ${{ matrix.cxx_options }} run: | pixi run -e ${{ matrix.environment }} build - name: Test Coal [MacOS/Linux/Windows] if: ${{ ! (contains(matrix.os, 'windows') && matrix.build_type == 'Debug') }} run: | pixi run -e ${{ matrix.environment }} ctest --test-dir build --output-on-failure - name: Show ccache statistics [MacOS/Linux/Windows] run: | pixi run -e ${{ matrix.environment }} ccache -sv coal-python-standalone-pixi: name: Python standalone - ${{ matrix.os }} - Env ${{ matrix.environment }} runs-on: ${{ matrix.os }} env: CCACHE_BASEDIR: "${GITHUB_WORKSPACE}" CCACHE_DIR: "${GITHUB_WORKSPACE}/.ccache" CCACHE_COMPRESS: true CCACHE_COMPRESSLEVEL: 6 # Since pixi will install a compiler, the compiler mtime will be changed. # This can invalidate the cache (https://ccache.dev/manual/latest.html#config_compiler_check) CCACHE_COMPILERCHECK: content strategy: fail-fast: false matrix: os: [ubuntu-latest, macos-latest, macos-15-intel, windows-latest] environment: [all, all-boost-python] steps: - uses: actions/checkout@v6 with: submodules: recursive - uses: actions/cache@v5 with: path: .ccache key: ccache-python-standalone-macos-linux-windows-pixi-${{ matrix.os }}-${{ matrix.environment }}-${{ github.sha }} restore-keys: ccache-python-standalone-macos-linux-windows-pixi-${{ matrix.os }}-${{ matrix.environment }}- - uses: prefix-dev/setup-pixi@v0.9.5 with: cache: true environments: ${{ matrix.environment }} - name: Clear ccache statistics [MacOS/Linux/Windows] run: | pixi run -e ${{ matrix.environment }} ccache -z - name: Build Coal cpp [MacOS/Linux/Windows] env: CMAKE_BUILD_PARALLEL_LEVEL: 2 shell: bash -el {0} run: | pixi run -e ${{ matrix.environment }} configure \ -DBUILD_PYTHON_INTERFACE=OFF \ -B build_cpp pixi run -e ${{ matrix.environment }} cmake --build build_cpp --target all pixi run -e ${{ matrix.environment }} cmake --install build_cpp - name: Build Coal standalone python [MacOS/Linux/Windows] env: CMAKE_BUILD_PARALLEL_LEVEL: 2 shell: bash -el {0} run: | pixi run -e ${{ matrix.environment }} configure \ -DBUILD_PYTHON_INTERFACE=ON \ -DBUILD_STANDALONE_PYTHON_INTERFACE=ON \ -B build_python pixi run -e ${{ matrix.environment }} cmake --build build_python --target all pixi run -e ${{ matrix.environment }} cmake --install build_python pixi run -e ${{ matrix.environment }} python -c "import coal" - name: Show ccache statistics [MacOS/Linux/Windows] run: | pixi run -e ${{ matrix.environment }} ccache -sv # coal-pixi-build: # name: Pixi build - ${{ matrix.os }} # runs-on: ${{ matrix.os }} # strategy: # fail-fast: false # matrix: # os: [ubuntu-latest, macos-latest, macos-15-intel, windows-latest] # steps: # - uses: actions/checkout@v6 # with: # submodules: recursive # - uses: prefix-dev/setup-pixi@v0.9.5 # env: # CMAKE_BUILD_PARALLEL_LEVEL: 2 # with: # cache: true # environments: test-pixi-build # - name: Test package [MacOS/Linux/Windows] # run: | # pixi run -e test-pixi-build test check: if: always() name: check-macos-linux-windows-pixi needs: - coal-pixi - coal-python-standalone-pixi # - coal-pixi-build runs-on: Ubuntu-latest steps: - name: Decide whether the needed jobs succeeded or failed uses: re-actors/alls-green@release/v1 with: jobs: ${{ toJSON(needs) }} ================================================ FILE: .github/workflows/nix.yml ================================================ name: "CI - Nix" on: push: branches: - devel pull_request: concurrency: group: ${{ github.workflow }}-${{ github.ref }} cancel-in-progress: true jobs: nix: runs-on: "${{ matrix.os }}-latest" strategy: matrix: os: [ubuntu, macos] extends: ["", "pkgs-eigen5", "pkgs-full", "pkgs-nanobind"] steps: - uses: actions/checkout@v6 - uses: cachix/install-nix-action@v31 - uses: cachix/cachix-action@v17 with: name: gepetto authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}' - run: nix flake check -L - run: nix build -L ".#${{ matrix.extends }}" check: if: always() name: check-macos-linux-nix runs-on: ubuntu-latest needs: - nix steps: - uses: re-actors/alls-green@release/v1 with: jobs: ${{ toJSON(needs) }} ================================================ FILE: .github/workflows/ros_ci.yml ================================================ # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) name: ROS-CI # This determines when this workflow is run on: push: branches: - devel paths-ignore: - .gitlab-ci.yml - .gitignore - '**.md' - CITATION.* - LICENSE - .pre-commit-config.yaml pull_request: paths-ignore: - .gitlab-ci.yml - .gitignore - '**.md' - CITATION.* - LICENSE - .pre-commit-config.yaml concurrency: group: ${{ github.workflow }}-${{ github.ref }} cancel-in-progress: true jobs: ros: name: ROS ${{ matrix.ROS_DISTRO }} - ${{ matrix.os }} strategy: fail-fast: false matrix: include: # ROS2 Humble Hawksbill (May 2022 - May 2027) - ROS_DISTRO: humble os: Ubuntu 22.04 (Jammy) # ROS2 Jazzy Jalisco (May 2024 - May 2029) - ROS_DISTRO: jazzy os: Ubuntu 24.04 (Noble) # ROS2 Kilted Kayu (May 2025 - December 2026) - ROS_DISTRO: kilted os: Ubuntu 24.04 (Noble) # ROS2 Rolling Ridley - ROS_DISTRO: rolling os: Ubuntu 24.04 (Noble) env: CMAKE_ARGS: -DBUILD_PYTHON_INTERFACE=ON -DCOAL_BUILD_TESTS=ON -DCOAL_BUILD_BENCHMARKS=ON VERBOSE_OUTPUT: true VERBOSE_TESTS: true ROS_DISTRO: ${{ matrix.ROS_DISTRO }} CCACHE_DIR: ${{ github.workspace }}/.ccache runs-on: ubuntu-latest steps: - uses: actions/checkout@v6 - uses: actions/cache@v5 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ matrix.ROS_DISTRO }}-${{github.run_id}} restore-keys: ccache-${{ matrix.ROS_DISTRO }}- - uses: ros-industrial/industrial_ci@f3c2dc8b4a9e6215f9c00f83e86ca0692970f81d check: if: always() name: check-ros-ci needs: - ros runs-on: ubuntu-latest steps: - name: Decide whether the needed jobs succeeded or failed uses: re-actors/alls-green@release/v1 with: jobs: ${{ toJSON(needs) }} ================================================ FILE: .github/workflows/update-flake-lock.yml ================================================ name: update-flake-lock on: workflow_dispatch: schedule: - cron: '0 11 12 * *' jobs: update-flake-inputs: runs-on: ubuntu-slim permissions: contents: write pull-requests: write steps: - name: Generate GitHub App Token id: app-token uses: actions/create-github-app-token@v3 with: app-id: ${{ secrets.GEPETTO_NIX_APP_ID }} private-key: ${{ secrets.GEPETTO_NIX_APP_PRIVATE_KEY }} - name: Checkout repository uses: actions/checkout@v6 with: token: ${{ steps.app-token.outputs.token }} - name: Setup Nix uses: cachix/install-nix-action@v31 - name: Update flake inputs uses: mic92/update-flake-inputs@v1 with: github-token: ${{ steps.app-token.outputs.token }} pr-labels: 'no-changelog' git-author-name: 'hrp2-14' git-author-email: '40568249+hrp2-14@users.noreply.github.com' ================================================ FILE: .github/workflows/update_pixi_lockfile.yml ================================================ name: CI - Update Pixi lockfile permissions: contents: write pull-requests: write on: workflow_dispatch: schedule: - cron: 0 5 1 * * jobs: pixi-update: runs-on: ubuntu-latest steps: - uses: actions/create-github-app-token@v3 id: generate-token with: app-id: ${{ secrets.APP_ID }} private-key: ${{ secrets.APP_PRIVATE_KEY }} - uses: actions/checkout@v6 with: token: ${{ steps.generate-token.outputs.token }} ref: devel # Make sure the value of GITHUB_TOKEN will not be persisted in repo's config persist-credentials: false - name: Set up pixi uses: prefix-dev/setup-pixi@v0.9.5 with: run-install: false - name: Update lockfile run: | set -o pipefail pixi update --json | pixi exec pixi-diff-to-markdown >> diff.md - name: Create pull request uses: peter-evans/create-pull-request@v8 with: token: ${{ steps.generate-token.outputs.token }} commit-message: 'pixi: Update pixi lockfile' title: Update pixi lockfile body-path: diff.md branch: topic/update-pixi base: devel labels: | pixi no changelog delete-branch: true add-paths: pixi.lock ================================================ FILE: .gitignore ================================================ build*/ Xcode/* *~ *.pyc .cache/ __pycache__/ # pixi environments .pixi *.egg-info ================================================ FILE: .gitlab-ci.yml ================================================ include: https://rainboard.laas.fr/project/coal/.gitlab-ci.yml ================================================ FILE: .gitmodules ================================================ [submodule "cmake"] branch = master path = cmake url = https://github.com/jrl-umi3218/jrl-cmakemodules.git ================================================ FILE: .pre-commit-config.yaml ================================================ ci: autoupdate_branch: devel autofix_prs: false autoupdate_schedule: quarterly submodules: true repos: - repo: https://github.com/astral-sh/ruff-pre-commit rev: v0.15.9 hooks: - id: ruff args: - --fix - --exit-non-zero-on-fix - id: ruff-format - repo: https://github.com/pre-commit/mirrors-clang-format rev: v22.1.2 hooks: - id: clang-format - repo: https://github.com/pre-commit/pre-commit-hooks rev: v6.0.0 hooks: - id: trailing-whitespace - repo: https://github.com/BlankSpruce/gersemi rev: 0.26.1 hooks: - id: gersemi ================================================ FILE: CHANGELOG.md ================================================ # Changelog All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] ### Added - broadphase: add functional API for collision and distance callbacks ([#724](https://github.com/coal-library/coal/pull/724)) - Added a second set of Python bindings based on nanobind ([#659](https://github.com/coal-library/coal/pull/659)) - ROS: jrl_cmakemodules dependency + kilted CI ([#769](https://github.com/coal-library/coal/pull/769)) - Added `SUFFIX_SO_VERSION` CMake option, default `OFF` ([#770](https://github.com/coal-library/coal/pull/770)) - Add pixi-build support ([#774](https://github.com/coal-library/coal/pull/774)) - contact patches: add helpers to reduce the size of a contact patch by keeping points that minimize the difference between the area of the original patch and the area of the new patch - handle the specific cases of n_desired = 1 or 2 ([805](https://github.com/coal-library/coal/pull/805)) - improve efficiency of ContactPatchSimplifierMaxArea and improve testing of patch simplifiers ([813](https://github.com/coal-library/coal/pull/813)) - timings: + and += operators ([805](https://github.com/coal-library/coal/pull/805)) - Add print methods for collision data (`Contact`, `CollisionRequest`, `CollisionResult`, `DistanceRequest`, `DistanceResult`, `ContactPatch`, `ContactPatchRequest` and `ContactPatchResult`) ([854](https://github.com/coal-library/coal/pull/854)). - One can now do `std::cout << contact` for example. - Added the `PrintableVisitor` in python bindings so that `print(contact)` works in python as well - Added `resolveReferences` method to `Contact` and `DistanceResult` to remap the `o1/o2` pointers (typically after serialization/deserialization) ([855](https://github.com/coal-library/coal/pull/855)). - Added copy constructors to `Contact::Contact(const Contact& other, const CollisionGeometry* new_o1, const CollisionGeometry* new_o2)` and `DistanceResult::DistanceResult(const DistanceResult& other, const CollisionGeometry* new_o1, const CollisionGeometry* new_o2)` to allow copying a `Contact` or `DistanceResult` while remapping the `o1/o2` pointers to new geometries. This is typically useful in the context of deep-copying ([#856](https://github.com/coal-library/coal/pull/820)). - Added the `COAL_EQUAL_OPERATOR_CHECK` macro. This macro can be overridden at compile time, extremely practial for debugging serialization for example. ([#859](https://github.com/coal-library/coal/pull/859)) ### Removed - Remove direct dependency to ([#744](https://github.com/coal-library/coal/pull/744)): - Boost::system - Boost::chrono - Boost::date_time - Boost::thread ### Fixed - Fix doc parsing via doxygen scripts ([#678](https://github.com/coal-library/coal/pull/678) [#699](https://github.com/coal-library/coal/pull/699)) - Correctly calculate AABB for pruned octrees ([#741](https://github.com/coal-library/coal/pull/741)) - Fix contact counting in octree collision detection with ShapeShapeCollide ([#746](https://github.com/coal-library/coal/pull/746)) - Fix sqrDistLowerBound in octree traversal with height field ([#753](https://github.com/coal-library/coal/pull/753)) - Fix contact patch computation by enforcing CCW construction of support sets ([#772](https://github.com/coal-library/coal/pull/772)) - Remove pixi 0.57 warnings ([#774](https://github.com/coal-library/coal/pull/774)) - Fix nanobind bindings' stub file ([#781](https://github.com/coal-library/coal/pull/781#event-20983199417)) - Remove Windows warnings when building benchmakrs ([789](https://github.com/coal-library/coal/pull/789)) - convex: a point can have more than 256 neighbors now. In fact, a point can have `std::numeric_limits::max()` number of neighbors, where IndexType is typically int16 or int32 ([805](https://github.com/coal-library/coal/pull/805)) - Fix octree against octree collision check ([#811](https://github.com/coal-library/coal/pull/811)) - Fix condition for negative bounding volume check ([#816](https://github.com/coal-library/coal/pull/816)) - Add missing calls to computeLocalAABB for internal objects ([#819](https://github.com/coal-library/coal/pull/819)) - Add missing override specifiers ([#820](https://github.com/coal-library/coal/pull/820)) - Fix Python error when accessing `geometry.convex` on BVH geometris ([#833](https://github.com/coal-library/coal/pull/833)) - Fix `Contact`, `CollisionResult` and `DistanceResult` operator== ([#856](https://github.com/coal-library/coal/pull/820)): - If the `o1/o2` pointers are different, we check whether or not the underlying geometries are the same. This is typically important in the context of serialization. - Fix using NaN to initialize collision data (`Contact`, `CollisionResult`, `DistanceResult`). This prevents the absurd `Contact contact; contact == contact; // false` problem. - Fix NaNs coming from GJK/EPA when the algorithms (correctly) early stopped. NaNs indicate failure. In the case that GJK/EPA early stopped but ran fine, we set non-computed data to inf instead of NaN. ### Changed - Float precision ([#665](https://github.com/coal-library/coal/pull/665)) - Rename `CoalScalar` to `Scalar` - Add option to switch between (default) double precision and float precision - Changed all the uses of `double` to `Scalar` in Coal - Fixed all the compilation warnings when compiling the library using float precision - Tracy profiling ([#668](https://github.com/coal-library/coal/pull/668)) - added cmake option `COAL_BUILD_WITH_TRACY` - put tracy scoped zones in broadphase and primitive shapes collision/distance queries - Use double precision for GJK/EPA when coal is compiled in float ([#674](https://github.com/coal-library/coal/pull/674)) - Everything is in float in coal (including the support functions), except the computations inside GJK/EPA - Allows GJK/EPA to avoid limitation of float precision - Renamed PyPI package from coal-library to coal ([#675](https://github.com/coal-library/coal/pull/675)) - Fixed malloc in COAL_ASSERT ([#687](https://github.com/coal-library/coal/pull/687)) - Introducing `Convex16` and `Convex32` to store neighbors and polygons indices as `uint16` or `uint32` ([#682](https://github.com/coal-library/coal/pull/682), [#716](https://github.com/coal-library/coal/pull/716)). - Along with #665, this allows to divide by two the memory footprint of `Convex`. - Fixed a bug in DynamicAABBTree broadphase that missed aabb overlaps when multiple planes/halfspaces are used in a scene. - Python version update ([#774](https://github.com/coal-library/coal/pull/774)): - Project is now tested with Python 3.10 and 3.14 - Python 3.10 is the minimal supported Python version - Nix: switch to flakoboros ([#852](https://github.com/coal-library/coal/pull/852)) ## [3.0.3] - 2026-05-05 - Backport removal of Boost::system ## [3.0.2] - 2025-09-29 ### Added - CMake: add COAL_DISABLE_HPP_FCL_WARNINGS option ([#709](https://github.com/coal-library/coal/pull/709)) - CMake: add support for BUILD_STANDALONE_PYTHON_INTERFACE ([#658](https://github.com/coal-library/coal/pull/658)) - Docker images `ghcr.io/coal-library/coal` ([#737](https://github.com/coal-library/coal/pull/737)) ### Removed - Remove constraints on supported doxygen version to generate the python documentation ([#681](https://github.com/coal-library/coal/pull/681)) - Remove useless COAL_WITH_CXX11_SUPPORT guard ([#688](https://github.com/coal-library/coal/pull/688)) - Remove qhull submodule, as ubuntu 20.04 is EoL ([#704](https://github.com/coal-library/coal/pull/704)) - Removed support for octomap < 1.8 ([#727](https://github.com/coal-library/coal/pull/727)) ### Changed - Formatted all CMake listfiles using gersemi, add gersemi to pre-commit configuration ([#657](https://github.com/coal-library/coal/pull/657/files)) ## [3.0.1] - 2025-02-12 ### Fixed - Remove CMake CMP0167 warnings ([#630](https://github.com/coal-library/coal/pull/630)) - Allow to run test in the build directory on Windows ([#630](https://github.com/coal-library/coal/pull/630)) - Updated nix flake from `hpp-fcl` to `coal` ([#632](https://github.com/coal-library/coal/pull/632) - Fix hpp-fclConfig.cmake on Windows ([#633](https://github.com/coal-library/coal/pull/633)) - Fix install version ([#651](https://github.com/coal-library/coal/pull/651)) ### Added - Add Pixi support ([#629](https://github.com/coal-library/coal/pull/629)) ### Changed - Set NOMINMAX as a public definitions on Windows ([#640](https://github.com/coal-library/coal/pull/640)) ## [3.0.0] - 2024-11-20 ### Added - Renaming the library from `hpp-fcl` to `coal`. Created a `COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL` CMake option for retro compatibility. This allows to still do `find_package(hpp-fcl)` and `#include ` in C++ and it allows to still do `import hppfcl` in python ([#596](https://github.com/coal-library/coal/pull/596)). - Added `Transform3f::Random` and `Transform3f::setRandom` ([#584](https://github.com/coal-library/coal/pull/584)) - New feature: computation of contact surfaces for any pair of primitive shapes (triangle, sphere, ellipsoid, plane, halfspace, cone, capsule, cylinder, convex) ([#574](https://github.com/coal-library/coal/pull/574)). - Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/coal-library/coal/pull/570)) - [#558](https://github.com/coal-library/coal/pull/558): - [internal] Removed dead code in `narrowphase/details.h` ([#558](https://github.com/coal-library/coal/pull/558)) - [internal] Removed specializations of methods of `GJKSolver`. Now the specializations are all handled by `ShapeShapeDistance` in `shape_shape_func.h`. - [new feature] Added support for Swept-Sphere primitives (sphere, box, capsule, cone, ellipsoid, triangle, halfspace, plane, convex mesh). - [API change] Renamed default convergence criterion from `VDB` to `Default` ([#556](https://github.com/coal-library/coal/pull/556)) - Fixed EPA returning nans on cases where it could return an estimate of the normal and penetration depth. ([#556](https://github.com/coal-library/coal/pull/556)) - Fixed too low tolerance in GJK/EPA asserts ([#554](https://github.com/coal-library/coal/pull/554)) - Fixed `normal_and_nearest_points` test (no need to have Eigen 3.4) ([#553](https://github.com/coal-library/coal/pull/553)) - [#549](https://github.com/coal-library/coal/pull/549) - Optimize EPA: ignore useless faces in EPA's polytope; warm-start support computation for `Convex`; fix edge-cases witness points computation. - Add `Serializable` trait to transform, collision data, collision geometries, bounding volumes, bvh models, hfields. Collision problems can now be serialized from C++ and sent to python and vice versa. - CMake: allow use of installed jrl-cmakemodules ([#564](https://github.com/coal-library/coal/pull/564)) - CMake: Add compatibility with jrl-cmakemodules workspace ([#610](https://github.com/coal-library/coal/pull/610)) - Python: add id() support for geometries ([#618](https://github.com/coal-library/coal/pull/618)). ### Fixed - Fix Fix serialization unit test when running without Qhull support ([#611](https://github.com/coal-library/coal/pull/611)) - Compiler warnings ([#601](https://github.com/coal-library/coal/pull/601), [#605](https://github.com/coal-library/coal/pull/605)) - CMake: fix assimp finder - Don't define GCC7 Boost serialization hack when `HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION` is defined ([#530](https://github.com/coal-library/coal/pull/530)) - Default parameters for narrowphase algorithms (GJK and EPA); fixed assertion checks that were sometimes failing in GJK simplex projection and BVH `collide` ([#531](https://github.com/coal-library/coal/pull/531)). - Created a new macro `HPP_FCL_ASSERT` which behaves as an assert by default. When the option `HPP_FCL_TURN_ASSERT_INTO_EXCEPTION` is turned on, it replaces the macro by an exception ([#533](https://github.com/coal-library/coal/pull/533)). Also fixed an EPA assert in `GJKSolver`. - Simplify internals of hpp-fcl ([#535](https://github.com/coal-library/coal/pull/535)): - Computing distance between 2 primitives shapes does not use a traversal node anymore. - Removed successive mallocs in GJK/EPA when using an instance of `GJKSolver` multiple times. - `GJKSolver` now deals with **all** statuses of GJK/EPA. Some of these statuses represent a bad behavior of GJK/EPA and now trigger an assertion in Debug mode. Usefull for debugging these algos. - Logging was added with macros like `HPP_FCL_LOG_(INFO/DEBUG/WARNING/ERROR)`; hpp-fcl can now log usefull info when the preprocessor option `HPP_FCL_ENABLE_LOGGING` is enabled. - Deprecated `enable_distance_lower_bound` in `CollisionRequest`; a lower bound on distance is always computed. - Deprecated `enable_nearest_points` in `DistanceRequest`; they are always computed and are the points of the shapes that achieve a distance of `DistanceResult::min_distance`. - Added `enable_signed_distance` flag in `DistanceRequest` (default `true`). Turn this of for better performance if only the distance when objects are disjoint is needed. - The internal collision and distance functions of hpp-fcl now use `CollisionRequest::enable_contact` and `DistanceRequest::enable_signed_distance` to control whether or not penetration information should be computed. There are many scenarios where we don't need the penetration information and only want to know if objects are colliding and compute their distance only if they are disjoint. These flags allow the user to control the trade-off between performance vs. information of the library. - Fix convergence criterion of EPA; made GJK and EPA convergence criterion absolute + relative to scale to the shapes' dimensions; remove max face/vertices fields from EPA (these can be deduced from the max number of iterations) - Account for lateral borders in Height Fields model. - Fix compilation error on recent APPLE compilers ([#539](https://github.com/coal-library/coal/pull/539)). - Fix printing of deprecated message ([#540](https://github.com/coal-library/coal/pull/540)). - Fix compilation with earlier Eigen version - Fix compilation warning message - Fix issue in Octomap.computeLocalAABB - Fix unsupported function for contact_patch_matrix - Fix Octomap dependency on ROS ## [2.4.5] - 2024-07-28 ### Fixed - Fix Octomap dependency on ROS ## [2.4.4] - 2024-03-06 ## [2.4.3] - 2024-03-06 ### Fixed - updated cmake module to fix documentation generation - test documentation in conda ci ## [2.4.2] - 2024-03-06 ### Fixed - Fix CMAKE_INSTALL_{} path for installation ([#543](https://github.com/coal-library/coal/pull/543)) ## [2.4.1] - 2024-01-23 ### Fixed - CachedMeshLoader checks file last modification time. - Fix call to clear methods for {Collision,Distance}Data inside init function ([#509](https://github.com/coal-library/coal/pull/509)) - CMake: fix submodule use in bindings in ([#512](https://github.com/coal-library/coal/pull/512)) - Fix bug in DynamicAABBTreeCollisionManager (see [#514](https://github.com/coal-library/coal/issues/514)) in ([#515](https://github.com/coal-library/coal/pull/515)) ### Added - In struct Contact - Documentation of the members, - initialization of normal, closest points and contact point in constructors - method getDistanceToCollision - New variant of GJK (PolyakAcceleration). - Specialization of distance computation between - Sphere and Capsule, - Ellipsoid and Halfspace, - Ellipsoid and Plane. - Collision computation between Octree and HeightField. ### Changed - Matrixx3f and Matrixx3i become row major. - Use shared pointers to vectors instead of arrays for vertices and triangles in class BVHModelBase. ### Removed - members related epa in class QueryRequest ## [2.4.0] - 2023-11-27 ### Added - Add method to `CollisionObject` to get `CollisionGeometry` raw pointer ### Fixed - Fix RPATH computation on OSX - Fix Python stubs generation on Windows ## [2.3.7] - 2023-11-15 ### What's Changed - Add Python 3.12 support by [@jorisv](https://github.com/jorisv) ([#471](https://github.com/coal-library/coal/pull/471)) - Enable ruff linting by [@nim65s](https://github.com/nim65s) ([#464](https://github.com/coal-library/coal/pull/464)) ## [2.3.6] - 2023-09-30 ### What's Changed - Update ROS_DISTRO by [@jcarpent](https://github.com/jcarpent) ([#442](https://github.com/coal-library/coal/pull/442)) - Add citations by [@jcarpent](https://github.com/jcarpent) ([#449](https://github.com/coal-library/coal/pull/449)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#444](https://github.com/coal-library/coal/pull/444)) - [WIP] Debug by [@jcarpent](https://github.com/jcarpent) ([#455](https://github.com/coal-library/coal/pull/455)) - CMake: require >= 3.10 by [@nim65s](https://github.com/nim65s) ([#453](https://github.com/coal-library/coal/pull/453)) - core: fix SaPCollisionManager::empty() by [@rujialiu](https://github.com/rujialiu) ([#454](https://github.com/coal-library/coal/pull/454)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#452](https://github.com/coal-library/coal/pull/452)) ### New Contributors - [@rujialiu](https://github.com/rujialiu) made their first contribution ([#454](https://github.com/coal-library/coal/pull/454)) ## [2.3.5] - 2023-07-11 ### What's Changed - Fix compilation warning by [@jcarpent](https://github.com/jcarpent) ([#434](https://github.com/coal-library/coal/pull/434)) - Fix parsing of doxygen doc by [@jcarpent](https://github.com/jcarpent) ([#439](https://github.com/coal-library/coal/pull/439)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#438](https://github.com/coal-library/coal/pull/438)) ## [2.3.4] - 2023-06-01 ### What's Changed - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#414](https://github.com/coal-library/coal/pull/414)) - Fix conversion warning by [@wxmerkt](https://github.com/wxmerkt) ([#417](https://github.com/coal-library/coal/pull/417)) - Add missing boost include by [@nim65s](https://github.com/nim65s) ([#418](https://github.com/coal-library/coal/pull/418)) - ci: update macos-linux-pip by [@nim65s](https://github.com/nim65s) ([#419](https://github.com/coal-library/coal/pull/419)) - Modernize Cmake use by [@nim65s](https://github.com/nim65s) ([#420](https://github.com/coal-library/coal/pull/420)) - tests: use boost::filesystem by [@nim65s](https://github.com/nim65s) ([#424](https://github.com/coal-library/coal/pull/424)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#425](https://github.com/coal-library/coal/pull/425)) - Update minimal Python version by [@jcarpent](https://github.com/jcarpent) ([#427](https://github.com/coal-library/coal/pull/427)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#430](https://github.com/coal-library/coal/pull/430)) - Sync submodule CMake by [@jcarpent](https://github.com/jcarpent) ([#431](https://github.com/coal-library/coal/pull/431)) ## [2.3.3] - 2023-05-09 ### What's Changed - update default C++ to 14 by [@nim65s](https://github.com/nim65s) ([#410](https://github.com/coal-library/coal/pull/410)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#413](https://github.com/coal-library/coal/pull/413)) ## [2.3.2] - 2023-04-27 ### What's Changed - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#391](https://github.com/coal-library/coal/pull/391)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#393](https://github.com/coal-library/coal/pull/393)) - Topic/rpath by [@nim65s](https://github.com/nim65s) ([#394](https://github.com/coal-library/coal/pull/394)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#396](https://github.com/coal-library/coal/pull/396)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#399](https://github.com/coal-library/coal/pull/399)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#402](https://github.com/coal-library/coal/pull/402)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#406](https://github.com/coal-library/coal/pull/406)) ## [2.3.1] - 2023-03-25 ### What's Changed - Remove useless call to /proc/cpuinfo by [@jcarpent](https://github.com/jcarpent) ([#385](https://github.com/coal-library/coal/pull/385)) - Add pip CI by [@nim65s](https://github.com/nim65s) ([#386](https://github.com/coal-library/coal/pull/386)) - [GJKSolver] Fix missing switch case in result status of GJK by [@lmontaut](https://github.com/lmontaut) ([#387](https://github.com/coal-library/coal/pull/387)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#388](https://github.com/coal-library/coal/pull/388)) ## [2.3.0] - 2023-03-17 ### What's Changed - [CI] Remove EOL Galactic by [@wxmerkt](https://github.com/wxmerkt) ([#366](https://github.com/coal-library/coal/pull/366)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#367](https://github.com/coal-library/coal/pull/367)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#368](https://github.com/coal-library/coal/pull/368)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#369](https://github.com/coal-library/coal/pull/369)) - Adding EarlyStopped flag in GJK by [@lmontaut](https://github.com/lmontaut) ([#371](https://github.com/coal-library/coal/pull/371)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#373](https://github.com/coal-library/coal/pull/373)) - Update CI by [@jcarpent](https://github.com/jcarpent) ([#374](https://github.com/coal-library/coal/pull/374)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#375](https://github.com/coal-library/coal/pull/375)) - Skip test if BUILD_TESTING is OFF by [@jcarpent](https://github.com/jcarpent) ([#378](https://github.com/coal-library/coal/pull/378)) ## [2.2.0] - 2022-12-12 ### What's Changed - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#358](https://github.com/coal-library/coal/pull/358)) - Extract checks if AABB overlap by [@jmirabel](https://github.com/jmirabel) ([#360](https://github.com/coal-library/coal/pull/360)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#361](https://github.com/coal-library/coal/pull/361)) - Sync submodule CMake by [@jcarpent](https://github.com/jcarpent) ([#362](https://github.com/coal-library/coal/pull/362)) - Add support of Pickling by [@jcarpent](https://github.com/jcarpent) ([#363](https://github.com/coal-library/coal/pull/363)) ## [2.1.4] - 2022-10-24 ### What's Changed - Sync submodule CMake by [@jcarpent](https://github.com/jcarpent) ([#352](https://github.com/coal-library/coal/pull/352)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#353](https://github.com/coal-library/coal/pull/353)) ## [2.1.3] - 2022-09-13 ### What's Changed - Minor boost cleanup by [@pantor](https://github.com/pantor) ([#331](https://github.com/coal-library/coal/pull/331)) - [CI] Activate ROS2 configurations by [@wxmerkt](https://github.com/wxmerkt) ([#332](https://github.com/coal-library/coal/pull/332)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#337](https://github.com/coal-library/coal/pull/337)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#341](https://github.com/coal-library/coal/pull/341)) - Fix shapeIntersect when for EPA FallBack by [@jcarpent](https://github.com/jcarpent) ([#342](https://github.com/coal-library/coal/pull/342)) - Fix findAssimp on Windows by [@jcarpent](https://github.com/jcarpent) ([#345](https://github.com/coal-library/coal/pull/345)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#347](https://github.com/coal-library/coal/pull/347)) ### New Contributors - [@pantor](https://github.com/pantor) made their first contribution ([#331](https://github.com/coal-library/coal/pull/331)) ## [2.1.2] - 2022-08-01 ### What's Changed - core: add EPA::FallBack condition to shapeDistance computation by [@lmontaut](https://github.com/lmontaut) ([#325](https://github.com/coal-library/coal/pull/325)) - CMake: update to eigenpy 2.7.10 by [@nim65s](https://github.com/nim65s) ([#327](https://github.com/coal-library/coal/pull/327)) ## [2.1.1] - 2022-07-25 ### What's Changed - cmake: relocatable package for recent CMake versions by [@nim65s](https://github.com/nim65s) ([#319](https://github.com/coal-library/coal/pull/319)) - ROS2/Colcon integration by [@wxmerkt](https://github.com/wxmerkt) ([#321](https://github.com/coal-library/coal/pull/321)) ## [2.1.0] - 2022-07-13 ### What's Changed - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#288](https://github.com/coal-library/coal/pull/288)) - Add enum helpers by [@jcarpent](https://github.com/jcarpent) ([#290](https://github.com/coal-library/coal/pull/290)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#294](https://github.com/coal-library/coal/pull/294)) - Ellipsoids in collision & distance matrices by [@lmontaut](https://github.com/lmontaut) ([#295](https://github.com/coal-library/coal/pull/295)) - doc: simplex projection in GJK class. by [@lmontaut](https://github.com/lmontaut) ([#296](https://github.com/coal-library/coal/pull/296)) - Feature: Nesterov acceleration for GJK by [@lmontaut](https://github.com/lmontaut) ([#289](https://github.com/coal-library/coal/pull/289)) - Add more testing to broadphase by [@jcarpent](https://github.com/jcarpent) ([#298](https://github.com/coal-library/coal/pull/298)) - Feature: adding convergence criterions for GJK algorithm by [@lmontaut](https://github.com/lmontaut) ([#299](https://github.com/coal-library/coal/pull/299)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#300](https://github.com/coal-library/coal/pull/300)) - Reorder triangles when computing convex hulls by [@lmontaut](https://github.com/lmontaut) ([#301](https://github.com/coal-library/coal/pull/301)) - Exposing gjk utils by [@lmontaut](https://github.com/lmontaut) ([#302](https://github.com/coal-library/coal/pull/302)) - Fix assert precision in GJK by [@jcarpent](https://github.com/jcarpent) ([#304](https://github.com/coal-library/coal/pull/304)) - Simplify GJKSolver settings by [@jcarpent](https://github.com/jcarpent) ([#305](https://github.com/coal-library/coal/pull/305)) - Add CollisionResult::nearest_points by [@jcarpent](https://github.com/jcarpent) ([#303](https://github.com/coal-library/coal/pull/303)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#306](https://github.com/coal-library/coal/pull/306)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#309](https://github.com/coal-library/coal/pull/309)) - Fix minimal value for GJK::distance_upper_bound by [@jcarpent](https://github.com/jcarpent) ([#310](https://github.com/coal-library/coal/pull/310)) - Fix incoherent overlap by [@jcarpent](https://github.com/jcarpent) ([#311](https://github.com/coal-library/coal/pull/311)) - Expose shared_ptr by [@Jiayuan-Gu](https://github.com/Jiayuan-Gu) ([#314](https://github.com/coal-library/coal/pull/314)) - test/gjk_convergence_criterion: Add check on GJK::Status by [@wxmerkt](https://github.com/wxmerkt) ([#315](https://github.com/coal-library/coal/pull/315)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#316](https://github.com/coal-library/coal/pull/316)) - Handle negative security margin by [@jcarpent](https://github.com/jcarpent) ([#312](https://github.com/coal-library/coal/pull/312)) ### New Contributors - [@Jiayuan-Gu](https://github.com/Jiayuan-Gu) made their first contribution ([#314](https://github.com/coal-library/coal/pull/314)) ## [2.0.1] - 2022-04-15 This PR mainly fixes packaging issues and removes compilation warnings. ### What's Changed - Zero warnings by [@wxmerkt](https://github.com/wxmerkt) ([#282](https://github.com/coal-library/coal/pull/282)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#283](https://github.com/coal-library/coal/pull/283)) - [pre-commit.ci] pre-commit autoupdate by [@pre-commit-ci](https://github.com/pre-commit-ci) ([#284](https://github.com/coal-library/coal/pull/284)) - Activate python3-pylatexenc dependency by [@wxmerkt](https://github.com/wxmerkt) ([#286](https://github.com/coal-library/coal/pull/286)) - Comment pylatexenc again since it's not available on the buildfarm by [@wxmerkt](https://github.com/wxmerkt) ([#287](https://github.com/coal-library/coal/pull/287)) ### New Contributors - [@pre-commit-ci](https://github.com/pre-commit-ci) made their first contribution ([#284](https://github.com/coal-library/coal/pull/284)) ## [2.0.0] - 2022-04-06 This new release reintroduces the full support of Broad phase within hpp-fcl while also enforcing C++11 as minimal standard. ### What's Changed - Add Ellipsoid by [@jcarpent](https://github.com/jcarpent) ([#259](https://github.com/coal-library/coal/pull/259)) - Removing comment about inflation. by [@lmontaut](https://github.com/lmontaut) ([#261](https://github.com/coal-library/coal/pull/261)) - Reintroduce broadphase by [@jcarpent](https://github.com/jcarpent) ([#260](https://github.com/coal-library/coal/pull/260)) - Simplify CollisionObject by removing cgeom_const by [@jcarpent](https://github.com/jcarpent) ([#263](https://github.com/coal-library/coal/pull/263)) - Address some warnings by [@wxmerkt](https://github.com/wxmerkt) ([#262](https://github.com/coal-library/coal/pull/262)) - Fix missing copy of aabb_local in CollisionGeometry by [@jcarpent](https://github.com/jcarpent) ([#264](https://github.com/coal-library/coal/pull/264)) - use std::shared_ptr, fix #218 by [@nim65s](https://github.com/nim65s) ([#266](https://github.com/coal-library/coal/pull/266)) - Fix broadphase warnings for clang (some conversion remain for g++) by [@wxmerkt](https://github.com/wxmerkt) ([#268](https://github.com/coal-library/coal/pull/268)) - [ComputeCollision] Return no collision if security_margin is set to -inf by [@florent-lamiraux](https://github.com/florent-lamiraux) ([#271](https://github.com/coal-library/coal/pull/271)) - tests: remove link to boost unit test framework by [@nim65s](https://github.com/nim65s) ([#270](https://github.com/coal-library/coal/pull/270)) - Fix computation of aabb_center by [@jcarpent](https://github.com/jcarpent) ([#273](https://github.com/coal-library/coal/pull/273)) - Add operator== and operator!= to CollisionGeometry by [@jcarpent](https://github.com/jcarpent) ([#274](https://github.com/coal-library/coal/pull/274)) - Merge pull request #276 from humanoid-path-planner/patch-release-1.8.1 by [@jcarpent](https://github.com/jcarpent) ([#277](https://github.com/coal-library/coal/pull/277)) - Fix some missing features in base classes by [@jcarpent](https://github.com/jcarpent) ([#275](https://github.com/coal-library/coal/pull/275)) - Add operator{==,!=} to CollisionObject by [@jcarpent](https://github.com/jcarpent) ([#278](https://github.com/coal-library/coal/pull/278)) - Configure and apply pre-commit by [@jcarpent](https://github.com/jcarpent) ([#280](https://github.com/coal-library/coal/pull/280)) - Fix DistanceCallBackBaseWrapper by [@jcarpent](https://github.com/jcarpent) ([#281](https://github.com/coal-library/coal/pull/281)) ### New Contributors - [@lmontaut](https://github.com/lmontaut) made their first contribution ([#261](https://github.com/coal-library/coal/pull/261)) ## [1.8.1] - 2022-03-20 ### What's Changed - Preparing for ROS1 and ROS2 release by [@wxmerkt](https://github.com/wxmerkt) ([#255](https://github.com/coal-library/coal/pull/255)) - Patch release 1.8.1 by [@wxmerkt](https://github.com/wxmerkt) ([#276](https://github.com/coal-library/coal/pull/276)) ## [1.8.0] - 2022-02-08 ### What's Changed - [CMake] Qhull is a private dependency by [@nim65s](https://github.com/nim65s) ([#247](https://github.com/coal-library/coal/pull/247)) - Remove useless warnings by [@jcarpent](https://github.com/jcarpent) ([#248](https://github.com/coal-library/coal/pull/248)) - fix submodule url by [@nim65s](https://github.com/nim65s) ([#246](https://github.com/coal-library/coal/pull/246)) - Remove warnings and add missing noalias by [@jcarpent](https://github.com/jcarpent) ([#249](https://github.com/coal-library/coal/pull/249)) - Function makeOctree returns a shared pointer by [@florent-lamiraux](https://github.com/florent-lamiraux) ([#254](https://github.com/coal-library/coal/pull/254)) - Add support of HeightField by [@jcarpent](https://github.com/jcarpent) ([#251](https://github.com/coal-library/coal/pull/251)) - [OcTree] Add method to save octree in obj file. by [@florent-lamiraux](https://github.com/florent-lamiraux) ([#256](https://github.com/coal-library/coal/pull/256)) - Fix C++98 compatibility by [@jcarpent](https://github.com/jcarpent) ([#258](https://github.com/coal-library/coal/pull/258)) ## [1.7.8] - 2021-10-30 ### What's Changed - Fix conversion by [@jcarpent](https://github.com/jcarpent) ([#242](https://github.com/coal-library/coal/pull/242)) - Fix exposition of vertices by [@jcarpent](https://github.com/jcarpent) ([#243](https://github.com/coal-library/coal/pull/243)) - Enhance Convex exposition by [@jcarpent](https://github.com/jcarpent) ([#244](https://github.com/coal-library/coal/pull/244)) - Sync submodule cmake by [@jcarpent](https://github.com/jcarpent) ([#245](https://github.com/coal-library/coal/pull/245)) ## [1.7.7] - 2021-09-13 This new release fixes several bugs within the framework. ## [1.7.6] - 2021-09-08 This new release improves the packaging of the project and integrates the Stub generation of Python bindings. ## [1.7.5] - 2021-07-30 This new release provides extended API exposition in Python, removes some code related to CDD while also trying to rely on the QHULL version present on the system. ## [1.7.4] - 2021-06-11 This release fixes several bugs: - correct update of the distance lower bound - fix memory footprint computation while also removing the support of Travis CI. ## [1.7.3] - 2021-05-26 This new release provides: - fixes of LINE and POINTS when loading meshes with assimp - removing of various warnings - computation of memory footprint for geometries ## [1.7.2] - 2021-04-19 This new release improves the loading of meshes using Assimp by automatically removing degenerated LINES and POINTS. ## [1.7.1] - 2021-04-02 This new release reduces the impact of timers on the computations. This should be used with care and can be enabled by setting the correct flag to true in the QueryRequest. ## [1.7.0] - 2021-03-31 This new release provides: - extended support for serialization - timing of the collision/distance computations - helpers to build octree - various bug fixes and interface improvements ## [1.6.0] - 2020-10-06 This new release provides: - functors for evaluating Collision and Distances (faster call) - extended support of v142 compiler - support of collision check between HalfSpace and Convex shapes - improvement of GJK solver - fixes on Python bindings ## [1.5.4] - 2020-09-22 In this new release, the support of collision checking between Convex objects and HalfSpace have been enhanced and some minor fixes have been provided. ## [1.5.3] - 2020-08-31 This new release provides better CMake packaging and improved GJK algorithms. ## [1.5.2] - 2020-08-15 This release improves the packaging of the project and provides fixes for the GJK solver. ## [1.5.1] - 2020-08-06 This new release fixes packaging issues with precedent release 1.5.0. It also provides additional fixes in main collision/distance algorithms. ## [1.4.6] - 2020-06-10 This new release enhances the packaging of the project and allows the compilation of FCL on Windows systems. ## [1.4.5] - 2020-06-03 Changes in v1.4.5: - Fix Python 3 doc generation - Fix packaging of the project - Compilation on Windows. - [CMake] Install missing header. - Add collide and distance prototype that update the GJK guess. - Add support function cached guess in queries and merge query attribute. - Add function to generate the convex hull. - Add hint to the support function + Fix usage of GJK guess. - [Python] Add constructor for class Convex. - [Python] Bind functions to create BVHModel. ## [1.4.4] - 2020-04-29 Changes in 1.4.4: - add MeshLoader::loadOctree - fix generation of XML documentation - fix generation of Doxygen documentation ## [1.4.3] - 2020-04-08 This new release fixes some packagings issues for OS X systems. ## [1.4.2] - 2020-04-04 Changes in v1.4.2: - don't require linking to eigenpy in .pc file. ## [1.4.1] - 2020-04-03 Changes in v1.4.1: - Bug fix + prepare optimization of collision using GJK / EPA - Add missing constructor for Transform3f ## [1.4.0] - 2020-04-03 Changes since v1.3.0: - Improve code efficiency + use shared memory between Numpy and Eigen - [Python] Doc and minor update + [C++] bugfix - [Python] Fix bindings of CollisionResult. - FIX: throw when no contact is available - Minor fix and computational improvments - [GJK/EPA] Fix bugs + Treat sphere as point and capsule as line segment. - Fix boxSphereDistance - Provide documentation for the Python bindings. - Generate Python documentation from doxygen documentation. - Fix issue when Python_EXECUTABLE is not defined - update CMake packaging ## [1.3.0] - 2020-01-28 This new release comes with: - the removing of the GJK solver - the Python bindings build by default - an improved documentation - additional Python bindings ## [1.2.2] - 2019-12-17 This new Release improves the Python bindings and fixes an important bug when checking the collision between two Capsules. Thanks to [@rstrudel](https://github.com/rstrudel) for this fix. ## [1.2.1] - 2019-12-09 This new release improves both the packaging of the project, which seems to be totally compatible with the new CMake linkage style. In addition, the bindings are now fully compatible with Pinocchio. ## [1.2.0] - 2019-11-22 Changes since v1.1.3: - Add python bindings - Update CMake - Add version support - New folder Internal for internal header - Travis: update CI & change policy to only perform build in DEBUG mode on Bionic - assimp: fix issue with recent version of assimp - [bindings] [CMakeLists] Use .so for Mac and .pyd for Windows, fix #86 - Organize documentation - [CMake] fix octomap detection - [Minor] update CMake module + fix visibility of some methods. - Enable Convex / Convex queries + Add Python bindings. - Fix unit-tests and compilation - [GJK] Fix GJK::encloseOrigin (fixes unit-tests) - Improve GJK implementation + OBB overlap test + bug fixes - Clean include BV/BVH/math/mesh_loader ## [1.1.3] - 2019-08-07 This new release enhances the compatibility of hpp-fcl with C++14 and more. This feature is requested for integration in Anaconda. ## [1.1.2] - 2019-08-05 This new release provides a fix in the parallelization of the computations and improves the packaging of the whole project. ## [1.0.2] - 2019-04-24 Changes since v1.0.1: - obb: fix compatibility with Eigen 3.0.5 - [CI] octomap for osx ## [1.0.1] - 2019-02-20 - Fix CI on OSX - Declare CachedMeshLoader::Key::operator< - minor details ## [0.7.0] - 2019-01-31 This release is mainly here to allow the packaging of HPP-RBPRM. Another release will follow with more news. ## [0.6.0] - 2018-10-22 - Fix bug when OCTOMAP is not found - move buildTrianglePlane and clipTriangle method from private to public - Fix bug with "\" symbols - [CMake] Add flags related to Octomap in pkg-config file and remove FCL_HAVE_EIGEN ## [0.5.1] - 2017-10-02 Now Eigen is at the heart of linear algebra computations. ## [0.5] - 2017-03-17 First release [Unreleased]: https://github.com/coal-library/coal/compare/v3.0.3...HEAD [3.0.3]: https://github.com/coal-library/coal/compare/v3.0.2...v3.0.3 [3.0.2]: https://github.com/coal-library/coal/compare/v3.0.1...v3.0.2 [3.0.1]: https://github.com/coal-library/coal/compare/v3.0.0...v3.0.1 [3.0.0]: https://github.com/coal-library/coal/compare/v2.4.5...v3.0.0 [2.4.5]: https://github.com/coal-library/coal/compare/v2.4.4...v2.4.5 [2.4.4]: https://github.com/coal-library/coal/compare/v2.4.3...v2.4.4 [2.4.3]: https://github.com/coal-library/coal/compare/v2.4.2...v2.4.3 [2.4.2]: https://github.com/coal-library/coal/compare/v2.4.1...v2.4.2 [2.4.1]: https://github.com/coal-library/coal/compare/v2.4.0...v2.4.1 [2.4.0]: https://github.com/coal-library/coal/compare/v2.3.7...v2.4.0 [2.3.7]: https://github.com/coal-library/coal/compare/2.3.6...v2.3.7 [2.3.6]: https://github.com/coal-library/coal/compare/v2.3.5...v2.3.6 [2.3.5]: https://github.com/coal-library/coal/compare/v2.3.4...v2.3.5 [2.3.4]: https://github.com/coal-library/coal/compare/v2.3.3...v2.3.4 [2.3.3]: https://github.com/coal-library/coal/compare/v2.3.2...v2.3.3 [2.3.2]: https://github.com/coal-library/coal/compare/v2.3.1...v2.3.2 [2.3.1]: https://github.com/coal-library/coal/compare/v2.3.0...v2.3.1 [2.3.0]: https://github.com/coal-library/coal/compare/v2.2.0...v2.3.0 [2.2.0]: https://github.com/coal-library/coal/compare/v2.1.4...v2.2.0 [2.1.4]: https://github.com/coal-library/coal/compare/v2.1.3...v2.1.4 [2.1.3]: https://github.com/coal-library/coal/compare/v2.1.2...v2.1.3 [2.1.2]: https://github.com/coal-library/coal/compare/v2.1.1...v2.1.2 [2.1.1]: https://github.com/coal-library/coal/compare/v2.1.0...v2.1.1 [2.1.0]: https://github.com/coal-library/coal/compare/v2.0.1...v2.1.0 [2.0.1]: https://github.com/coal-library/coal/compare/v2.0.0...v2.0.1 [2.0.0]: https://github.com/coal-library/coal/compare/v1.8.1...v2.0.0 [1.8.1]: https://github.com/coal-library/coal/compare/v1.8.0...v1.8.1 [1.8.0]: https://github.com/coal-library/coal/compare/v1.7.8...v1.8.0 [1.7.8]: https://github.com/coal-library/coal/compare/v1.7.7...v1.7.8 [1.7.7]: https://github.com/coal-library/coal/compare/v1.7.6...v1.7.7 [1.7.6]: https://github.com/coal-library/coal/compare/v1.7.5...v1.7.6 [1.7.5]: https://github.com/coal-library/coal/compare/v1.7.4...v1.7.5 [1.7.4]: https://github.com/coal-library/coal/compare/v1.7.3...v1.7.4 [1.7.3]: https://github.com/coal-library/coal/compare/v1.7.2...v1.7.3 [1.7.2]: https://github.com/coal-library/coal/compare/v1.7.1...v1.7.2 [1.7.1]: https://github.com/coal-library/coal/compare/v1.7.0...v1.7.1 [1.7.0]: https://github.com/coal-library/coal/compare/v1.6.0...v1.7.0 [1.6.0]: https://github.com/coal-library/coal/compare/v1.5.4...v1.6.0 [1.5.4]: https://github.com/coal-library/coal/compare/v1.5.3...v1.5.4 [1.5.3]: https://github.com/coal-library/coal/compare/v1.5.2...v1.5.3 [1.5.2]: https://github.com/coal-library/coal/compare/v1.5.1...v1.5.2 [1.5.1]: https://github.com/coal-library/coal/compare/v1.4.6...v1.5.1 [1.4.6]: https://github.com/coal-library/coal/compare/v1.4.5...v1.4.6 [1.4.5]: https://github.com/coal-library/coal/compare/v1.4.4...v1.4.5 [1.4.4]: https://github.com/coal-library/coal/compare/v1.4.3...v1.4.4 [1.4.3]: https://github.com/coal-library/coal/compare/v1.4.2...v1.4.3 [1.4.2]: https://github.com/coal-library/coal/compare/v1.4.1...v1.4.2 [1.4.1]: https://github.com/coal-library/coal/compare/v1.4.0...v1.4.1 [1.4.0]: https://github.com/coal-library/coal/compare/v1.3.0...v1.4.0 [1.3.0]: https://github.com/coal-library/coal/compare/v1.2.2...v1.3.0 [1.2.2]: https://github.com/coal-library/coal/compare/v1.2.1...v1.2.2 [1.2.1]: https://github.com/coal-library/coal/compare/v1.2.0...v1.2.1 [1.2.0]: https://github.com/coal-library/coal/compare/v1.1.3...v1.2.0 [1.1.3]: https://github.com/coal-library/coal/compare/v1.1.2...v1.1.3 [1.1.2]: https://github.com/coal-library/coal/compare/v1.0.2...v1.1.2 [1.0.2]: https://github.com/coal-library/coal/compare/v1.0.1...v1.0.2 [1.0.1]: https://github.com/coal-library/coal/compare/v0.7.0...v1.0.1 [0.7.0]: https://github.com/coal-library/coal/compare/v0.6.0...v0.7.0 [0.6.0]: https://github.com/coal-library/coal/compare/v0.5.1...v0.6.0 [0.5.1]: https://github.com/coal-library/coal/compare/v0.5...v0.5.1 [0.5]: https://github.com/coal-library/coal/releases/tag/v0.5 ================================================ FILE: CITATION.bib ================================================ @misc{coalweb, author = {Jia Pan and Sachin Chitta and Dinesh Manocha and Florent Lamiraux and Joseph Mirabel and Justin Carpentier and Louis Montaut and others}, title = {Coal: an extension of the Flexible Collision Library}, howpublished = {https://github.com/coal-library/coal}, year = {2015--2024} } ================================================ FILE: CITATION.cff ================================================ cff-version: 1.2.0 message: "Thanks for using Coal. Please use the following metadata to cite us in your documents." authors: - family-names: Pan given-names: Jia - family-names: Chitta given-names: Sachin - family-names: Manocha given-names: Dinesh - family-names: Mirabel given-names: Joseph - family-names: Carpentier given-names: Justin orcid: "https://orcid.org/0000-0001-6585-2894" - family-names: Montaut given-names: Louis title: "Coal - An extension of the Flexible Collision Library" abstract: "An extension of the Flexible Collision Library" version: 3.0.3 date-released: "2026-05-05" license: BSD-2-Clause url: "https://github.com/coal-library/coal" ================================================ FILE: CMakeLists.txt ================================================ # # Software License Agreement (BSD License) # # Copyright (c) 2014-2025 CNRS-LAAS, INRIA Author: Florent Lamiraux, Joseph # Mirabel All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright notice, this # list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # * Neither the name of CNRS-LAAS. nor the names of its contributors may be used # to endorse or promote products derived from this software without specific # prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. cmake_minimum_required(VERSION 3.22) set(CXX_DISABLE_WERROR TRUE) set(PROJECT_NAME coal) set(PROJECT_ORG "coal-library") set( PROJECT_DESCRIPTION "Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library" ) set(PROJECT_URL "https://github.com/coal-library/coal") if(NOT BUILD_STANDALONE_PYTHON_INTERFACE) set(PROJECT_USE_CMAKE_EXPORT TRUE) endif() set(PROJECT_COMPATIBILITY_VERSION AnyNewerVersion) # To enable jrl-cmakemodules compatibility with workspace we must define the two # following lines set(PROJECT_AUTO_RUN_FINALIZE FALSE) set(PROJECT_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}) set(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE) set(DOXYGEN_USE_TEMPLATE_CSS TRUE) # ---------------------------------------------------- # --- OPTIONS --------------------------------------- # Need to be set before including base.cmake # ---------------------------------------------------- option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) option(SUFFIX_SO_VERSION "Suffix library name with its version" OFF) option( COAL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical Coal asserts to exception." FALSE ) option( COAL_ENABLE_LOGGING "Activate logging for warnings or error messages. Turned on by default in Debug." FALSE ) option( COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL "Make Coal retro-compatible with HPP-FCL." FALSE ) option( COAL_USE_FLOAT_PRECISION "Use float precision (32-bit) instead of the default double precision (64-bit)" FALSE ) option( COAL_BUILD_WITH_TRACY "Build with tracy profiler for performance analysis" FALSE ) # Check if the submodule cmake have been initialized set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake") message(STATUS "JRL cmakemodules found in 'cmake/' git submodule") else() find_package(jrl-cmakemodules QUIET CONFIG) if(jrl-cmakemodules_FOUND) get_property( JRL_CMAKE_MODULES TARGET jrl-cmakemodules::jrl-cmakemodules PROPERTY INTERFACE_INCLUDE_DIRECTORIES ) message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}") else() message(STATUS "JRL cmakemodules not found. Let's fetch it.") include(FetchContent) FetchContent_Declare( "jrl-cmakemodules" GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git" ) FetchContent_MakeAvailable("jrl-cmakemodules") FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES) endif() endif() # Use BoostConfig module distributed by boost library instead of using FindBoost # module distributed by CMake. There is one unresolved issue with FindBoost and # clang-cl so we deactivate it in this case. if(NOT WIN32 OR NOT ${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") if(POLICY CMP0167) cmake_policy(SET CMP0167 NEW) # Set a default value to this policy to avoid issue with find_dependency # macro redefinition with different policy in some modules. set(CMAKE_POLICY_DEFAULT_CMP0167 NEW) endif() endif() include("${JRL_CMAKE_MODULES}/base.cmake") COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX) project(${PROJECT_NAME} ${PROJECT_ARGS}) include("${JRL_CMAKE_MODULES}/boost.cmake") include("${JRL_CMAKE_MODULES}/python.cmake") include("${JRL_CMAKE_MODULES}/apple.cmake") include("${JRL_CMAKE_MODULES}/ide.cmake") include("${JRL_CMAKE_MODULES}/tracy.cmake") include(CMakeDependentOption) set( CMAKE_MODULE_PATH ${JRL_CMAKE_MODULES}/find-external/assimp/ ${CMAKE_MODULE_PATH} ) function(set_standard_output_directory target) set_target_properties( ${target} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib ) endfunction() SET_DEFAULT_CMAKE_BUILD_TYPE("RelWithDebInfo") # If needed, fix CMake policy for APPLE systems APPLY_DEFAULT_APPLE_CONFIGURATION() cmake_dependent_option( COAL_PYTHON_NANOBIND "Build the nanobind-based bindings." OFF BUILD_PYTHON_INTERFACE OFF ) # Stubs only optional for legacy Boost.Python bindings if(NOT COAL_PYTHON_NANOBIND) cmake_dependent_option( GENERATE_PYTHON_STUBS "Generate the Python stubs for the Python bindings (only for legacy Boost.Python bindings)" OFF BUILD_PYTHON_INTERFACE OFF ) endif() ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.0") # -- tracy profiling (optional) if(COAL_BUILD_WITH_TRACY) # assume it is installed somewhere ADD_PROJECT_DEPENDENCY(Tracy REQUIRED) set_target_properties( Tracy::TracyClient PROPERTIES POSITION_INDEPENDENT_CODE True ) endif(COAL_BUILD_WITH_TRACY) # Required dependencies SET_BOOST_DEFAULT_OPTIONS() EXPORT_BOOST_DEFAULT_OPTIONS() ADD_PROJECT_DEPENDENCY(Boost REQUIRED serialization filesystem) if(COAL_ENABLE_LOGGING) ADD_PROJECT_DEPENDENCY(Boost REQUIRED log) endif() if(Boost_VERSION_STRING VERSION_LESS 1.81) # Default C++ version should be C++11 CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE) else() # Boost.Math will be C++14 starting in July 2023 (Boost 1.82 release) CHECK_MINIMAL_CXX_STANDARD(14 ENFORCE) endif() # Optional dependencies ADD_PROJECT_DEPENDENCY(octomap 1.8.0 PKG_CONFIG_REQUIRES "octomap >= 1.8") if(octomap_FOUND) set(COAL_HAS_OCTOMAP TRUE) string(REPLACE "." ";" VERSION_LIST ${octomap_VERSION}) list(GET VERSION_LIST 0 OCTOMAP_MAJOR_VERSION) list(GET VERSION_LIST 1 OCTOMAP_MINOR_VERSION) list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION) message(STATUS "COAL uses Octomap") else() set(COAL_HAS_OCTOMAP FALSE) message(STATUS "COAL does not use Octomap") endif() option(COAL_HAS_QHULL "use qhull library to compute convex hulls." FALSE) if(COAL_HAS_QHULL) find_package(Qhull REQUIRED COMPONENTS qhull_r qhullcpp) endif() find_package(assimp REQUIRED) set( ${PROJECT_NAME}_HEADERS include/coal/alloca.h include/coal/collision_data.h include/coal/BV/kIOS.h include/coal/BV/BV.h include/coal/BV/RSS.h include/coal/BV/OBBRSS.h include/coal/BV/BV_node.h include/coal/BV/AABB.h include/coal/BV/OBB.h include/coal/BV/kDOP.h include/coal/broadphase/broadphase.h include/coal/broadphase/broadphase_SSaP.h include/coal/broadphase/broadphase_SaP.h include/coal/broadphase/broadphase_bruteforce.h include/coal/broadphase/broadphase_collision_manager.h include/coal/broadphase/broadphase_continuous_collision_manager-inl.h include/coal/broadphase/broadphase_continuous_collision_manager.h include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h include/coal/broadphase/broadphase_dynamic_AABB_tree.h include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h include/coal/broadphase/broadphase_interval_tree.h include/coal/broadphase/broadphase_spatialhash-inl.h include/coal/broadphase/broadphase_spatialhash.h include/coal/broadphase/broadphase_callbacks.h include/coal/broadphase/default_broadphase_callbacks.h include/coal/broadphase/detail/hierarchy_tree-inl.h include/coal/broadphase/detail/hierarchy_tree.h include/coal/broadphase/detail/hierarchy_tree_array-inl.h include/coal/broadphase/detail/hierarchy_tree_array.h include/coal/broadphase/detail/interval_tree.h include/coal/broadphase/detail/interval_tree_node.h include/coal/broadphase/detail/morton-inl.h include/coal/broadphase/detail/morton.h include/coal/broadphase/detail/node_base-inl.h include/coal/broadphase/detail/node_base.h include/coal/broadphase/detail/node_base_array-inl.h include/coal/broadphase/detail/node_base_array.h include/coal/broadphase/detail/simple_hash_table-inl.h include/coal/broadphase/detail/simple_hash_table.h include/coal/broadphase/detail/simple_interval-inl.h include/coal/broadphase/detail/simple_interval.h include/coal/broadphase/detail/sparse_hash_table-inl.h include/coal/broadphase/detail/sparse_hash_table.h include/coal/broadphase/detail/spatial_hash-inl.h include/coal/broadphase/detail/spatial_hash.h include/coal/narrowphase/narrowphase.h include/coal/narrowphase/gjk.h include/coal/narrowphase/narrowphase_defaults.h include/coal/narrowphase/minkowski_difference.h include/coal/narrowphase/support_data.h include/coal/narrowphase/support_functions.h include/coal/shape/convex.h include/coal/shape/convex.hxx include/coal/shape/geometric_shape_to_BVH_model.h include/coal/shape/geometric_shapes.h include/coal/shape/geometric_shapes.hxx include/coal/shape/geometric_shapes_traits.h include/coal/shape/geometric_shapes_utility.h include/coal/distance_func_matrix.h include/coal/collision.h include/coal/collision_func_matrix.h include/coal/contact_patch.h include/coal/contact_patch_func_matrix.h include/coal/contact_patch/contact_patch_solver.h include/coal/contact_patch/contact_patch_solver.hxx include/coal/contact_patch/contact_patch_simplifier.h include/coal/contact_patch/polygon_convex_hull.h include/coal/distance.h include/coal/math/matrix_3f.h include/coal/math/vec_3f.h include/coal/math/types.h include/coal/math/transform.h include/coal/data_types.h include/coal/BVH/BVH_internal.h include/coal/BVH/BVH_model.h include/coal/BVH/BVH_front.h include/coal/BVH/BVH_utility.h include/coal/collision_object.h include/coal/collision_utility.h include/coal/hfield.h include/coal/fwd.hh include/coal/logging.h include/coal/mesh_loader/assimp.h include/coal/mesh_loader/loader.h include/coal/internal/BV_fitter.h include/coal/internal/BV_splitter.h include/coal/internal/shape_shape_func.h include/coal/internal/shape_shape_contact_patch_func.h include/coal/internal/intersect.h include/coal/internal/intersect.hxx include/coal/internal/tools.h include/coal/internal/traversal_node_base.h include/coal/internal/traversal_node_bvh_shape.h include/coal/internal/traversal_node_bvhs.h include/coal/internal/traversal_node_hfield_shape.h include/coal/internal/traversal_node_setup.h include/coal/internal/traversal_node_shapes.h include/coal/internal/traversal_recurse.h include/coal/internal/traversal.h include/coal/serialization/fwd.h include/coal/serialization/serializer.h include/coal/serialization/archive.h include/coal/serialization/transform.h include/coal/serialization/AABB.h include/coal/serialization/BV_node.h include/coal/serialization/BV_splitter.h include/coal/serialization/BVH_model.h include/coal/serialization/collision_data.h include/coal/serialization/contact_patch.h include/coal/serialization/collision_object.h include/coal/serialization/convex.h include/coal/serialization/eigen.h include/coal/serialization/geometric_shapes.h include/coal/serialization/memory.h include/coal/serialization/OBB.h include/coal/serialization/RSS.h include/coal/serialization/OBBRSS.h include/coal/serialization/kIOS.h include/coal/serialization/kDOP.h include/coal/serialization/hfield.h include/coal/serialization/quadrilateral.h include/coal/serialization/triangle.h include/coal/timings.h include/coal/shared_ptr_comparison.h ) # boost::span requires Boost >= 1.78.0. # On Ubuntu 22.04 the system Boost is 1.74 and is too old. # We must install boost/core/span headers. if(Boost_VERSION VERSION_LESS 1.78) message(INFO "Boost <= 1.78 detected: boost::span headers will be installed") list( APPEND ${PROJECT_NAME}_HEADERS include/coal/third_party/boost/core/data.hpp include/coal/third_party/boost/core/detail/assert.hpp include/coal/third_party/boost/core/make_span.hpp include/coal/third_party/boost/core/span.hpp ) endif() if( COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL AND NOT BUILD_STANDALONE_PYTHON_INTERFACE ) set( HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS include/hpp/fcl/broadphase/broadphase_bruteforce.h include/hpp/fcl/broadphase/broadphase_callbacks.h include/hpp/fcl/broadphase/broadphase_collision_manager.h include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h include/hpp/fcl/broadphase/broadphase.h include/hpp/fcl/broadphase/broadphase_interval_tree.h include/hpp/fcl/broadphase/broadphase_SaP.h include/hpp/fcl/broadphase/broadphase_spatialhash.h include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h include/hpp/fcl/broadphase/broadphase_SSaP.h include/hpp/fcl/broadphase/default_broadphase_callbacks.h include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h include/hpp/fcl/broadphase/detail/hierarchy_tree.h include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h include/hpp/fcl/broadphase/detail/interval_tree.h include/hpp/fcl/broadphase/detail/interval_tree_node.h include/hpp/fcl/broadphase/detail/morton.h include/hpp/fcl/broadphase/detail/morton-inl.h include/hpp/fcl/broadphase/detail/node_base_array.h include/hpp/fcl/broadphase/detail/node_base_array-inl.h include/hpp/fcl/broadphase/detail/node_base.h include/hpp/fcl/broadphase/detail/node_base-inl.h include/hpp/fcl/broadphase/detail/simple_hash_table.h include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h include/hpp/fcl/broadphase/detail/simple_interval.h include/hpp/fcl/broadphase/detail/simple_interval-inl.h include/hpp/fcl/broadphase/detail/sparse_hash_table.h include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h include/hpp/fcl/broadphase/detail/spatial_hash.h include/hpp/fcl/broadphase/detail/spatial_hash-inl.h include/hpp/fcl/BV/AABB.h include/hpp/fcl/BV/BV.h include/hpp/fcl/BV/BV_node.h include/hpp/fcl/BVH/BVH_front.h include/hpp/fcl/BVH/BVH_internal.h include/hpp/fcl/BVH/BVH_model.h include/hpp/fcl/BVH/BVH_utility.h include/hpp/fcl/BV/kDOP.h include/hpp/fcl/BV/kIOS.h include/hpp/fcl/BV/OBB.h include/hpp/fcl/BV/OBBRSS.h include/hpp/fcl/BV/RSS.h include/hpp/fcl/coal.hpp include/hpp/fcl/collision_data.h include/hpp/fcl/collision_func_matrix.h include/hpp/fcl/collision.h include/hpp/fcl/collision_object.h include/hpp/fcl/collision_utility.h include/hpp/fcl/config.hh include/hpp/fcl/contact_patch/contact_patch_solver.h include/hpp/fcl/contact_patch/contact_patch_solver.hxx include/hpp/fcl/contact_patch_func_matrix.h include/hpp/fcl/contact_patch.h include/hpp/fcl/data_types.h include/hpp/fcl/deprecated.hh include/hpp/fcl/distance_func_matrix.h include/hpp/fcl/distance.h include/hpp/fcl/fwd.hh include/hpp/fcl/hfield.h include/hpp/fcl/internal/BV_fitter.h include/hpp/fcl/internal/BV_splitter.h include/hpp/fcl/internal/intersect.h include/hpp/fcl/internal/shape_shape_contact_patch_func.h include/hpp/fcl/internal/shape_shape_func.h include/hpp/fcl/internal/tools.h include/hpp/fcl/internal/traversal.h include/hpp/fcl/internal/traversal_node_base.h include/hpp/fcl/internal/traversal_node_bvhs.h include/hpp/fcl/internal/traversal_node_bvh_shape.h include/hpp/fcl/internal/traversal_node_hfield_shape.h include/hpp/fcl/internal/traversal_node_setup.h include/hpp/fcl/internal/traversal_node_shapes.h include/hpp/fcl/internal/traversal_recurse.h include/hpp/fcl/internal/traversal_node_octree.h include/hpp/fcl/logging.h include/hpp/fcl/math/matrix_3f.h include/hpp/fcl/math/transform.h include/hpp/fcl/math/types.h include/hpp/fcl/math/vec_3f.h include/hpp/fcl/mesh_loader/assimp.h include/hpp/fcl/mesh_loader/loader.h include/hpp/fcl/narrowphase/gjk.h include/hpp/fcl/narrowphase/minkowski_difference.h include/hpp/fcl/narrowphase/narrowphase_defaults.h include/hpp/fcl/narrowphase/narrowphase.h include/hpp/fcl/narrowphase/support_data.h include/hpp/fcl/narrowphase/support_functions.h include/hpp/fcl/narrowphase/support_functions.hxx include/hpp/fcl/octree.h include/hpp/fcl/serialization/AABB.h include/hpp/fcl/serialization/archive.h include/hpp/fcl/serialization/BVH_model.h include/hpp/fcl/serialization/BV_node.h include/hpp/fcl/serialization/BV_splitter.h include/hpp/fcl/serialization/collision_data.h include/hpp/fcl/serialization/collision_object.h include/hpp/fcl/serialization/contact_patch.h include/hpp/fcl/serialization/convex.h include/hpp/fcl/serialization/eigen.h include/hpp/fcl/serialization/fwd.h include/hpp/fcl/serialization/geometric_shapes.h include/hpp/fcl/serialization/hfield.h include/hpp/fcl/serialization/kDOP.h include/hpp/fcl/serialization/kIOS.h include/hpp/fcl/serialization/memory.h include/hpp/fcl/serialization/OBB.h include/hpp/fcl/serialization/OBBRSS.h include/hpp/fcl/serialization/octree.h include/hpp/fcl/serialization/quadrilateral.h include/hpp/fcl/serialization/RSS.h include/hpp/fcl/serialization/serializer.h include/hpp/fcl/serialization/transform.h include/hpp/fcl/serialization/triangle.h include/hpp/fcl/shape/convex.h include/hpp/fcl/shape/convex.hxx include/hpp/fcl/shape/geometric_shapes.h include/hpp/fcl/shape/geometric_shapes.hxx include/hpp/fcl/shape/geometric_shapes_traits.h include/hpp/fcl/shape/geometric_shapes_utility.h include/hpp/fcl/shape/geometric_shape_to_BVH_model.h include/hpp/fcl/timings.h include/hpp/fcl/warning.hh ) list(APPEND ${PROJECT_NAME}_HEADERS ${HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS}) if(NOT BUILD_STANDALONE_PYTHON_INTERFACE) HEADER_INSTALL( COMPONENT hpp-fcl-compatibility ${HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS} ) endif() endif() if(COAL_HAS_OCTOMAP) list( APPEND ${PROJECT_NAME}_HEADERS include/coal/octree.h include/coal/serialization/octree.h include/coal/internal/traversal_node_octree.h ) endif(COAL_HAS_OCTOMAP) add_subdirectory(doc) if(NOT BUILD_STANDALONE_PYTHON_INTERFACE) add_subdirectory(src) endif() if(BUILD_PYTHON_INTERFACE) if(BUILD_STANDALONE_PYTHON_INTERFACE) ADD_PROJECT_DEPENDENCY(${PROJECT_NAME} REQUIRED CONFIG) endif() add_custom_target(${PROJECT_NAME}_python) set_target_properties( ${PROJECT_NAME}_python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True ) if(COAL_PYTHON_NANOBIND) find_package(Python REQUIRED COMPONENTS Interpreter Development.Module) # jrl-cmakemodules use PYTHON_EXECUTABLE defined in FINDPYTHON macro. # Since we doesn't call it in this branch, we must redefine it. set(PYTHON_EXECUTABLE ${Python_EXECUTABLE}) add_subdirectory(python-nb) else() set(PYTHON_COMPONENTS Interpreter Development NumPy) FINDPYTHON(REQUIRED) ADD_PROJECT_PRIVATE_DEPENDENCY(eigenpy 2.9.2 REQUIRED) add_subdirectory(python) endif() endif() if(BUILD_TESTING) add_subdirectory(test) endif(BUILD_TESTING) PKG_CONFIG_APPEND_LIBS("coal") if(COAL_HAS_OCTOMAP) # FCL_HAVE_OCTOMAP kept for backward compatibility reasons. PKG_CONFIG_APPEND_CFLAGS( "-DCOAL_HAS_OCTOMAP -DCOAL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}" ) endif(COAL_HAS_OCTOMAP) if( COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL AND NOT BUILD_STANDALONE_PYTHON_INTERFACE ) include(CMakePackageConfigHelpers) write_basic_package_version_file( hpp-fclConfigVersion.cmake VERSION 3.0.0 COMPATIBILITY AnyNewerVersion ) install( FILES hpp-fclConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/hpp-fclConfigVersion.cmake DESTINATION lib/cmake/hpp-fcl COMPONENT hpp-fcl-compatibility ) include("${JRL_CMAKE_MODULES}/install-helpers.cmake") ADD_INSTALL_TARGET(NAME hpp-fcl-compatibility COMPONENT hpp-fcl-compatibility) endif() SETUP_PROJECT_FINALIZE() ================================================ FILE: LICENSE ================================================ Software License Agreement (BSD License) Copyright (c) 2008-2014, Willow Garage, Inc. Copyright (c) 2014-2015, Open Source Robotics Foundation Copyright (c) 2014-2023, CNRS Copyright (c) 2018-2025, INRIA All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of Open Source Robotics Foundation nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ================================================ FILE: README.md ================================================ # Coal — An extension of the Flexible Collision Library

Pipeline status Documentation Coverage report Conda Downloads Conda Version PyPI version black ruff

[FCL](https://github.com/flexible-collision-library/fcl) was forked in 2015, creating a new project called HPP-FCL. Since then, a large part of the code has been rewritten or removed (unused and untested code), and new features have been introduced (see below). Due to these major changes, it was decided in 2024 to rename the HPP-FCL project to **Coal**. If you use **Coal** in your projects and research papers, we would appreciate it if you would [cite it](https://raw.githubusercontent.com/coal-library/coal/devel/CITATION.bib). ## New features Compared to the original [FCL](https://github.com/flexible-collision-library/fcl) library, the main new features are: - dedicated and efficient implementations of the GJK and the EPA algorithms (we do not rely on [libccd](https://github.com/danfis/libccd)) - the support of safety margins for collision detection - an accelerated version of collision detection *à la Nesterov*, which leads to increased performance (up to a factor of 2). More details are available in this [paper](https://hal.archives-ouvertes.fr/hal-03662157/) - the computation of a lower bound of the distance between two objects when collision checking is performed, and no collision is found - the implementation of Python bindings for easy code prototyping - the support of new geometries such as height fields, capsules, ellipsoids, etc. - enhance reliability with the fix of a myriad of bugs - efficient computation of **contact points** and **contact patches** between objects - full support of object serialization via Boost.Serialization Note: the broad phase was reintroduced by [Justin Carpentier](https://github.com/jcarpent) in 2022, based on the FCL version 0.7.0. This project is now used in several robotics frameworks such as [Pinocchio](https://github.com/stack-of-tasks/pinocchio), an open-source library which implements efficient and versatile rigid-body dynamics algorithms, the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc), an open-source library for Motion and Manipulation Planning. **Coal** has recently also been used to develop [Simple](https://github.com/Simple-Robotics/Simple), a new (differentiable) and efficient simulator for robotics and beyond. ## A high-performance library Unlike the original FCL library, Coal implements the well-established [GJK algorithm](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm) and [its variants](https://hal.archives-ouvertes.fr/hal-03662157/) for collision detection and distance computation. These implementations lead to state-of-the-art performance, as shown in the figures below. On the one hand, we have benchmarked Coal against major state-of-the-art software alternatives: 1. the [Bullet simulator](https://github.com/bulletphysics/bullet3), 2. the original [FCL library](https://github.com/flexible-collision-library/fcl) (used in the [Drake framework]()), 3. the [libccd library](https://github.com/danfis/libccd) (used in [MuJoCo](http://mujoco.org/)). The results are depicted in the following figure, which notably shows that the accelerated variants of GJK largely outperform by a large margin (from 5x up to 15x times faster). Please notice that the y-axis is in log scale.

Coal vs the rest of the world

On the other hand, why do we care about dedicated collision detection solvers like GJK for the narrow phase? Why can't we simply formulate the collision detection problem as a quadratic problem and call an off-the-shelf optimization solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite))? Here is why:

Coal vs generic QP solvers

One can observe that GJK-based approaches largely outperform solutions based on classic optimization solvers (e.g., QP solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite)), notably for large geometries composed of tens or hundreds of vertices. ## Open-source projects relying on Coal - [Pinocchio](https://github.com/stack-of-tasks/pinocchio) A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives. - [IfcOpenShell](https://github.com/IfcOpenShell/IfcOpenShell) Open source IFC library and geometry engine. - [Crocoddyl](https://github.com/loco-3d/crocoddyl) A software to realize model predictive control for complex robotics platforms. - [TSID](https://github.com/stack-of-tasks/tsid/) A software that implements a Task Space Inverse Dynamics QP. - [HPP](https://humanoid-path-planner.github.io/hpp-doc/) A SDK that implements motion planners for humanoids and other robots. - [Jiminy](https://github.com/duburcqa/jiminy) A simulator based on Pinocchio. - [ocs2](https://github.com/leggedrobotics/ocs2) A toolbox for Optimal Control for Switched Systems (OCS2) ## Installation ### Conda Coal can be installed from the [conda-forge channel](https://anaconda.org/conda-forge/coal): ```bash conda install coal -c conda-forge ``` ### Docker ``` docker run --rm -it ghcr.io/coal-library/coal:devel ``` ## Build You can find build instruction [here](./development/build.md). ## C++ example Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install coal`. The `.so` library, include files and python bindings will then be installed under `$CONDA_PREFIX/lib`, `$CONDA_PREFIX/include` and `$CONDA_PREFIX/lib/python3.XX/site-packages`. Here is an example of using Coal in C++: ```cpp #include "coal/math/transform.h" #include "coal/mesh_loader/loader.h" #include "coal/BVH/BVH_model.h" #include "coal/collision.h" #include "coal/collision_data.h" #include #include // Function to load a convex mesh from a `.obj`, `.stl` or `.dae` file. // // This function imports the object inside the file as a BVHModel, i.e. a point cloud // which is hierarchically transformed into a tree of bounding volumes. // The leaves of this tree are the individual points of the point cloud // stored in the `.obj` file. // This BVH can then be used for collision detection. // // For better computational efficiency, we sometimes prefer to work with // the convex hull of the point cloud. This insures that the underlying object // is convex and thus very fast collision detection algorithms such as // GJK or EPA can be called with this object. // Consequently, after creating the BVH structure from the point cloud, this function // also computes its convex hull. std::shared_ptr loadConvexMesh(const std::string& file_name) { coal::NODE_TYPE bv_type = coal::BV_AABB; coal::MeshLoader loader(bv_type); coal::BVHModelPtr_t bvh = loader.load(file_name); bvh->buildConvexHull(true, "Qt"); return bvh->convex; } int main() { // Create the coal shapes. // Coal supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes, // halfspace and convex meshes (i.e. convex hulls of clouds of points). // It also supports BVHs (bounding volumes hierarchies), height-fields and octrees. std::shared_ptr shape1 = std::make_shared(0.7, 1.0, 0.8); std::shared_ptr shape2 = loadConvexMesh("../path/to/mesh/file.obj"); // Define the shapes' placement in 3D space coal::Transform3s T1; T1.setQuatRotation(coal::Quaternion3f::UnitRandom()); T1.setTranslation(coal::Vec3s::Random()); coal::Transform3s T2 = coal::Transform3s::Identity(); T2.setQuatRotation(coal::Quaternion3f::UnitRandom()); T2.setTranslation(coal::Vec3s::Random()); // Define collision requests and results. // // The collision request allows to set parameters for the collision pair. // For example, we can set a positive or negative security margin. // If the distance between the shapes is less than the security margin, the shapes // will be considered in collision. // Setting a positive security margin can be usefull in motion planning, // i.e to prevent shapes from getting too close to one another. // In physics simulation, allowing a negative security margin may be usefull to stabilize the simulation. coal::CollisionRequest col_req; col_req.security_margin = 1e-1; // A collision result stores the result of the collision test (signed distance between the shapes, // witness points location, normal etc.) coal::CollisionResult col_res; // Collision call coal::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res); // We can access the collision result once it has been populated std::cout << "Collision? " << col_res.isCollision() << "\n"; if (col_res.isCollision()) { coal::Contact contact = col_res.getContact(0); // The penetration depth does **not** take into account the security margin. // Consequently, the penetration depth is the true signed distance which separates the shapes. // To have the distance which takes into account the security margin, we can simply add the two together. std::cout << "Penetration depth: " << contact.penetration_depth << "\n"; std::cout << "Distance between the shapes including the security margin: " << contact.penetration_depth + col_req.security_margin << "\n"; std::cout << "Witness point on shape1: " << contact.nearest_points[0].transpose() << "\n"; std::cout << "Witness point on shape2: " << contact.nearest_points[1].transpose() << "\n"; std::cout << "Normal: " << contact.normal.transpose() << "\n"; } // Before calling another collision test, it is important to clear the previous results stored in the collision result. col_res.clear(); return 0; } ``` ## Python example Here is the C++ example from above translated in python using the python bindings of Coal: ```python import numpy as np import coal # Optional: # The Pinocchio library is a rigid body algorithms library and has a handy SE3 module. # It can be installed as simply as `conda -c conda-forge install pinocchio`. # Installing pinocchio also installs coal. import pinocchio as pin def loadConvexMesh(file_name: str): loader = coal.MeshLoader() bvh: coal.BVHModelBase = loader.load(file_name) bvh.buildConvexHull(True, "Qt") return bvh.convex if __name__ == "__main__": # Create coal shapes shape1 = coal.Ellipsoid(0.7, 1.0, 0.8) shape2 = loadConvexMesh("../path/to/mesh/file.obj") # Define the shapes' placement in 3D space T1 = coal.Transform3s() T1.setTranslation(pin.SE3.Random().translation) T1.setRotation(pin.SE3.Random().rotation) T2 = coal.Transform3s(); # Using np arrays also works T1.setTranslation(np.random.rand(3)) T2.setRotation(pin.SE3.Random().rotation) # Define collision requests and results col_req = coal.CollisionRequest() col_res = coal.CollisionResult() # Collision call coal.collide(shape1, T1, shape2, T2, col_req, col_res) # Accessing the collision result once it has been populated print("Is collision? ", {col_res.isCollision()}) if col_res.isCollision(): contact: coal.Contact = col_res.getContact(0) print("Penetration depth: ", contact.penetration_depth) print("Distance between the shapes including the security margin: ", contact.penetration_depth + col_req.security_margin) print("Witness point shape1: ", contact.getNearestPoint1()) print("Witness point shape2: ", contact.getNearestPoint2()) print("Normal: ", contact.normal) # Before running another collision call, it is important to clear the old one col_res.clear() ``` ## Acknowledgments The development of **Coal** is actively supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr) and, to some extent, [Eureka Robotics](https://eurekarobotics.com/). ================================================ FILE: colcon.pkg ================================================ { "hooks": [ "share/hpp-fcl/hook/ament_prefix_path.dsv", "share/hpp-fcl/hook/python_path.dsv" ] } ================================================ FILE: development/build.md ================================================ # Build and install from source with Pixi To build Coal from source the easiest way is to use [Pixi](https://pixi.sh/latest/#installation). [Pixi](https://pixi.sh/latest/) is a cross-platform package management tool for developers that will install all required dependencies in `.pixi` directory. It's used by our CI agent so you have the guarantee to get the right dependencies. Run the following command to install dependencies, configure, build and test the project: ```bash pixi run test ``` The project will be built in the `build` directory. You can run `pixi shell` and build the project with `cmake` and `ninja` manually. ================================================ FILE: development/release.md ================================================ # Release with Pixi To create a release with Pixi run the following commands on the **devel** branch: ```bash COAL_VERSION=X.Y.Z pixi run release-new-version git push origin git push origin vX.Y.Z ``` Where `X.Y.Z` is the new version. Be careful to follow the [Semantic Versioning](https://semver.org/spec/v2.0.0.html) rules. You will find the following assets: - `./build_new_version/coal-X.Y.Z.tar.gz` - `./build_new_version/coal-X.Y.Z.tar.gz.sig` Then, create a new release on [GitHub](https://github.com/coal-library/coal/releases/new) with: * Tag: vX.Y.Z * Title: Coal X.Y.Z * Body: ``` ## What's Changed CHANGELOG CONTENT **Full Changelog**: https://github.com/coal-library/coal/compare/vXX.YY.ZZ...vX.Y.Z ``` Where `XX.YY.ZZ` is the last release version. Then upload `coal-X.Y.Z.tar.gz` and `coal-X.Y.Z.tar.gz.sig` and publish the release. ================================================ FILE: development/scripts/pixi/activation.bat ================================================ :: Find Python executable path for nanobind and Boost.Python for /f "tokens=*" %%i in ('python -c "import sys; print(sys.executable)"') do set PYTHON_EXECUTABLE=%%i :: Set default build value only if not previously set if not defined COAL_BUILD_TYPE (set COAL_BUILD_TYPE=Release) if not defined COAL_PYTHON_STUBS (set COAL_PYTHON_STUBS=ON) if not defined COAL_PYTHON_NANOBIND (set COAL_PYTHON_NANOBIND=ON) if not defined COAL_HAS_QHULL (set COAL_HAS_QHULL=OFF) ================================================ FILE: development/scripts/pixi/activation.sh ================================================ #! /bin/bash # Activation script # Remove flags setup from cxx-compiler unset CFLAGS unset CPPFLAGS unset CXXFLAGS unset DEBUG_CFLAGS unset DEBUG_CPPFLAGS unset DEBUG_CXXFLAGS unset LDFLAGS if [[ $host_alias == *"apple"* ]]; then # On OSX setting the rpath and -L it's important to use the conda libc++ instead of the system one. # If conda-forge use install_name_tool to package some libs, -headerpad_max_install_names is then mandatory export LDFLAGS="-Wl,-headerpad_max_install_names -Wl,-rpath,$CONDA_PREFIX/lib -L$CONDA_PREFIX/lib" elif [[ $host_alias == *"linux"* ]]; then # On GNU/Linux, I don't know if these flags are mandatory with g++ but # it allow to use clang++ as compiler export LDFLAGS="-Wl,-rpath,$CONDA_PREFIX/lib -Wl,-rpath-link,$CONDA_PREFIX/lib -L$CONDA_PREFIX/lib" # Conda compiler is named x86_64-conda-linux-gnu-c++, ccache can't resolve it # (https://ccache.dev/manual/latest.html#config_compiler_type) export CCACHE_COMPILERTYPE=gcc fi # Without -isystem, some LSP can't find headers export COAL_CXX_FLAGS="$CXXFLAGS -isystem $CONDA_PREFIX/include" # Set Python interpreter path for nanobind export PYTHON_EXECUTABLE=$(which python) # Set default build value only if not previously set export COAL_BUILD_TYPE=${COAL_BUILD_TYPE:=Release} export COAL_PYTHON_STUBS=${COAL_PYTHON_STUBS:=ON} export COAL_PYTHON_NANOBIND=${COAL_PYTHON_NANOBIND:=ON} export COAL_HAS_QHULL=${COAL_HAS_QHULL:=OFF} ================================================ FILE: doc/CMakeLists.txt ================================================ set(DOXYGEN_XML_OUTPUT "doxygen-xml" PARENT_SCOPE) set(DOXYGEN_FILE_PATTERNS "*.h *.hh *.hxx" PARENT_SCOPE) set(DOXYGEN_GENERATE_XML "YES" PARENT_SCOPE) set(DOXYGEN_EXPAND_ONLY_PREDEF "NO" PARENT_SCOPE) set(DOXYGEN_ENABLE_PREPROCESSING "YES" PARENT_SCOPE) set(DOXYGEN_MACRO_EXPANSION "YES" PARENT_SCOPE) set(DOXYGEN_EXCLUDE "${PROJECT_SOURCE_DIR}/include/hpp/") # We must not document octree if Octomap is not setup. # This create a build issue when building the bindings because doxygen-autodoc will # include octree.h that will include octomap.h. if(NOT COAL_HAS_OCTOMAP) set( DOXYGEN_EXCLUDE "${DOXYGEN_EXCLUDE} ${PROJECT_SOURCE_DIR}/include/coal/octree.h" ) set( DOXYGEN_EXCLUDE "${DOXYGEN_EXCLUDE} ${PROJECT_SOURCE_DIR}/include/coal/serialization/octree.h" ) set( DOXYGEN_EXCLUDE "${DOXYGEN_EXCLUDE} ${PROJECT_SOURCE_DIR}/include/coal/internal/traversal_node_octree.h" ) endif() set(DOXYGEN_EXCLUDE ${DOXYGEN_EXCLUDE} PARENT_SCOPE) set(DOXYGEN_PREDEFINED "IS_DOXYGEN" PARENT_SCOPE) ================================================ FILE: doc/Doxyfile.extra.in ================================================ USE_MATHJAX= YES ================================================ FILE: doc/generate_distance_plot.py ================================================ import matplotlib.pyplot as plt import numpy as np interactive = False m = 1.0 b = 1.2 mb = m + b X = np.array([-mb / 2, 0, m, mb, 2 * mb]) # X = np.linspace(-1, 4., 21) def dlb(d): if d < 0: return None if d > mb: u = d - mb return mb - m + u / 2 return d - m plt.figure(figsize=(9, 3.5)) # plt.plot(X, X-m, ":k") # plt.plot([m+b, X[-1]], [b, b], ":k") plt.fill_between( [m + b, X[-1]], [b, b], [b, X[-1] - m], alpha=0.2, hatch="|", facecolor="g", label="Distance lower band area", ) plt.plot(X, [dlb(x) for x in X], "-g", label="distance lower bound") # plt.plot([X[0], m, m, X[-1]], [0, 0, b, b], ":k") plt.axvspan(X[0], m, alpha=0.5, hatch="\\", facecolor="r", label="Collision area") ax = plt.gca() ax.set_xlabel("Object distance") ax.set_xticks([0, m, mb]) ax.set_xticklabels(["0", "security margin", "security margin\n+ break distance"]) ax.set_yticks([0, b]) ax.set_yticklabels(["0", "break distance"]) ax.grid(which="major", ls="solid") ax.grid(which="minor", ls="dashed") plt.axvline(0, ls="solid") # plt.axvline(m, ls="dashed", label="margin") # plt.axvline(mb, ls="dashed") plt.axhline(0.0, ls="solid") plt.title("Collision and distance lower band") plt.legend(loc="lower right") if interactive: plt.show() else: import os.path as path dir_path = path.dirname(path.realpath(__file__)) plt.savefig( path.join(dir_path, "distance_computation.png"), bbox_inches="tight", orientation="landscape", ) ================================================ FILE: doc/gjk.py ================================================ #!/usr/bin/env python3 import pdb import sys # ABC = AB^AC # (ABC^AJ).a = (j.c - j.b) a.a + (j.a - j.c) b.a + (j.b - j.a) c.a, for j = b or c segment_fmt = "{j}a_aa" plane_fmt = "" edge_fmt = "{j}a * {b}a_{c}a + {j}{b} * {c}a_aa - {j}{c} * {b}a_aa" # These checks must be negative and not positive, as in the cheat sheet. # They are the same as in the cheat sheet, except that we consider (...).dot(A) # instead of (...).dot(-A) plane_tests = ["C.dot (a_cross_b)", "D.dot(a_cross_c)", "-D.dot(a_cross_b)"] checks = ( plane_tests + [edge_fmt.format(**{"j": j, "b": "b", "c": "c"}) for j in ["b", "c"]] + [edge_fmt.format(**{"j": j, "b": "c", "c": "d"}) for j in ["c", "d"]] + [edge_fmt.format(**{"j": j, "b": "d", "c": "b"}) for j in ["d", "b"]] + [segment_fmt.format(**{"j": j}) for j in ["b", "c", "d"]] ) checks_hr = ( ["ABC.AO >= 0", "ACD.AO >= 0", "ADB.AO >= 0"] + ["(ABC ^ {}).AO >= 0".format(n) for n in ["AB", "AC"]] + ["(ACD ^ {}).AO >= 0".format(n) for n in ["AC", "AD"]] + ["(ADB ^ {}).AO >= 0".format(n) for n in ["AD", "AB"]] + ["AB.AO >= 0", "AC.AO >= 0", "AD.AO >= 0"] ) # weights of the checks. weights = ( [ 2, ] * 3 + [ 3, ] * 6 + [ 1, ] * 3 ) # Segment tests first, because they have lower weight. # tests = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, ] tests = [ 9, 10, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, ] assert len(tests) == len(checks) assert sorted(tests) == list(range(len(tests))) regions = [ "ABC", "ACD", "ADB", "AB", "AC", "AD", "A", "Inside", ] cases = list(range(len(regions))) # The following 3 lists refer to table doc/GJK_tetrahedra_boolean_table.ods # A check ID is (+/- (index+1)) where a minus sign encodes a NOT operation # and index refers to an index in list checks. # definitions is a list of list of check IDs to be ANDed. # For instance, a0.a3.!a4 -> [ 1, 4, -5] definitions = [ [1, 4, -5], [2, 6, -7], [3, 8, -9], [-4, 9, 10], [-6, 5, 11], [-8, 7, 12], [-10, -11, -12], [-1, -2, -3], ] # conditions is a list of (list of (list of check IDs to be ANDed) to be ORed). conditions = [ [], [], [], [], [], [], [], [], # [ [10, 11, 12], ], # I don't think this is always true... ] # rejections is a list of (list of (list of check IDs to be ANDed) to be ORed). rejections = [ [ [2, 6, 7], [3, -8, -9], ], [ [3, 8, 9], [1, -4, -5], ], [ [1, 4, 5], [2, -6, -7], ], [ [-1, -3], ], [ [-2, -1], ], [ [-3, -2], ], [ [4, -5], [6, -7], [8, -9], ], [], ] implications = [ [ [ 4, 5, 10, ], [11], ], [ [ 6, 7, 11, ], [12], ], [ [ 8, 9, 12, ], [10], ], [ [ -4, -5, 11, ], [10], ], [ [ -6, -7, 12, ], [11], ], [ [ -8, -9, 10, ], [12], ], [[1, 4, 5, 6], [-7]], [[2, 6, 9, 8], [-9]], [[3, 8, 9, 4], [-5]], [ [ -4, 5, 10, ], [-11], ], [ [ 4, -5, -10, ], [11], ], [ [ -6, 7, 11, ], [-12], ], [ [ 6, -7, -11, ], [12], ], [ [ -8, 9, 12, ], [-10], ], [ [ 8, -9, -12, ], [10], ], [[10, 3, 9, -12, 4, -5], [1]], [[10, -3, 1, -4], [9]], [[10, -3, -1, 2, -6, 11], [5]], [[-10, 11, 2, -12, -5, -1], [6]], [[-10, 11, -2, 1, 5], [-6]], [[-10, -11, 12, 1, -7, -2, 4], [-5]], [[-10, -11, 12, -3, 2, 7], [-8]], [[-10, -11, 12, -3, -2], [-1]], ] def set_test_values(current_tests, test_values, itest, value): def satisfies(values, indices): for k in indices: if k > 0 and values[k - 1] is not True: return False if k < 0 and values[-k - 1] is not False: return False return True remaining_tests = list(current_tests) next_test_values = list(test_values) remaining_tests.remove(itest) next_test_values[itest] = value rerun = True while rerun: rerun = False for impl in implications: if satisfies(next_test_values, impl[0]): for id in impl[1]: k = (id - 1) if id > 0 else (-id - 1) if k in remaining_tests: next_test_values[k] = id > 0 remaining_tests.remove(k) rerun = True else: if next_test_values[k] != (id > 0): raise ValueError("Absurd case") return remaining_tests, next_test_values def set_tests_values(current_tests, test_values, itests, values): for itest, value in zip(itests, values): current_tests, test_values = set_test_values( current_tests, test_values, itest, value ) return current_tests, test_values def apply_test_values(cases, test_values): def canSatisfy(values, indices): for k in indices: if k > 0 and values[k - 1] is False: return False if k < 0 and values[-k - 1] is True: return False return True def satisfies(values, indices): for k in indices: if k > 0 and values[k - 1] is not True: return False if k < 0 and values[-k - 1] is not False: return False return True # Check all cases. left_cases = [] for case in cases: defi = definitions[case] conds = conditions[case] rejs = rejections[case] if satisfies(test_values, defi): # A definition is True, stop recursion return [case] if not canSatisfy(test_values, defi): continue for cond in conds: if satisfies(test_values, cond): # A condition is True, stop recursion return [case] append = True for rej in rejs: if satisfies(test_values, rej): # A rejection is True, discard this case append = False break if append: left_cases.append(case) return left_cases def max_number_of_tests( current_tests, cases, test_values=[ None, ] * len(tests), prevBestScore=float("inf"), prevScore=0, ): for test in current_tests: assert test_values[test] is None, "Test " + str(test) + " already performed" left_cases = apply_test_values(cases, test_values) if len(left_cases) == 1: return prevScore, { "case": left_cases[0], } elif len(left_cases) == 0: return prevScore, { "case": None, "comments": [ "applied " + str(test_values), "to " + ", ".join([regions[c] for c in cases]), ], } assert len(current_tests) > 0, "No more test but " + str(left_cases) + " remains" currentBestScore = prevBestScore bestScore = float("inf") bestOrder = [None, None] for i, test in enumerate(current_tests): assert bestScore >= currentBestScore currentScore = prevScore + len(left_cases) * weights[test] # currentScore = prevScore + weights[test] if currentScore > currentBestScore: # Cannot do better -> stop continue try: remaining_tests, next_test_values = set_test_values( current_tests, test_values, test, True ) except ValueError: remaining_tests = None if remaining_tests is not None: # Do not put this in try catch as I do not want other ValueError to be # understood as an infeasible branch. score_if_t, order_if_t = max_number_of_tests( remaining_tests, left_cases, next_test_values, currentBestScore, currentScore, ) if score_if_t >= currentBestScore: # True didn't do better -> stop continue else: score_if_t, order_if_t = prevScore, None try: remaining_tests, next_test_values = set_test_values( current_tests, test_values, test, False ) except ValueError: remaining_tests = None if remaining_tests is not None: # Do not put this in try catch as I do not want other ValueError to be # understood as an infeasible branch. score_if_f, order_if_f = max_number_of_tests( remaining_tests, left_cases, next_test_values, currentBestScore, currentScore, ) else: score_if_f, order_if_f = prevScore, None currentScore = max(score_if_t, score_if_f) if currentScore < bestScore: if currentScore < currentBestScore: bestScore = currentScore bestOrder = {"test": test, "true": order_if_t, "false": order_if_f} # pdb.set_trace() currentBestScore = currentScore if len(tests) == len(current_tests): print("New best score: {}".format(currentBestScore)) return bestScore, bestOrder def printComments(order, indent, file): if "comments" in order: for comment in order["comments"]: print(indent + "// " + comment, file=file) def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]): if start: print( "bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)", file=file, ) print("{", file=file) print( indent + "// The code of this function was generated using doc/gjk.py", file=file, ) print(indent + "const vertex_id_t a = 3, b = 2, c = 1, d = 0;", file=file) for v in "abcd": print( indent + "const Vec3s& {} (current.vertex[{}]->w);".format(v.upper(), v), file=file, ) print(indent + "const Scalar aa = A.squaredNorm();".format(), file=file) for v in "dcb": for m in "abcd": if m <= v: print( indent + "const Scalar {0}{1} = {2}.dot({3});".format( v, m, v.upper(), m.upper() ), file=file, ) else: print( indent + "const Scalar& {0}{1} = {1}{0};".format(v, m), file=file, ) print(indent + "const Scalar {0}a_aa = {0}a - aa;".format(v), file=file) for l0, l1 in zip("bcd", "cdb"): print( indent + "const Scalar {0}a_{1}a = {0}a - {1}a;".format(l0, l1), file=file, ) for v in "bc": print( indent + "const Vec3s a_cross_{0} = A.cross({1});".format(v, v.upper()), file=file, ) print("", file=file) print("#define REGION_INSIDE() " + indent + "\\", file=file) print(indent + " ray.setZero(); \\", file=file) print(indent + " next.vertex[0] = current.vertex[d]; \\", file=file) print(indent + " next.vertex[1] = current.vertex[c]; \\", file=file) print(indent + " next.vertex[2] = current.vertex[b]; \\", file=file) print(indent + " next.vertex[3] = current.vertex[a]; \\", file=file) print(indent + " next.rank=4; \\", file=file) print(indent + " return true;", file=file) print("", file=file) if "case" in order: case = order["case"] if case is None: print( indent + "// There are no case corresponding to this set of tests.", file=file, ) printComments(order, indent, file) print(indent + "assert(false);", file=file) return region = regions[case] print(indent + "// Region " + region, file=file) printComments(order, indent, file) toFree = ["b", "c", "d"] if region == "Inside": print(indent + "REGION_INSIDE()", file=file) toFree = [] elif region == "A": print(indent + "originToPoint (current, a, A, next, ray);", file=file) elif len(region) == 2: region[0] B = region[1] print( indent + "originToSegment " "(current, a, {b}, A, {B}, {B}-A, -{b}a_aa, next, ray);".format( **{"b": B.lower(), "B": B} ), file=file, ) toFree.remove(B.lower()) elif len(region) == 3: B = region[1] C = region[2] test = plane_tests[["ABC", "ACD", "ADB"].index(region)] if test.startswith("-"): test = test[1:] else: test = "-" + test print( indent + "originToTriangle " "(current, a, {b}, {c}, ({B}-A).cross({C}-A), {t}, next, ray);".format( **{"b": B.lower(), "c": C.lower(), "B": B, "C": C, "t": test} ), file=file, ) toFree.remove(B.lower()) toFree.remove(C.lower()) else: assert False, "Unknown region " + region for pt in toFree: print( indent + "free_v[nfree++] = current.vertex[{}];".format(pt), file=file ) else: assert "test" in order and "true" in order and "false" in order check = checks[order["test"]] check_hr = checks_hr[order["test"]] printComments(order, indent, file) nextTests_t = curTests + [ "a" + str(order["test"] + 1), ] nextTests_f = curTests + [ "!a" + str(order["test"] + 1), ] if order["true"] is None: if order["false"] is None: print( indent + """assert(false && "Case {} should never happen.");""".format( check_hr ) ) else: print( indent + "assert(!({} <= 0)); // Not {} / {}".format( check, check_hr, ".".join(nextTests_f) ), file=file, ) printOrder( order["false"], indent=indent, start=False, file=file, curTests=nextTests_f, ) elif order["false"] is None: print( indent + "assert({} <= 0); // {} / {}".format( check, check_hr, ".".join(nextTests_t) ), file=file, ) printOrder( order["true"], indent=indent, start=False, file=file, curTests=nextTests_t, ) else: print( indent + "if ({} <= 0) {{ // if {} / {}".format( check, check_hr, ".".join(nextTests_t) ), file=file, ) printOrder( order["true"], indent=indent + " ", start=False, file=file, curTests=nextTests_t, ) print( indent + "}} else {{ // not {} / {}".format(check_hr, ".".join(nextTests_f)), file=file, ) printOrder( order["false"], indent=indent + " ", start=False, file=file, curTests=nextTests_f, ) print(indent + "}} // end of {}".format(check_hr), file=file) if start: print("", file=file) print("#undef REGION_INSIDE", file=file) print(indent + "return false;", file=file) print("}", file=file) def unit_tests(): # a4, a5, a10, a11, a12 cases = list(range(len(regions))) pdb.set_trace() left_cases = apply_test_values( cases, test_values=[ None, None, None, True, True, None, None, None, None, True, True, True, ], ) assert len(left_cases) > 1 # unit_tests() score, order = max_number_of_tests(tests, cases) print(score) printOrder(order, indent=" ") # TODO add weights such that: # - it is preferred to have all the use of one check in one branch. # idea: ponderate by the number of remaining tests. ================================================ FILE: doc/mesh-mesh-collision-call.dot ================================================ digraph CD { rankdir = BT compound=true size = 11.7 "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box] "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" [shape = box] "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" [shape = box] "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, Scalar& sqrDistLowerBound)" [shape = box] "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, Scalar& sqrDistLowerBound)" [shape = box] "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" [shape = box] "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" [shape = box] "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, Scalar& sqrDistLowerBound)" [shape = box] "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" [shape = box] "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n Scalar& squaredLowerBoundDistance)" [shape = box] "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n Scalar& squaredLowerBoundDistance)" "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, Scalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n Scalar& squaredLowerBoundDistance)" "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, Scalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, Scalar& sqrDistLowerBound)" "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"-> "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, Scalar& sqrDistLowerBound)" "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, Scalar& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, Scalar& sqrDistLowerBound)" -> "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, Scalar& sqrDistLowerBound)" "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, Scalar& sqrDistLowerBound)" "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" [color=red] "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" [color = red] "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\n -request\n - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nScalar& sqrDistLowerBound) const" } ================================================ FILE: doc/notes ================================================ In include/hpp/fcl/traversal: - some initialize method compute the position of all triangles in global frame before performing collision test, - bool initialize(MeshShapeCollisionTraversalNode& node, ... - bool initialize(ShapeMeshCollisionTraversalNode& node, ... - bool initialize(MeshCollisionTraversalNode& node,... - other do not. - bool initialize(MeshOcTreeCollisionTraversalNode& node, ... - bool initialize(OcTreeMeshCollisionTraversalNode& node, ... - bool initialize(MeshShapeCollisionTraversalNodeOBB& node, ... - bool initialize(MeshShapeCollisionTraversalNodeRSS& node, ... - bool initialize(MeshShapeCollisionTraversalNodekIOS& node, ... - bool initialize(MeshShapeCollisionTraversalNodeOBBRSS& node, ... - bool initialize(ShapeMeshCollisionTraversalNodeOBB& node, ... - bool initialize(ShapeMeshCollisionTraversalNodeRSS& node, ... - bool initialize(ShapeMeshCollisionTraversalNodekIOS& node, ... - bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS& node, ... - bool initialize(MeshCollisionTraversalNodeOBB& node, ... - bool initialize(MeshCollisionTraversalNodeRSS& node, ... - bool initialize(MeshCollisionTraversalNodekIOS& node, ... - bool initialize(MeshCollisionTraversalNodeOBBRSS& node, ... ------------------------------------------------------------------------------- classes - MeshCollisionTraversalNodeRSS, - MeshCollisionTraversalNodekIOS, - MeshCollisionTraversalNodeOBBRSS derive from - MeshCollisionTraversalNode , - MeshCollisionTraversalNode , - MeshCollisionTraversalNode . They store the relative position and orientation between two objects to test for collision. before calling overlap function, this additional information computes the relative positions of the bounding volumes. ------------------------------------------------------------------------------- For primitive shapes, collision and distance computation are very close. ShapeShapeCollide calls indeed ShapeShapeDistance. It would be convenient to merge CollisionRequest and DistanceRequest on the one hand and CollisionResult and DistanceResult on the other hand into two classes InteractionRequest and InteractionResult. ================================================ FILE: doc/python/doxygen-boost.hh ================================================ #ifndef DOXYGEN_BOOST_DOC_HH #define DOXYGEN_BOOST_DOC_HH #ifndef DOXYGEN_DOC_HH #error "You should have included doxygen.hh first." #endif // DOXYGEN_DOC_HH #include namespace doxygen { namespace visitor { template struct member_func_impl : boost::python::def_visitor< member_func_impl > { member_func_impl(const char* n, const function_type& f) : name(n), function(f) {} member_func_impl(const char* n, const function_type& f, policy_type p) : name(n), function(f), policy(p) {} template inline void visit(classT& c) const { // Either a boost::python::keyword object or a void_ object call(c, member_func_args(function)); } template inline void call( classT& c, const boost::python::detail::keywords& args) const { c.def(name, function, member_func_doc(function), args, policy); } template inline void call(classT& c, const void_&) const { c.def(name, function, member_func_doc(function), policy); } const char* name; const function_type& function; policy_type policy; }; // TODO surprisingly, this does not work when defined here but it works when // defined after the generated files are included. template inline member_func_impl member_func( const char* name, const function_type& function) { return member_func_impl(name, function); } template inline member_func_impl member_func( const char* name, const function_type& function, const policy_type& policy) { return member_func_impl(name, function, policy); } #define DOXYGEN_DOC_DECLARE_INIT_VISITOR(z, nargs, unused) \ template \ struct init_##nargs##_impl \ : boost::python::def_visitor< \ init_##nargs##_impl > { \ typedef constructor_##nargs##_impl \ constructor; \ typedef boost::python::init init_base; \ \ template \ inline void visit(classT& c) const { \ call(c, constructor::args()); \ } \ \ template \ void call(classT& c, \ const boost::python::detail::keywords& args) const { \ c.def(init_base(constructor::doc(), args)); \ } \ \ template \ void call(classT& c, const void_&) const { \ c.def(init_base(constructor::doc())); \ } \ }; \ \ template \ inline init_##nargs##_impl \ init() { \ return init_##nargs##_impl(); \ } BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR, DOXYGEN_DOC_DECLARE_INIT_VISITOR, ~) #undef DOXYGEN_DOC_DECLARE_INIT_VISITOR } // namespace visitor template void def(const char* name, Func func) { boost::python::def(name, func, member_func_doc(func)); } } // namespace doxygen #endif // DOXYGEN_BOOST_DOC_HH ================================================ FILE: doc/python/doxygen.hh ================================================ #ifndef DOXYGEN_DOC_HH #define DOXYGEN_DOC_HH #include #include #include #ifndef DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR #define DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR 10 #endif namespace doxygen { typedef boost::mpl::void_ void_; template struct class_doc_impl { static inline const char* run() { return ""; } static inline const char* attribute(const char*) { return ""; } }; template inline const char* class_doc() { return class_doc_impl<_class>::run(); } template inline const char* class_attrib_doc(const char* name) { return class_doc_impl<_class>::attribute(name); } template inline const char* member_func_doc(FuncPtr) { return ""; } template inline void_ member_func_args(FuncPtr) { return void_(); } #define DOXYGEN_DOC_DECLARE_CONSTRUCTOR(z, nargs, unused) \ template \ struct constructor_##nargs##_impl { \ static inline const char* doc() { return ""; } \ static inline void_ args() { return void_(); } \ }; \ \ template \ inline const char* constructor_doc() { \ return constructor_##nargs##_impl::doc(); \ } BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR, DOXYGEN_DOC_DECLARE_CONSTRUCTOR, ~) #undef DOXYGEN_DOC_DECLARE_CONSTRUCTOR /* template inline const char* constructor_doc () { return ""; } */ template struct destructor_doc_impl { static inline const char* run() { return ""; } }; template inline const char* destructor_doc() { return destructor_doc_impl::run(); } /* TODO class attribute can be handled by template const char* attribute_doc (AttributeType Class::* ptr) { // Body looks like // if (ptr == &Class::attributeName) // return "attrib documentation"; return "undocumented"; } */ } // namespace doxygen #endif // DOXYGEN_DOC_HH ================================================ FILE: doc/python/doxygen_xml_parser.py ================================================ #!/usr/bin/python3 # ruff: noqa: E501 from __future__ import print_function from lxml import etree from os import path from xml_docstring import XmlDocString import sys template_file_header = """#ifndef DOXYGEN_AUTODOC_{header_guard} #define DOXYGEN_AUTODOC_{header_guard} #include "{path}/doxygen.hh" """ template_file_footer = """ #endif // DOXYGEN_AUTODOC_{header_guard} """ template_class_doc = """ template <{tplargs}> struct class_doc_impl< {classname} > {{ static inline const char* run () {{ return "{docstring}"; }} static inline const char* attribute (const char* attrib) {{{attributes} (void)attrib; // turn off unused parameter warning. return ""; }} }};""" template_class_attribute_body = """ if (strcmp(attrib, "{attribute}") == 0) return "{docstring}";""" template_constructor_doc = """ template <{tplargs}> struct constructor_{nargs}_impl< {classname_prefix}{comma}{argsstring} > {{ static inline const char* doc () {{ return "{docstring}"; }} static inline boost::python::detail::keywords<{nargs}+1> args () {{ return ({argnamesstring}); }} }};""" template_destructor_doc = """ template <{tplargs}> struct destructor_doc_impl < {classname_prefix} > {{ static inline const char* run () {{ return "{docstring}"; }} }};""" template_member_func_doc = """ {template}inline const char* member_func_doc ({rettype} ({classname_prefix}*function_ptr) {argsstring}) {{{body} return ""; }}""" template_member_func_doc_body = """ if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername})) return "{docstring}";""" template_member_func_args = """ {template}inline boost::python::detail::keywords<{n}> member_func_args ({rettype} ({classname_prefix}*function_ptr) {argsstring}) {{{body} return ({default_args}); }}""" template_member_func_args_body = """ if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername})) return ({args});""" template_static_func_doc = """ {template}inline const char* member_func_doc ({rettype} (*function_ptr) {argsstring}) {{{body} return ""; }}""" template_static_func_doc_body = """ if (function_ptr == static_cast<{rettype} (*) {argsstring}>(&{namespace}::{membername})) return "{docstring}";""" template_open_namespace = """namespace {namespace} {{""" template_close_namespace = """}} // namespace {namespace}""" template_include_intern = """#include """ template_include_extern = """#include <{filename}> """ def _templateParamToDict(param): type_ = param.find("type") declname = param.find("declname") defname = param.find("defname") # FIXME type may contain references in two ways: # - the real param type # - the name of the template argument is recognized as the name of a type... if defname is None and declname is None: typetext = type_.text if typetext is None: typetext = "" for c in type_.iter(): if c == type_: continue if c.text is not None: typetext += c.text if c.tail is not None: typetext += c.tail if typetext.startswith("typename") or typetext.startswith("class"): if sys.version_info.major == 2: s = typetext.split() return {"type": s[0].strip(), "name": typetext[len(s[0]) :].strip()} else: s = typetext.split(maxsplit=1) assert len(s) == 2 return {"type": s[0].strip(), "name": s[1].strip()} else: return {"type": type_.text, "name": ""} else: if type_.text is None: # type_ is not a typename but an existing type type_ = type_.find("ref") assert defname.text == declname.text return {"type": type_.text, "name": defname.text} def makeHeaderGuard(filename): return filename.upper().replace(".", "_").replace("/", "_").replace("-", "_") def format_description(brief, detailed): b = [el.text.strip() for el in brief.iter() if el.text] if brief is not None else [] d = ( [el.text.strip() for el in detailed.iter() if el.text] if detailed is not None else [] ) text = "".join(b) if d: text += "\n" + "".join(d) return text class Reference(object): def __init__(self, index, id=None, name=None): self.id = id self.name = name self.index = index def xmlToType(self, node, array=None, parentClass=None, tplargs=None): """ - node: - parentClass: a class - tplargs: if one of the args is parentClass and no template arguments are provided, set the template arguments to this value - array: content of the sibling tag 'array' """ if node.text is not None: t = node.text.strip() else: t = "" for c in node.iterchildren(): if c.tag == "ref": refid = c.attrib["refid"] if parentClass is not None and refid == parentClass.id: t += " " + parentClass.name if c.tail is not None and c.tail.lstrip()[0] != "<": if tplargs is not None: t += tplargs elif ( parentClass is not None and isinstance(parentClass, ClassCompound) and parentClass.hasTypeDef(c.text.strip()) ): parent_has_templates = len(parentClass.template_params) > 0 if parent_has_templates: t += " typename " + parentClass._className() + "::" else: t += " " + parentClass._className() + "::" self_has_templates = ( c.tail is not None and c.tail.strip().find("<") != -1 ) if self_has_templates: t += " template " t += c.text.strip() elif self.index.hasref(refid): t += " " + self.index.getref(refid).name else: self.index.output.warn("Unknown reference: ", c.text, refid) t += " " + c.text.strip() else: if c.text is not None: t += " " + c.text.strip() if c.tail is not None: t += " " + c.tail.strip() if array is not None: t += array.text return t # Only for function as of now. class MemberDef(Reference): def __init__(self, index, memberdefxml, parent): super(MemberDef, self).__init__( index=index, id=memberdefxml.attrib["id"], name=memberdefxml.find("definition").text, ) self.parent = parent self.xml = memberdefxml self.const = memberdefxml.attrib["const"] == "yes" self.static = memberdefxml.attrib["static"] == "yes" self.rettype = memberdefxml.find("type") self.params = tuple( [ (param.find("type"), param.find("declname"), param.find("array")) for param in self.xml.findall("param") ] ) self.special = ( self.rettype.text is None and len(self.rettype.getchildren()) == 0 ) # assert self.special or len(self.rettype.text) > 0 self._templateParams(self.xml.find("templateparamlist")) def _templateParams(self, tpl): if tpl is not None: self.template_params = tuple( [_templateParamToDict(param) for param in tpl.iterchildren(tag="param")] ) else: self.template_params = tuple() def prototypekey(self): prototype = ( self.xmlToType(self.rettype, parentClass=self.parent), tuple([tuple(t.items()) for t in self.template_params]), tuple( [ self.xmlToType(param.find("type"), parentClass=self.parent) for param in self.xml.findall("param") ] ), self.const, ) return prototype def s_prototypeArgs(self): return "({0}){1}".format(self.s_args(), " const" if self.const else "") def s_args(self): # If the class is templated, check if one of the argument is the class itself. # If so, we must add the template arguments to the class (if there is none) if len(self.parent.template_params) > 0: tplargs = ( " <" + ", ".join([d["name"] for d in self.parent.template_params]) + " > " ) args = ", ".join( [ self.xmlToType( type, array, parentClass=self.parent, tplargs=tplargs ) for type, declname, array in self.params ] ) else: args = ", ".join( [ self.xmlToType(type, array, parentClass=self.parent) for type, declname, array in self.params ] ) return args def s_tpldecl(self): if len(self.template_params) == 0: return "" return ", ".join([d["type"] + " " + d["name"] for d in self.template_params]) def s_rettype(self): assert not self.special, ( "Member {} ({}) is a special function and no return type".format( self.name, self.id ) ) if len(self.parent.template_params) > 0: tplargs = ( " <" + ", ".join([d["name"] for d in self.parent.template_params]) + " > " ) else: tplargs = None if isinstance(self.parent, ClassCompound): return self.xmlToType( self.rettype, parentClass=self.parent, tplargs=tplargs ) else: return self.xmlToType(self.rettype) def s_name(self): return self.xml.find("name").text.strip() def s_docstring(self): return self.index.xml_docstring.getDocString( self.xml.find("briefdescription"), self.xml.find("detaileddescription"), self.index.output, ) def n_args(self): return len(self.params) def s_argnamesstring(self): def getdeclname(i, declname): if declname is None or declname.text is None or declname.text.strip() == "": return "arg{}".format(i) return declname.text.strip() arg = """boost::python::arg("{}")""" argnames = [ "self", ] + [getdeclname(i, declname) for i, (_, declname, _) in enumerate(self.params)] return ", ".join([arg.format(n) for n in argnames]) def include(self): loc = self.xml.find("location") # The location is based on $CMAKE_SOURCE_DIR. Remove first directory. return loc.attrib["file"].split("/", 1)[1] class CompoundBase(Reference): def __init__(self, compound, index): self.compound = compound self.filename = path.join(index.directory, compound.attrib["refid"] + ".xml") self.tree = etree.parse(self.filename) self.definition = self.tree.getroot().find("compounddef") super(CompoundBase, self).__init__( index, id=self.definition.attrib["id"], name=self.definition.find("compoundname").text, ) class NamespaceCompound(CompoundBase): def __init__(self, *args): super(NamespaceCompound, self).__init__(*args) self.typedefs = [] self.enums = [] self.static_funcs = [] self.template_params = tuple() # Add references for section in self.definition.iterchildren("sectiondef"): assert "kind" in section.attrib kind = section.attrib["kind"] if kind == "enum": self.parseEnumSection(section) elif kind == "typedef": self.parseTypedefSection(section) elif kind == "func": self.parseFuncSection(section) def parseEnumSection(self, section): for member in section.iterchildren("memberdef"): ref = Reference( index=self.index, id=member.attrib["id"], name=self.name + "::" + member.find("name").text, ) self.index.registerReference(ref) self.enums.append(member) for value in member.iterchildren("enumvalue"): ref = Reference( index=self.index, id=value.attrib["id"], name=self.name + "::" + member.find("name").text, ) def parseTypedefSection(self, section): for member in section.iterchildren("memberdef"): ref = Reference( index=self.index, id=member.attrib["id"], name=self.name + "::" + member.find("name").text, ) self.index.registerReference(ref) self.typedefs.append(member) def parseFuncSection(self, section): for member in section.iterchildren("memberdef"): self.static_funcs.append(MemberDef(self.index, member, self)) def innerNamespace(self): return self.name def write(self, output): pass class ClassCompound(CompoundBase): def __init__(self, *args): super(ClassCompound, self).__init__(*args) self.member_funcs = list() self.static_funcs = list() self.special_funcs = list() self.attributes = list() self.struct = self.compound.attrib["kind"] == "struct" self.public = self.definition.attrib["prot"] == "public" self.template_specialization = self.name.find("<") > 0 self.typedef = dict() # Handle templates self._templateParams(self.definition.find("templateparamlist")) for memberdef in self.definition.iter(tag="memberdef"): if memberdef.attrib["prot"] != "public": continue if memberdef.attrib["kind"] == "variable": self._attribute(memberdef) elif memberdef.attrib["kind"] == "typedef": ref = Reference( index=self.index, id=memberdef.attrib["id"], name=self._className() + "::" + memberdef.find("name").text, ) self.index.registerReference(ref) self.typedef[memberdef.find("name").text.strip()] = True elif memberdef.attrib["kind"] == "enum": if memberdef.find("name").text is None: ref_name = self._className() + "::" + "anonymous_enum" else: ref_name = self._className() + "::" + memberdef.find("name").text ref = Reference( index=self.index, id=memberdef.attrib["id"], name=ref_name, ) self.index.registerReference(ref) for value in memberdef.iterchildren("enumvalue"): value_ref = Reference( index=self.index, id=value.attrib["id"], name=ref.name, ) self.index.registerReference(value_ref) elif memberdef.attrib["kind"] == "function": self._memberfunc(memberdef) def _templateParams(self, tpl): if tpl is not None: self.template_params = tuple( [_templateParamToDict(param) for param in tpl.iterchildren(tag="param")] ) else: self.template_params = tuple() def _templateDecl(self): if not hasattr(self, "template_params") or len(self.template_params) == 0: return "" return ", ".join([d["type"] + " " + d["name"] for d in self.template_params]) def _className(self): if not hasattr(self, "template_params") or len(self.template_params) == 0: return self.name return ( self.name + " <" + ", ".join([d["name"] for d in self.template_params]) + " >" ) def hasTypeDef(self, typename): return typename in self.typedef def innerNamespace(self): return self._className() def _memberfunc(self, member): m = MemberDef(self.index, member, self) if m.special: self.special_funcs.append(m) elif m.static: self.static_funcs.append(m) else: self.member_funcs.append(m) def _writeClassDoc(self, output): docstring = self.index.xml_docstring.getDocString( self.definition.find("briefdescription"), self.definition.find("detaileddescription"), self.index.output, ) attribute_docstrings = "" for member in self.attributes: _dc = self.index.xml_docstring.getDocString( member.find("briefdescription"), member.find("detaileddescription"), self.index.output, ) if len(_dc) == 0: continue attribute_docstrings += template_class_attribute_body.format( attribute=member.find("name").text, docstring=_dc, ) if len(docstring) == 0 and len(attribute_docstrings) == 0: return output.out( template_class_doc.format( tplargs=self._templateDecl(), classname=self._className(), docstring=docstring, attributes=attribute_docstrings, ) ) def write(self, output): if not self.public: return if self.template_specialization: output.warn( "Disable class {} because template argument are not resolved for templated class specialization.".format( self.name ) ) return include = self.definition.find("includes") if include is None: output.err("Does not know where to write doc of", self.name) return output.open(include.text) output.out(template_include_extern.format(filename=include.text)) output.out(template_open_namespace.format(namespace="doxygen")) # Write class doc self._writeClassDoc(output) # Group member function by prototype member_funcs = dict() for m in self.member_funcs: prototype = m.prototypekey() if prototype in member_funcs: member_funcs[prototype].append(m) else: member_funcs[prototype] = [ m, ] classname_prefix = self._className() + "::" for member in self.special_funcs: docstring = member.s_docstring() argnamesstring = member.s_argnamesstring() if len(docstring) == 0 and len(argnamesstring) == 0: continue if member.s_name()[0] == "~": output.out( template_destructor_doc.format( tplargs=self._templateDecl(), classname_prefix=self._className(), docstring=docstring, ) ) else: output.out( template_constructor_doc.format( tplargs=", ".join( [ d["type"] + " " + d["name"] for d in self.template_params + member.template_params ] ), nargs=len(member.params), comma=", " if len(member.params) > 0 else "", classname_prefix=self._className(), argsstring=member.s_args(), docstring=docstring, argnamesstring=argnamesstring, ) ) for prototype, members in member_funcs.items(): # remove undocumented members documented_members = [] docstrings = [] argnamesstrings = [] for member in members: docstring = member.s_docstring() argnamesstring = member.s_argnamesstring() if len(docstring) == 0 and len(argnamesstring) == 0: continue documented_members.append(member) docstrings.append(docstring) argnamesstrings.append(argnamesstring) if len(documented_members) == 0: continue # Write docstrings body = "".join( [ template_member_func_doc_body.format( classname_prefix=classname_prefix, membername=member.s_name(), docstring=docstring, rettype=member.s_rettype(), argsstring=member.s_prototypeArgs(), ) for member, docstring in zip(documented_members, docstrings) ] ) member = members[0] tplargs = ", ".join( [ d["type"] + " " + d["name"] for d in self.template_params + member.template_params ] ) output.out( template_member_func_doc.format( template=( "template <{}>\n".format(tplargs) if len(tplargs) > 0 else "" ), rettype=member.s_rettype(), classname_prefix=classname_prefix, argsstring=member.s_prototypeArgs(), body=body, ) ) # Write argnamesstrings body = "".join( [ template_member_func_args_body.format( classname_prefix=classname_prefix, membername=member.s_name(), args=argnamesstring, rettype=member.s_rettype(), argsstring=member.s_prototypeArgs(), ) for member, argnamesstring in zip( documented_members, argnamesstrings ) ] ) n_args = member.n_args() default_args = ", ".join( [ """boost::python::arg("self")""", ] + ["""boost::python::arg("arg{}")""".format(i) for i in range(n_args)] ) output.out( template_member_func_args.format( template=( "template <{}>\n".format(tplargs) if len(tplargs) > 0 else "" ), rettype=member.s_rettype(), n=n_args + 1, default_args=default_args, classname_prefix=classname_prefix, argsstring=member.s_prototypeArgs(), body=body, ) ) output.out(template_close_namespace.format(namespace="doxygen")) output.close() def _attribute(self, member): self.attributes.append(member) class Index: """ This class is responsible for generating the list of all C++-usable documented elements. """ def __init__(self, input, output): self.tree = etree.parse(input) self.directory = path.dirname(input) self.xml_docstring = XmlDocString(self) self.compounds = list() self.references = dict() self.output = output def parseCompound(self): for compound in self.tree.getroot().iterchildren("compound"): if compound.attrib["kind"] in ["class", "struct"]: obj = ClassCompound(compound, self) elif compound.attrib["kind"] == "namespace": obj = NamespaceCompound(compound, self) if obj.id not in self.compounds: self.compounds.append(obj.id) self.registerReference(obj) def write(self): # Header self.output.open("doxygen_xml_parser_for_cmake.hh") # self.output.out ("// Generated on {}".format (asctime())) self.output.close() # Implement template specialization for classes and member functions for id in self.compounds: compound = self.references[id] compound.write(self.output) self.output.open("functions.h") # Implement template specialization for static functions static_funcs = dict() prototypes = list() includes = list() for id in self.compounds: compound = self.references[id] for m in compound.static_funcs: include = m.include() if include not in includes: includes.append(include) docstring = m.s_docstring() if len(docstring) == 0: continue prototype = m.prototypekey() if prototype in static_funcs: static_funcs[prototype].append((m, docstring)) else: static_funcs[prototype] = [ (m, docstring), ] prototypes.append(prototype) self.output.out( "".join( [ template_include_extern.format(filename=filename) for filename in includes ] ) ) self.output.out(template_open_namespace.format(namespace="doxygen")) for prototype in prototypes: member_and_docstring_s = static_funcs[prototype] body = "".join( [ template_static_func_doc_body.format( namespace=member.parent.innerNamespace(), membername=member.s_name(), docstring=docstring, rettype=member.s_rettype(), argsstring=member.s_prototypeArgs(), ) for member, docstring in member_and_docstring_s ] ) member = member_and_docstring_s[0][0] # TODO fix case of static method in templated class. tplargs = ", ".join( [ d["type"] + " " + d["name"] for d in member.parent.template_params + member.template_params ] ) self.output.out( template_static_func_doc.format( template=( "template <{}>\n".format(tplargs) if len(tplargs) > 0 else "" ), rettype=member.s_rettype(), argsstring=member.s_prototypeArgs(), body=body, ) ) self.output.out(template_close_namespace.format(namespace="doxygen")) self.output.close() def registerReference(self, obj, overwrite=True): if obj.id in self.references: if obj.name != self.references[obj.id].name: self.output.warn( "!!!! Compounds " + obj.id + " already exists.", obj.name, self.references[obj.id].name, ) else: self.output.warn("Reference " + obj.id + " already exists.", obj.name) if not overwrite: return self.references[obj.id] = obj def hasref(self, id): return id in self.references def getref(self, id): return self.references[id] class OutputStreams(object): def __init__(self, output_dir, warn, error, errorPrefix=""): self.output_dir = output_dir self._out = None self._warn = warn self._err = error self.errorPrefix = errorPrefix self._created_files = dict() def open(self, name): assert self._out is None, "You did not close the previous file" import os fullname = os.path.join(self.output_dir, name) dirname = os.path.dirname(fullname) if not os.path.isdir(dirname): os.makedirs(dirname) if name in self._created_files: self._out = self._created_files[name] else: import codecs if sys.version_info >= (3,): encoding = "utf-8" else: encoding = "latin1" self._out = codecs.open(fullname, mode="w", encoding=encoding) self._created_files[name] = self._out # Header self.out( template_file_header.format( path=os.path.dirname(os.path.abspath(__file__)), header_guard=makeHeaderGuard(name), ) ) def close(self): self._out = None def writeFooterAndCloseFiles(self): for n, f in self._created_files.items(): # Footer self._out = f self.out( template_file_footer.format( header_guard=makeHeaderGuard(n), ) ) f.close() self._created_files.clear() self._out = None def out(self, *args): if sys.version_info >= (3,): print(*args, file=self._out) else: print(" ".join(str(arg) for arg in args).decode("latin1"), file=self._out) def warn(self, *args): print(self.errorPrefix, *args, file=self._warn) def err(self, *args): print(self.errorPrefix, *args, file=self._err) if __name__ == "__main__": import argparse parser = argparse.ArgumentParser( description="Process Doxygen XML documentation and generate C++ code." ) parser.add_argument("doxygen_index_xml", type=str, help="the Doxygen XML index.") parser.add_argument("output_directory", type=str, help="the output directory.") args = parser.parse_args() index = Index( input=sys.argv[1], output=OutputStreams(args.output_directory, sys.stdout, sys.stderr), ) index.parseCompound() index.write() index.output.writeFooterAndCloseFiles() assert index.output._out is None ================================================ FILE: doc/python/xml_docstring.py ================================================ class XmlDocString(object): def __init__(self, index): self.index = index self.tags = { "para": self.para, "ref": self.ref, "briefdescription": self.otherTags, "detaileddescription": self.otherTags, "parameterlist": self.parameterlist, "parameterdescription": self.otherTags, "emphasis": self.emphasis, "simplesect": self.simplesect, "formula": self.formula, "itemizedlist": self.itemizedlist, "listitem": self.listitem, } self.unkwownTags = set() self.unkwownReferences = dict() self._linesep = '\\n"\n"' try: from pylatexenc.latex2text import LatexNodes2Text self.latex = LatexNodes2Text() except ImportError: self.latex = None def clear(self): self.lines = [] self.unkwownTags.clear() self.unkwownReferences.clear() def writeErrors(self, output): ret = False for t in self.unkwownTags: output.warn("Unknown tag: ", t) ret = True for ref, node in self.unkwownReferences.items(): output.warn("Unknown reference: ", ref, node.text) ret = True return ret def _write(self, str): nlines = str.split("\n") if len(self.lines) == 0: self.lines += nlines else: self.lines[-1] += nlines[0] self.lines += nlines[1:] # self.lines += nlines[1:] def _newline(self, n=1): self.lines.extend( [ "", ] * n ) def _clean(self): s = 0 for line in self.lines: if len(line.strip()) == 0: s += 1 else: break e = len(self.lines) for line in reversed(self.lines): if len(line.strip()) == 0: e -= 1 else: break self.lines = self.lines[s:e] def getDocString(self, brief, detailled, output): self.clear() if brief is not None: self.visit(brief) if detailled is not None and len(detailled.getchildren()) > 0: if brief is not None: self._newline() self.visit(detailled) from sys import version_info self.writeErrors(output) self._clean() if version_info[0] == 2: return self._linesep.join(self.lines).encode("utf-8") else: return self._linesep.join(self.lines) def visit(self, node): assert isinstance(node.tag, str) tag = node.tag if tag not in self.tags: self.unknownTag(node) else: self.tags[tag](node) def unknownTag(self, node): self.unkwownTags.add(node.tag) self.otherTags(node) def otherTags(self, node): if node.text: self._write(node.text.strip().replace('"', r"\"")) for c in node.iterchildren(): self.visit(c) if c.tail: self._write(c.tail.strip().replace('"', r"\"")) def emphasis(self, node): self._write("*") self.otherTags(node) self._write("*") def simplesect(self, node): self._write(node.attrib["kind"].title() + ": ") self.otherTags(node) def para(self, node): if node.text: self._write(node.text.replace('"', r"\"")) for c in node.iterchildren(): self.visit(c) if c.tail: self._write(c.tail.replace('"', r"\"")) self._newline() def ref(self, node): refid = node.attrib["refid"] if self.index.hasref(refid): self._write(self.index.getref(refid).name) else: self.unkwownReferences[refid] = node self._write(node.text) assert len(node.getchildren()) == 0 def parameterlist(self, node): self._newline() self._write(node.attrib["kind"].title()) self._newline() for item in node.iterchildren("parameteritem"): self.parameteritem(item) def parameteritem(self, node): indent = " " self._write(indent + "- ") # should contain two children assert len(node.getchildren()) == 2 namelist = node.find("parameternamelist") desc = node.find("parameterdescription") sep = "" for name in namelist.iterchildren("parametername"): self._write(sep + name.text) sep = ", " self._write(" ") self.visit(desc) def itemizedlist(self, node): self._newline() self.otherTags(node) def listitem(self, node): self._write("- ") self.otherTags(node) def formula(self, node): if node.text: if self.latex is None: self._write(node.text.strip()) else: self._write(self.latex.latex_to_text(node.text)) ================================================ FILE: doc/shape-mesh-collision-call.dot ================================================ digraph CD { rankdir = BT compound=true size = 11.7 "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" [shape = box] "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" [shape = box] "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box] "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box] "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box] "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box] "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, Scalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box] "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box] "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red] "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red] "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, Scalar& sqrDistLowerBound)\nBV/OBB.cpp" "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, Scalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, Scalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" } ================================================ FILE: doc/shape-shape-collision-call.dot ================================================ digraph CD { rankdir = BT compound=true size = 11.7 "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box] "template\nScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box] "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box] "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box] "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box] "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box] "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template\nScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" "template\nScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" -> "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" -> "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" } ================================================ FILE: dockgen.toml ================================================ [eigenpy] url = "github:stack-of-tasks" [coal] url = "." apt_deps = ["libassimp-dev", "liboctomap-dev"] src_deps = ["eigenpy"] ================================================ FILE: flake.nix ================================================ { description = "An extension of the Flexible Collision Library"; inputs = { gepetto.url = "github:gepetto/nix"; # eigenpy v3.12.0 does not handle eigen v5, so we need devel for now eigenpy.url = "github:stack-of-tasks/eigenpy"; eigenpy.inputs.gepetto.follows = "gepetto"; }; outputs = inputs: inputs.gepetto.lib.mkFlakoboros inputs ( { lib, ... }: { overlays = [ inputs.eigenpy.overlays.flakoboros ]; extraDevPyPackages = [ "coal" ]; overrideAttrs.coal = { drv-prev, ... }: { cmakeFlags = drv-prev.cmakeFlags ++ [ "-DCOAL_DISABLE_HPP_FCL_WARNINGS=ON" "-DGENERATE_PYTHON_STUBS=OFF" ]; src = lib.fileset.toSource { root = ./.; fileset = lib.fileset.unions [ ./CMakeLists.txt ./doc ./hpp-fclConfig.cmake ./include ./package.xml ./python ./python-nb ./src ./test ]; }; }; extends = { inherit (inputs.eigenpy.overlays) eigen5; full = _final: prev: { pythonPackagesExtensions = prev.pythonPackagesExtensions ++ [ (_python-final: python-prev: { coal = python-prev.coal.override { buildStandalone = false; }; }) ]; }; nanobind = _final: prev: { pythonPackagesExtensions = prev.pythonPackagesExtensions ++ [ (python-final: python-prev: { coal = python-prev.coal.overrideAttrs (super: { pname = "coal-nb"; cmakeFlags = super.cmakeFlags ++ [ "-DCOAL_PYTHON_NANOBIND=ON" ]; postPatch = '' substituteInPlace python-nb/CMakeLists.txt --replace-fail \ "$""{Python_SITELIB}" \ "${python-final.python.sitePackages}" ''; propagatedBuildInputs = super.propagatedBuildInputs ++ [ python-final.nanobind python-final.nanoeigenpy ]; pythonImportsCheck = [ "coal" ]; # hppfcl is broken with nanobind }); }) ]; }; }; } ); } ================================================ FILE: hpp-fclConfig.cmake ================================================ # This file provide bacward compatiblity for `find_package(hpp-fcl)`. if(NOT COAL_DISABLE_HPP_FCL_WARNINGS) message( WARNING "Please update your CMake from 'hpp-fcl' to 'coal', or define COAL_DISABLE_HPP_FCL_WARNINGS" ) endif() find_package(coal REQUIRED) if(NOT TARGET hpp-fcl::hpp-fcl) add_library(hpp-fcl::hpp-fcl INTERFACE IMPORTED) # Compute the installation prefix relative to this file. # This code is taken from generated cmake xxxTargets.cmake. get_filename_component(_IMPORT_PREFIX "${CMAKE_CURRENT_LIST_FILE}" PATH) get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH) get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH) get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH) if(_IMPORT_PREFIX STREQUAL "/") set(_IMPORT_PREFIX "") endif() set_target_properties( hpp-fcl::hpp-fcl PROPERTIES INTERFACE_COMPILE_DEFINITIONS "COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL" INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include" INTERFACE_LINK_LIBRARIES "coal::coal" ) endif() ================================================ FILE: include/coal/BV/AABB.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_AABB_H #define COAL_AABB_H #include "coal/fwd.hh" #include "coal/data_types.h" namespace coal { struct CollisionRequest; class Plane; class Halfspace; /// @defgroup Bounding_Volume Bounding volumes /// Classes of different types of bounding volume. /// @{ /// @brief A class describing the AABB collision structure, which is a box in 3D /// space determined by two diagonal points class COAL_DLLAPI AABB { public: /// @brief The min point in the AABB Vec3s min_; /// @brief The max point in the AABB Vec3s max_; /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf) AABB(); /// @brief Creating an AABB at position v with zero size AABB(const Vec3s& v) : min_(v), max_(v) {} /// @brief Creating an AABB with two endpoints a and b AABB(const Vec3s& a, const Vec3s& b) : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {} /// @brief Creating an AABB centered as core and is of half-dimension delta AABB(const AABB& core, const Vec3s& delta) : min_(core.min_ - delta), max_(core.max_ + delta) {} /// @brief Creating an AABB contains three points AABB(const Vec3s& a, const Vec3s& b, const Vec3s& c) : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {} AABB(const AABB& other) = default; AABB& operator=(const AABB& other) = default; AABB& update(const Vec3s& a, const Vec3s& b) { min_ = a.cwiseMin(b); max_ = a.cwiseMax(b); return *this; } /// @brief Comparison operator bool operator==(const AABB& other) const { COAL_EQUAL_OPERATOR_CHECK(min_ == other.min_); COAL_EQUAL_OPERATOR_CHECK(max_ == other.max_); return true; } bool operator!=(const AABB& other) const { return !(*this == other); } /// @name Bounding volume API /// Common API to BVs. /// @{ /// @brief Check whether the AABB contains a point inline bool contain(const Vec3s& p) const { if (p[0] < min_[0] || p[0] > max_[0]) return false; if (p[1] < min_[1] || p[1] > max_[1]) return false; if (p[2] < min_[2] || p[2] > max_[2]) return false; return true; } /// @brief Check whether two AABB are overlap inline bool overlap(const AABB& other) const { if (min_[0] > other.max_[0]) return false; if (min_[1] > other.max_[1]) return false; if (min_[2] > other.max_[2]) return false; if (max_[0] < other.min_[0]) return false; if (max_[1] < other.min_[1]) return false; if (max_[2] < other.min_[2]) return false; return true; } /// @brief Check whether AABB overlaps a plane bool overlap(const Plane& p) const; /// @brief Check whether AABB overlaps a halfspace bool overlap(const Halfspace& hs) const; /// @brief Check whether two AABB are overlap bool overlap(const AABB& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const; /// @brief Distance between two AABBs Scalar distance(const AABB& other) const; /// @brief Distance between two AABBs; P and Q, should not be NULL, return the /// nearest points Scalar distance(const AABB& other, Vec3s* P, Vec3s* Q) const; /// @brief Merge the AABB and a point inline AABB& operator+=(const Vec3s& p) { min_ = min_.cwiseMin(p); max_ = max_.cwiseMax(p); return *this; } /// @brief Merge the AABB and another AABB inline AABB& operator+=(const AABB& other) { min_ = min_.cwiseMin(other.min_); max_ = max_.cwiseMax(other.max_); return *this; } /// @brief Return the merged AABB of current AABB and the other one inline AABB operator+(const AABB& other) const { AABB res(*this); return res += other; } /// @brief Size of the AABB (used in BV_Splitter to order two AABBs) inline Scalar size() const { return (max_ - min_).squaredNorm(); } /// @brief Center of the AABB inline Vec3s center() const { return (min_ + max_) * 0.5; } /// @brief Width of the AABB inline Scalar width() const { return max_[0] - min_[0]; } /// @brief Height of the AABB inline Scalar height() const { return max_[1] - min_[1]; } /// @brief Depth of the AABB inline Scalar depth() const { return max_[2] - min_[2]; } /// @brief Volume of the AABB inline Scalar volume() const { return width() * height() * depth(); } /// @} /// @brief Check whether the AABB contains another AABB inline bool contain(const AABB& other) const { return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); } /// @brief Check whether two AABB are overlap and return the overlap part inline bool overlap(const AABB& other, AABB& overlap_part) const { if (!overlap(other)) { return false; } overlap_part.min_ = min_.cwiseMax(other.min_); overlap_part.max_ = max_.cwiseMin(other.max_); return true; } /// @brief Check whether two AABB are overlapped along specific axis inline bool axisOverlap(const AABB& other, int axis_id) const { if (min_[axis_id] > other.max_[axis_id]) return false; if (max_[axis_id] < other.min_[axis_id]) return false; return true; } /// @brief expand the half size of the AABB by delta, and keep the center /// unchanged. inline AABB& expand(const Vec3s& delta) { min_ -= delta; max_ += delta; return *this; } /// @brief expand the half size of the AABB by a scalar delta, and keep the /// center unchanged. inline AABB& expand(const Scalar delta) { min_.array() -= delta; max_.array() += delta; return *this; } /// @brief expand the aabb by increase the thickness of the plate by a ratio inline AABB& expand(const AABB& core, Scalar ratio) { min_ = min_ * ratio - core.min_; max_ = max_ * ratio - core.max_; return *this; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** @} */ // end of Bounding_Volume /// @brief translate the center of AABB by t static inline AABB translate(const AABB& aabb, const Vec3s& t) { AABB res(aabb); res.min_ += t; res.max_ += t; return res; } static inline AABB rotate(const AABB& aabb, const Matrix3s& R) { AABB res(R * aabb.min_); Vec3s corner(aabb.min_); const Eigen::DenseIndex bit[3] = {1, 2, 4}; for (Eigen::DenseIndex ic = 1; ic < 8; ++ic) { // ic = 0 corresponds to aabb.min_. Skip it. for (Eigen::DenseIndex i = 0; i < 3; ++i) { corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i]; } res += R * corner; } return res; } /// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) /// and b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2); /// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) /// and b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound); } // namespace coal #endif ================================================ FILE: include/coal/BV/BV.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_BV_H #define COAL_BV_H #include "coal/BV/kDOP.h" #include "coal/BV/AABB.h" #include "coal/BV/OBB.h" #include "coal/BV/RSS.h" #include "coal/BV/OBBRSS.h" #include "coal/BV/kIOS.h" #include "coal/math/transform.h" /** @brief Main namespace */ namespace coal { /// @cond IGNORE namespace details { /// @brief Convert a bounding volume of type BV1 in configuration tf1 to a /// bounding volume of type BV2 in I configuration. template struct Converter { static void convert(const BV1& bv1, const Transform3s& tf1, BV2& bv2); static void convert(const BV1& bv1, BV2& bv2); }; /// @brief Convert from AABB to AABB, not very tight but is fast. template <> struct Converter { static void convert(const AABB& bv1, const Transform3s& tf1, AABB& bv2) { const Vec3s& center = bv1.center(); Scalar r = (bv1.max_ - bv1.min_).norm() * Scalar(0.5); const Vec3s center2 = tf1.transform(center); bv2.min_ = center2 - Vec3s::Constant(r); bv2.max_ = center2 + Vec3s::Constant(r); } static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; } }; template <> struct Converter { static void convert(const AABB& bv1, const Transform3s& tf1, OBB& bv2) { bv2.To = tf1.transform(bv1.center()); bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; bv2.axes = tf1.getRotation(); } static void convert(const AABB& bv1, OBB& bv2) { bv2.To = bv1.center(); bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; bv2.axes.setIdentity(); } }; template <> struct Converter { static void convert(const OBB& bv1, const Transform3s& tf1, OBB& bv2) { bv2.extent = bv1.extent; bv2.To = tf1.transform(bv1.To); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; } static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; } }; template <> struct Converter { static void convert(const OBBRSS& bv1, const Transform3s& tf1, OBB& bv2) { Converter::convert(bv1.obb, tf1, bv2); } static void convert(const OBBRSS& bv1, OBB& bv2) { Converter::convert(bv1.obb, bv2); } }; template <> struct Converter { static void convert(const RSS& bv1, const Transform3s& tf1, OBB& bv2) { const Scalar half = Scalar(0.5); bv2.extent = Vec3s(bv1.length[0] * half + bv1.radius, bv1.length[1] * half + bv1.radius, bv1.radius); bv2.To = tf1.transform(bv1.Tr); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; } static void convert(const RSS& bv1, OBB& bv2) { const Scalar half = Scalar(0.5); bv2.extent = Vec3s(bv1.length[0] * half + bv1.radius, bv1.length[1] * half + bv1.radius, bv1.radius); bv2.To = bv1.Tr; bv2.axes = bv1.axes; } }; template struct Converter { static void convert(const BV1& bv1, const Transform3s& tf1, AABB& bv2) { const Vec3s& center = bv1.center(); const Scalar half = Scalar(0.5); Scalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * half; const Vec3s center2 = tf1.transform(center); bv2.min_ = center2 - Vec3s::Constant(r); bv2.max_ = center2 + Vec3s::Constant(r); } static void convert(const BV1& bv1, AABB& bv2) { const Vec3s& center = bv1.center(); const Scalar half = Scalar(0.5); Scalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * half; bv2.min_ = center - Vec3s::Constant(r); bv2.max_ = center + Vec3s::Constant(r); } }; template struct Converter { static void convert(const BV1& bv1, const Transform3s& tf1, OBB& bv2) { AABB bv; Converter::convert(bv1, bv); Converter::convert(bv, tf1, bv2); } static void convert(const BV1& bv1, OBB& bv2) { AABB bv; Converter::convert(bv1, bv); Converter::convert(bv, bv2); } }; template <> struct Converter { static void convert(const OBB& bv1, const Transform3s& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.To); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; bv2.radius = bv1.extent[2]; bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius); bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius); } static void convert(const OBB& bv1, RSS& bv2) { bv2.Tr = bv1.To; bv2.axes = bv1.axes; bv2.radius = bv1.extent[2]; bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius); bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius); } }; template <> struct Converter { static void convert(const RSS& bv1, const Transform3s& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.Tr); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; bv2.radius = bv1.radius; bv2.length[0] = bv1.length[0]; bv2.length[1] = bv1.length[1]; } static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; } }; template <> struct Converter { static void convert(const OBBRSS& bv1, const Transform3s& tf1, RSS& bv2) { Converter::convert(bv1.rss, tf1, bv2); } static void convert(const OBBRSS& bv1, RSS& bv2) { Converter::convert(bv1.rss, bv2); } }; template <> struct Converter { static void convert(const AABB& bv1, const Transform3s& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.center()); /// Sort the AABB edges so that AABB extents are ordered. Scalar d[3] = {bv1.width(), bv1.height(), bv1.depth()}; Eigen::DenseIndex id[3] = {0, 1, 2}; for (Eigen::DenseIndex i = 1; i < 3; ++i) { for (Eigen::DenseIndex j = i; j > 0; --j) { if (d[j] > d[j - 1]) { { Scalar tmp = d[j]; d[j] = d[j - 1]; d[j - 1] = tmp; } { Eigen::DenseIndex tmp = id[j]; id[j] = id[j - 1]; id[j - 1] = tmp; } } } } const Vec3s extent = (bv1.max_ - bv1.min_) * 0.5; bv2.radius = extent[id[2]]; bv2.length[0] = (extent[id[0]] - bv2.radius) * 2; bv2.length[1] = (extent[id[1]] - bv2.radius) * 2; const Matrix3s& R = tf1.getRotation(); const bool left_hand = (id[0] == (id[1] + 1) % 3); if (left_hand) bv2.axes.col(0) = -R.col(id[0]); else bv2.axes.col(0) = R.col(id[0]); bv2.axes.col(1) = R.col(id[1]); bv2.axes.col(2) = R.col(id[2]); } static void convert(const AABB& bv1, RSS& bv2) { convert(bv1, Transform3s(), bv2); } }; template <> struct Converter { static void convert(const AABB& bv1, const Transform3s& tf1, OBBRSS& bv2) { Converter::convert(bv1, tf1, bv2.obb); Converter::convert(bv1, tf1, bv2.rss); } static void convert(const AABB& bv1, OBBRSS& bv2) { Converter::convert(bv1, bv2.obb); Converter::convert(bv1, bv2.rss); } }; } // namespace details /// @endcond /// @brief Convert a bounding volume of type BV1 in configuration tf1 to /// bounding volume of type BV2 in identity configuration. template static inline void convertBV(const BV1& bv1, const Transform3s& tf1, BV2& bv2) { details::Converter::convert(bv1, tf1, bv2); } /// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2 /// in identity configuration. template static inline void convertBV(const BV1& bv1, BV2& bv2) { details::Converter::convert(bv1, bv2); } } // namespace coal #endif ================================================ FILE: include/coal/BV/BV_node.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_BV_NODE_H #define COAL_BV_NODE_H #include "coal/data_types.h" #include "coal/BV/BV.h" namespace coal { /// @defgroup Construction_Of_BVH Construction of BVHModel /// Classes which are used to build a BVHModel (Bounding Volume Hierarchy) /// @{ /// @brief BVNodeBase encodes the tree structure for BVH struct COAL_DLLAPI BVNodeBase { /// @brief An index for first child node or primitive /// If the value is positive, it is the index of the first child bv node /// If the value is negative, it is -(primitive index + 1) /// Zero is not used. int first_child; /// @brief The start id the primitive belonging to the current node. The index /// is referred to the primitive_indices in BVHModel and from that we can /// obtain the primitive's index in original data indirectly. unsigned int first_primitive; /// @brief The number of primitives belonging to the current node unsigned int num_primitives; /// @brief Default constructor BVNodeBase() : first_child(0), first_primitive( (std::numeric_limits::max)()) // value we should help // to raise an issue , num_primitives(0) {} /// @brief Equality operator bool operator==(const BVNodeBase& other) const { COAL_EQUAL_OPERATOR_CHECK(first_child == other.first_child); COAL_EQUAL_OPERATOR_CHECK(first_primitive == other.first_primitive); COAL_EQUAL_OPERATOR_CHECK(num_primitives == other.num_primitives); return true; } /// @brief Difference operator bool operator!=(const BVNodeBase& other) const { return !(*this == other); } /// @brief Whether current node is a leaf node (i.e. contains a primitive /// index inline bool isLeaf() const { return first_child < 0; } /// @brief Return the primitive index. The index is referred to the original /// data (i.e. vertices or tri_indices) in BVHModel inline int primitiveId() const { return -(first_child + 1); } /// @brief Return the index of the first child. The index is referred to the /// bounding volume array (i.e. bvs) in BVHModel inline int leftChild() const { return first_child; } /// @brief Return the index of the second child. The index is referred to the /// bounding volume array (i.e. bvs) in BVHModel inline int rightChild() const { return first_child + 1; } }; /// @brief A class describing a bounding volume node. It includes the tree /// structure providing in BVNodeBase and also the geometry data provided in BV /// template parameter. template struct COAL_DLLAPI BVNode : public BVNodeBase { typedef BVNodeBase Base; /// @brief bounding volume storing the geometry BV bv; /// @brief Equality operator bool operator==(const BVNode& other) const { COAL_EQUAL_OPERATOR_CHECK(Base::operator==(other)); COAL_EQUAL_OPERATOR_CHECK(bv == other.bv); return true; } /// @brief Difference operator bool operator!=(const BVNode& other) const { return !(*this == other); } /// @brief Check whether two BVNode collide bool overlap(const BVNode& other) const { return bv.overlap(other.bv); } /// @brief Check whether two BVNode collide bool overlap(const BVNode& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const { return bv.overlap(other.bv, request, sqrDistLowerBound); } /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and /// the underlying BV supports distance, return the nearest points. Scalar distance(const BVNode& other, Vec3s* P1 = NULL, Vec3s* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } /// @brief Access to the center of the BV Vec3s getCenter() const { return bv.center(); } /// @brief Access to the orientation of the BV const Matrix3s& getOrientation() const { static const Matrix3s id3 = Matrix3s::Identity(); return id3; } /// \cond EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// \endcond }; template <> inline const Matrix3s& BVNode::getOrientation() const { return bv.axes; } template <> inline const Matrix3s& BVNode::getOrientation() const { return bv.axes; } template <> inline const Matrix3s& BVNode::getOrientation() const { return bv.obb.axes; } } // namespace coal #endif ================================================ FILE: include/coal/BV/OBB.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_OBB_H #define COAL_OBB_H #include "coal/fwd.hh" #include "coal/data_types.h" namespace coal { struct CollisionRequest; /// @addtogroup Bounding_Volume /// @{ /// @brief Oriented bounding box class struct COAL_DLLAPI OBB { EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @brief Orientation of OBB. axis[i] is the ith column of the orientation /// matrix for the box; it is also the i-th principle direction of the box. We /// assume that axis[0] corresponds to the axis with the longest box edge, /// axis[1] corresponds to the shorter one and axis[2] corresponds to the /// shortest one. Matrix3s axes; /// @brief Center of OBB Vec3s To; /// @brief Half dimensions of OBB Vec3s extent; OBB() : axes(Matrix3s::Zero()), To(Vec3s::Zero()), extent(Vec3s::Zero()) {} /// @brief Equality operator bool operator==(const OBB& other) const { COAL_EQUAL_OPERATOR_CHECK(axes == other.axes); COAL_EQUAL_OPERATOR_CHECK(To == other.To); COAL_EQUAL_OPERATOR_CHECK(extent == other.extent); return true; } /// @brief Difference operator bool operator!=(const OBB& other) const { return !(*this == other); } /// @brief Check whether the OBB contains a point. bool contain(const Vec3s& p) const; /// Check collision between two OBB /// @return true if collision happens. bool overlap(const OBB& other) const; /// Check collision between two OBB /// @return true if collision happens. /// @retval sqrDistLowerBound squared lower bound on distance between boxes if /// they do not overlap. bool overlap(const OBB& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const; /// @brief Distance between two OBBs, not implemented. Scalar distance(const OBB& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; /// @brief A simple way to merge the OBB and a point (the result is not /// compact). OBB& operator+=(const Vec3s& p); /// @brief Merge the OBB and another OBB (the result is not compact). OBB& operator+=(const OBB& other) { *this = *this + other; return *this; } /// @brief Return the merged OBB of current OBB and the other one (the result /// is not compact). OBB operator+(const OBB& other) const; /// @brief Size of the OBB (used in BV_Splitter to order two OBBs) inline Scalar size() const { return extent.squaredNorm(); } /// @brief Center of the OBB inline const Vec3s& center() const { return To; } /// @brief Width of the OBB. inline Scalar width() const { return 2 * extent[0]; } /// @brief Height of the OBB. inline Scalar height() const { return 2 * extent[1]; } /// @brief Depth of the OBB inline Scalar depth() const { return 2 * extent[2]; } /// @brief Volume of the OBB inline Scalar volume() const { return width() * height() * depth(); } }; /** @} */ // end of Bounding_Volume /// @brief Translate the OBB bv COAL_DLLAPI OBB translate(const OBB& bv, const Vec3s& t); /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and /// b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2); /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and /// b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound); /// Check collision between two boxes /// @param B, T orientation and position of first box, /// @param a half dimensions of first box, /// @param b half dimensions of second box. /// The second box is in identity configuration. COAL_DLLAPI bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b); } // namespace coal #endif ================================================ FILE: include/coal/BV/OBBRSS.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_OBBRSS_H #define COAL_OBBRSS_H #include "coal/fwd.hh" #include "coal/BV/OBB.h" #include "coal/BV/RSS.h" namespace coal { struct CollisionRequest; /// @addtogroup Bounding_Volume /// @{ /// @brief Class merging the OBB and RSS, can handle collision and distance /// simultaneously struct COAL_DLLAPI OBBRSS { EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @brief OBB member, for rotation OBB obb; /// @brief RSS member, for distance RSS rss; /// @brief Equality operator bool operator==(const OBBRSS& other) const { COAL_EQUAL_OPERATOR_CHECK(obb == other.obb); COAL_EQUAL_OPERATOR_CHECK(rss == other.rss); return true; } /// @brief Difference operator bool operator!=(const OBBRSS& other) const { return !(*this == other); } /// @brief Check whether the OBBRSS contains a point inline bool contain(const Vec3s& p) const { return obb.contain(p); } /// @brief Check collision between two OBBRSS bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); } /// Check collision between two OBBRSS /// @retval sqrDistLowerBound squared lower bound on distance between /// objects if they do not overlap. bool overlap(const OBBRSS& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const { return obb.overlap(other.obb, request, sqrDistLowerBound); } /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the /// nearest points Scalar distance(const OBBRSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const { return rss.distance(other.rss, P, Q); } /// @brief Merge the OBBRSS and a point OBBRSS& operator+=(const Vec3s& p) { obb += p; rss += p; return *this; } /// @brief Merge two OBBRSS OBBRSS& operator+=(const OBBRSS& other) { *this = *this + other; return *this; } /// @brief Merge two OBBRSS OBBRSS operator+(const OBBRSS& other) const { OBBRSS result; result.obb = obb + other.obb; result.rss = rss + other.rss; return result; } /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS) inline Scalar size() const { return obb.size(); } /// @brief Center of the OBBRSS inline const Vec3s& center() const { return obb.center(); } /// @brief Width of the OBRSS inline Scalar width() const { return obb.width(); } /// @brief Height of the OBBRSS inline Scalar height() const { return obb.height(); } /// @brief Depth of the OBBRSS inline Scalar depth() const { return obb.depth(); } /// @brief Volume of the OBBRSS inline Scalar volume() const { return obb.volume(); } }; /** @} */ // end of Bounding_Volume /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) /// and b2 is in indentity inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1, const OBBRSS& b2) { return overlap(R0, T0, b1.obb, b2.obb); } /// Check collision between two OBBRSS /// @param R0, T0 configuration of b1 /// @param b1 first OBBRSS in configuration (R0, T0) /// @param b2 second OBBRSS in identity position /// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do /// not overlap. inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1, const OBBRSS& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound) { return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound); } /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) /// and b2 is in indentity; P and Q, is not NULL, returns the nearest points inline Scalar distance(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3s* P = NULL, Vec3s* Q = NULL) { return distance(R0, T0, b1.rss, b2.rss, P, Q); } } // namespace coal #endif ================================================ FILE: include/coal/BV/RSS.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_RSS_H #define COAL_RSS_H #include "coal/fwd.hh" #include "coal/data_types.h" #include namespace coal { struct CollisionRequest; /// @addtogroup Bounding_Volume /// @{ /// @brief A class for rectangle sphere-swept bounding volume struct COAL_DLLAPI RSS { EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @brief Orientation of RSS. axis[i] is the ith column of the orientation /// matrix for the RSS; it is also the i-th principle direction of the RSS. We /// assume that axis[0] corresponds to the axis with the longest length, /// axis[1] corresponds to the shorter one and axis[2] corresponds to the /// shortest one. Matrix3s axes; /// @brief Origin of the rectangle in RSS Vec3s Tr; /// @brief Side lengths of rectangle Scalar length[2]; /// @brief Radius of sphere summed with rectangle to form RSS Scalar radius; ///  @brief Default constructor with default values RSS() : axes(Matrix3s::Zero()), Tr(Vec3s::Zero()), radius(-1) { length[0] = 0; length[1] = 0; } /// @brief Equality operator bool operator==(const RSS& other) const { COAL_EQUAL_OPERATOR_CHECK(axes == other.axes); COAL_EQUAL_OPERATOR_CHECK(Tr == other.Tr); COAL_EQUAL_OPERATOR_CHECK(length[0] == other.length[0]); COAL_EQUAL_OPERATOR_CHECK(length[1] == other.length[1]); COAL_EQUAL_OPERATOR_CHECK(radius == other.radius); return true; } /// @brief Difference operator bool operator!=(const RSS& other) const { return !(*this == other); } /// @brief Check whether the RSS contains a point bool contain(const Vec3s& p) const; /// @brief Check collision between two RSS bool overlap(const RSS& other) const; /// Not implemented bool overlap(const RSS& other, const CollisionRequest&, Scalar& sqrDistLowerBound) const { sqrDistLowerBound = std::numeric_limits::quiet_NaN(); return overlap(other); } /// @brief the distance between two RSS; P and Q, if not NULL, return the /// nearest points Scalar distance(const RSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; /// @brief A simple way to merge the RSS and a point, not compact. /// @todo This function may have some bug. RSS& operator+=(const Vec3s& p); /// @brief Merge the RSS and another RSS inline RSS& operator+=(const RSS& other) { *this = *this + other; return *this; } /// @brief Return the merged RSS of current RSS and the other one RSS operator+(const RSS& other) const; /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) inline Scalar size() const { return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + 2 * radius); } /// @brief The RSS center inline const Vec3s& center() const { return Tr; } /// @brief Width of the RSS inline Scalar width() const { return length[0] + 2 * radius; } /// @brief Height of the RSS inline Scalar height() const { return length[1] + 2 * radius; } /// @brief Depth of the RSS inline Scalar depth() const { return 2 * radius; } /// @brief Volume of the RSS inline Scalar volume() const { return (length[0] * length[1] * 2 * radius + 4 * boost::math::constants::pi() * radius * radius * radius); } /// @brief Check collision between two RSS and return the overlap part. /// For RSS, we return nothing, as the overlap part of two RSSs usually is not /// a RSS. bool overlap(const RSS& other, RSS& /*overlap_part*/) const { return overlap(other); } }; /** @} */ // end of Bounding_Volume /// @brief distance between two RSS bounding volumes /// P and Q (optional return values) are the closest points in the rectangles, /// not the RSS. But the direction P - Q is the correct direction for cloest /// points Notice that P and Q are both in the local frame of the first RSS (not /// global frame and not even the local frame of object 1) COAL_DLLAPI Scalar distance(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2, Vec3s* P = NULL, Vec3s* Q = NULL); /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and /// b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2); /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and /// b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound); } // namespace coal #endif ================================================ FILE: include/coal/BV/kDOP.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_KDOP_H #define COAL_KDOP_H #include "coal/fwd.hh" #include "coal/data_types.h" namespace coal { struct CollisionRequest; /// @addtogroup Bounding_Volume /// @{ /// @brief KDOP class describes the KDOP collision structures. K is set as the /// template parameter, which should be 16, 18, or 24 /// The KDOP structure is defined by some pairs of parallel planes defined by /// some axes. /// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off /// some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 8 /// (0,-1,0) and (0,1,0) -> indices 1 and 9 /// (0,0,-1) and (0,0,1) -> indices 2 and 10 /// (-1,-1,0) and (1,1,0) -> indices 3 and 11 /// (-1,0,-1) and (1,0,1) -> indices 4 and 12 /// (0,-1,-1) and (0,1,1) -> indices 5 and 13 /// (-1,1,0) and (1,-1,0) -> indices 6 and 14 /// (-1,0,1) and (1,0,-1) -> indices 7 and 15 /// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off /// some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 9 /// (0,-1,0) and (0,1,0) -> indices 1 and 10 /// (0,0,-1) and (0,0,1) -> indices 2 and 11 /// (-1,-1,0) and (1,1,0) -> indices 3 and 12 /// (-1,0,-1) and (1,0,1) -> indices 4 and 13 /// (0,-1,-1) and (0,1,1) -> indices 5 and 14 /// (-1,1,0) and (1,-1,0) -> indices 6 and 15 /// (-1,0,1) and (1,0,-1) -> indices 7 and 16 /// (0,-1,1) and (0,1,-1) -> indices 8 and 17 /// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off /// some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 12 /// (0,-1,0) and (0,1,0) -> indices 1 and 13 /// (0,0,-1) and (0,0,1) -> indices 2 and 14 /// (-1,-1,0) and (1,1,0) -> indices 3 and 15 /// (-1,0,-1) and (1,0,1) -> indices 4 and 16 /// (0,-1,-1) and (0,1,1) -> indices 5 and 17 /// (-1,1,0) and (1,-1,0) -> indices 6 and 18 /// (-1,0,1) and (1,0,-1) -> indices 7 and 19 /// (0,-1,1) and (0,1,-1) -> indices 8 and 20 /// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21 /// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 /// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 template class COAL_DLLAPI KDOP { protected: /// @brief Origin's distances to N KDOP planes Eigen::Array dist_; public: /// @brief Creating kDOP containing nothing KDOP(); /// @brief Creating kDOP containing only one point KDOP(const Vec3s& v); /// @brief Creating kDOP containing two points KDOP(const Vec3s& a, const Vec3s& b); /// @brief Equality operator bool operator==(const KDOP& other) const { COAL_EQUAL_OPERATOR_CHECK((dist_ == other.dist_).all()); return true; } /// @brief Difference operator bool operator!=(const KDOP& other) const { return (dist_ != other.dist_).any(); } /// @brief Check whether two KDOPs overlap. bool overlap(const KDOP& other) const; /// @brief Check whether two KDOPs overlap. /// @return true if collision happens. /// @retval sqrDistLowerBound squared lower bound on distance between boxes if /// they do not overlap. bool overlap(const KDOP& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const; /// @brief The distance between two KDOP, Not implemented. Scalar distance(const KDOP& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; /// @brief Merge the point and the KDOP KDOP& operator+=(const Vec3s& p); /// @brief Merge two KDOPs KDOP& operator+=(const KDOP& other); /// @brief Create a KDOP by mergin two KDOPs KDOP operator+(const KDOP& other) const; /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) inline Scalar size() const { return width() * width() + height() * height() + depth() * depth(); } /// @brief The (AABB) center inline Vec3s center() const { return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2; } /// @brief The (AABB) width inline Scalar width() const { return dist_[N / 2] - dist_[0]; } /// @brief The (AABB) height inline Scalar height() const { return dist_[N / 2 + 1] - dist_[1]; } /// @brief The (AABB) depth inline Scalar depth() const { return dist_[N / 2 + 2] - dist_[2]; } /// @brief The (AABB) volume inline Scalar volume() const { return width() * height() * depth(); } inline Scalar dist(short i) const { return dist_[i]; } inline Scalar& dist(short i) { return dist_[i]; } //// @brief Check whether one point is inside the KDOP bool inside(const Vec3s& p) const; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** @} */ // end of Bounding_Volume template bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/) { COAL_THROW_PRETTY("not implemented", std::logic_error); } template bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/, const CollisionRequest& /*request*/, Scalar& /*sqrDistLowerBound*/) { COAL_THROW_PRETTY("not implemented", std::logic_error); } /// @brief translate the KDOP BV template COAL_DLLAPI KDOP translate(const KDOP& bv, const Vec3s& t); } // namespace coal #endif ================================================ FILE: include/coal/BV/kIOS.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_KIOS_H #define COAL_KIOS_H #include "coal/fwd.hh" #include "coal/BV/OBB.h" namespace coal { struct CollisionRequest; /// @addtogroup Bounding_Volume /// @{ /// @brief A class describing the kIOS collision structure, which is a set of /// spheres. class COAL_DLLAPI kIOS { /// @brief One sphere in kIOS struct COAL_DLLAPI kIOS_Sphere { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3s o; Scalar r; bool operator==(const kIOS_Sphere& other) const { COAL_EQUAL_OPERATOR_CHECK(o == other.o); COAL_EQUAL_OPERATOR_CHECK(r == other.r); return true; } bool operator!=(const kIOS_Sphere& other) const { return !(*this == other); } }; /// @brief generate one sphere enclosing two spheres static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1) { Vec3s d = s1.o - s0.o; Scalar dist2 = d.squaredNorm(); Scalar diff_r = s1.r - s0.r; /** The sphere with the larger radius encloses the other */ if (diff_r * diff_r >= dist2) { if (s1.r > s0.r) return s1; else return s0; } else /** spheres partially overlapping or disjoint */ { float dist = (float)std::sqrt(dist2); kIOS_Sphere s; s.r = dist + s0.r + s1.r; if (dist > 0) s.o = s0.o + d * ((s.r - s0.r) / dist); else s.o = s0.o; return s; } } public: /// @brief Equality operator bool operator==(const kIOS& other) const { COAL_EQUAL_OPERATOR_CHECK(obb == other.obb); COAL_EQUAL_OPERATOR_CHECK(num_spheres == other.num_spheres); for (size_t k = 0; k < num_spheres; ++k) { COAL_EQUAL_OPERATOR_CHECK(spheres[k] == other.spheres[k]); } return true; } /// @brief Difference operator bool operator!=(const kIOS& other) const { return !(*this == other); } static constexpr size_t max_num_spheres = 5; /// @brief The (at most) five spheres for intersection kIOS_Sphere spheres[max_num_spheres]; /// @brief The number of spheres, no larger than 5 unsigned int num_spheres; /// @ OBB related with kIOS OBB obb; /// @brief Check whether the kIOS contains a point bool contain(const Vec3s& p) const; /// @brief Check collision between two kIOS bool overlap(const kIOS& other) const; /// @brief Check collision between two kIOS bool overlap(const kIOS& other, const CollisionRequest&, Scalar& sqrDistLowerBound) const; /// @brief The distance between two kIOS Scalar distance(const kIOS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; /// @brief A simple way to merge the kIOS and a point kIOS& operator+=(const Vec3s& p); /// @brief Merge the kIOS and another kIOS kIOS& operator+=(const kIOS& other) { *this = *this + other; return *this; } /// @brief Return the merged kIOS of current kIOS and the other one kIOS operator+(const kIOS& other) const; /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) Scalar size() const; /// @brief Center of the kIOS const Vec3s& center() const { return spheres[0].o; } /// @brief Width of the kIOS Scalar width() const; /// @brief Height of the kIOS Scalar height() const; /// @brief Depth of the kIOS Scalar depth() const; /// @brief Volume of the kIOS Scalar volume() const; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** @} */ // end of Bounding_Volume /// @brief Translate the kIOS BV COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3s& t); /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) /// and b2 is in identity. /// @todo Not efficient COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2); /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) /// and b2 is in identity. /// @todo Not efficient COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound); /// @brief Approximate distance between two kIOS bounding volumes /// @todo P and Q is not returned, need implementation COAL_DLLAPI Scalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2, Vec3s* P = NULL, Vec3s* Q = NULL); } // namespace coal #endif ================================================ FILE: include/coal/BVH/BVH_front.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_BVH_FRONT_H #define COAL_BVH_FRONT_H #include #include "coal/config.hh" namespace coal { /// @brief Front list acceleration for collision /// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where /// the traversal terminates while performing a query during a given time /// instance. The front list reflects the subset of a BVTT that is traversed for /// that particular proximity query. struct COAL_DLLAPI BVHFrontNode { /// @brief The nodes to start in the future, i.e. the wave front of the /// traversal tree. unsigned int left, right; /// @brief The front node is not valid when collision is detected on the front /// node. bool valid; BVHFrontNode(unsigned int left_, unsigned int right_) : left(left_), right(right_), valid(true) {} }; /// @brief BVH front list is a list of front nodes. typedef std::list BVHFrontList; /// @brief Add new front node into the front list inline void updateFrontList(BVHFrontList* front_list, unsigned int b1, unsigned int b2) { if (front_list) front_list->push_back(BVHFrontNode(b1, b2)); } } // namespace coal #endif ================================================ FILE: include/coal/BVH/BVH_internal.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_BVH_INTERNAL_H #define COAL_BVH_INTERNAL_H #include "coal/data_types.h" namespace coal { /// @brief States for BVH construction /// empty->begun->processed ->replace_begun->processed -> ...... /// | /// |-> update_begun -> updated -> ..... enum BVHBuildState { BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry /// primitives BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for /// updating geometry primitives BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, /// ready for ccd use BVH_BUILD_STATE_REPLACE_BEGUN /// after beginReplaceModel(), state for /// replacing geometry primitives }; /// @brief Error code for BVH enum BVHReturnCode { BVH_OK = 0, /// BVH is valid BVH_ERR_MODEL_OUT_OF_MEMORY = -1, /// Cannot allocate memory for vertices and triangles BVH_ERR_BUILD_OUT_OF_SEQUENCE = -2, /// BVH construction does not follow correct sequence BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -4, /// BVH geometry in previous frame is not prepared BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid BVH_ERR_UNKNOWN = -8 /// Unknown failure }; /// @brief BVH model type enum BVHModelType { BVH_MODEL_UNKNOWN, /// @brief unknown model type BVH_MODEL_TRIANGLES, /// @brief triangle model BVH_MODEL_POINTCLOUD /// @brief point cloud model }; } // namespace coal #endif ================================================ FILE: include/coal/BVH/BVH_model.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2020-2022, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_BVH_MODEL_H #define COAL_BVH_MODEL_H #include "coal/fwd.hh" #include "coal/collision_object.h" #include "coal/shared_ptr_comparison.h" #include "coal/BVH/BVH_internal.h" #include "coal/BV/BV_node.h" #include #include #include namespace coal { /// @addtogroup Construction_Of_BVH /// @{ template class ConvexBaseTpl; template class BVFitter; template class BVSplitter; /// @brief A base class describing the bounding hierarchy of a mesh model or a /// point cloud model (which is viewed as a degraded version of mesh) class COAL_DLLAPI BVHModelBase : public CollisionGeometry { public: typedef typename Triangle32::IndexType IndexType; typedef ConvexBaseTpl ConvexType; /// @brief Geometry point data std::shared_ptr> vertices; /// @brief Geometry triangle index data, will be NULL for point clouds std::shared_ptr> tri_indices; /// @brief Geometry point data in previous frame std::shared_ptr> prev_vertices; /// @brief Number of triangles unsigned int num_tris; /// @brief Number of points unsigned int num_vertices; /// @brief The state of BVH building process BVHBuildState build_state; /// @brief Convex representation of this object // TODO: deprecated shared_ptr convex; /// @brief Model type described by the instance BVHModelType getModelType() const { if (num_tris && num_vertices) return BVH_MODEL_TRIANGLES; else if (num_vertices) return BVH_MODEL_POINTCLOUD; else return BVH_MODEL_UNKNOWN; } /// @brief Constructing an empty BVH BVHModelBase(); /// @brief copy from another BVH BVHModelBase(const BVHModelBase& other); /// @brief deconstruction, delete mesh data related. virtual ~BVHModelBase() {} /// @brief Get the object type: it is a BVH OBJECT_TYPE getObjectType() const override { return OT_BVH; } /// @brief Compute the AABB for the BVH, used for broad-phase collision void computeLocalAABB() override; /// @brief Begin a new BVH model int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0); /// @brief Add one point in the new BVH model int addVertex(const Vec3s& p); /// @brief Add points in the new BVH model int addVertices(const MatrixX3s& points); /// @brief Add triangles in the new BVH model int addTriangles(const Matrixx3i& triangles); /// @brief Add one triangle in the new BVH model int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); /// @brief Add a set of triangles in the new BVH model int addSubModel(const std::vector& ps, const std::vector& ts); /// @brief Add a set of points in the new BVH model int addSubModel(const std::vector& ps); /// @brief End BVH model construction, will build the bounding volume /// hierarchy int endModel(); /// @brief Replace the geometry information of current frame (i.e. should have /// the same mesh topology with the previous frame) int beginReplaceModel(); /// @brief Replace one point in the old BVH model int replaceVertex(const Vec3s& p); /// @brief Replace one triangle in the old BVH model int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); /// @brief Replace a set of points in the old BVH model int replaceSubModel(const std::vector& ps); /// @brief End BVH model replacement, will also refit or rebuild the bounding /// volume hierarchy int endReplaceModel(bool refit = true, bool bottomup = true); /// @brief Replace the geometry information of current frame (i.e. should have /// the same mesh topology with the previous frame). The current frame will be /// saved as the previous frame in prev_vertices. int beginUpdateModel(); /// @brief Update one point in the old BVH model int updateVertex(const Vec3s& p); /// @brief Update one triangle in the old BVH model int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); /// @brief Update a set of points in the old BVH model int updateSubModel(const std::vector& ps); /// @brief End BVH model update, will also refit or rebuild the bounding /// volume hierarchy int endUpdateModel(bool refit = true, bool bottomup = true); /// @brief Build this \ref Convex "Convex" representation of this /// model. The result is stored in attribute \ref convex. \note this only /// takes the points of this model. It does not check that the /// object is convex. It does not compute a convex hull. void buildConvexRepresentation(bool share_memory); /// @brief Build a convex hull /// and store it in attribute \ref convex. /// \param keepTriangle whether the convex should be triangulated. /// \param qhullCommand see \ref ConvexBase::convexHull. /// \return \c true if this object is convex, hence the convex hull represents /// the same object. /// \sa ConvexBase::convexHull /// \warning At the moment, the return value only checks whether there are as /// many points in the convex hull as in the original object. This is /// neither necessary (duplicated vertices get merged) nor sufficient /// (think of a U with 4 vertices and 3 edges). bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL); virtual int memUsage(const bool msg = false) const = 0; /// @brief This is a special acceleration: BVH_model default stores the BV's /// transform in world coordinate. However, we can also store each BV's /// transform related to its parent BV node. When traversing the BVH, this can /// save one matrix transformation. virtual void makeParentRelative() = 0; Vec3s computeCOM() const override { Scalar vol = 0; Vec3s com(0, 0, 0); if (!(vertices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " "vertices." << std::endl; return com; } const std::vector& vertices_ = *vertices; if (!(tri_indices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " "triangles." << std::endl; return com; } const std::vector& tri_indices_ = *tri_indices; for (unsigned int i = 0; i < num_tris; ++i) { const Triangle32& tri = tri_indices_[i]; Scalar d_six_vol = (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); vol += d_six_vol; com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) * d_six_vol; } return com / (vol * 4); } Scalar computeVolume() const override { Scalar vol = 0; if (!(vertices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " "vertices." << std::endl; return vol; } const std::vector& vertices_ = *vertices; if (!(tri_indices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " "triangles." << std::endl; return vol; } const std::vector& tri_indices_ = *tri_indices; for (unsigned int i = 0; i < num_tris; ++i) { const Triangle32& tri = tri_indices_[i]; Scalar d_six_vol = (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); vol += d_six_vol; } return vol / 6; } Matrix3s computeMomentofInertia() const override { Matrix3s C = Matrix3s::Zero(); Matrix3s C_canonical; C_canonical << Scalar(1 / 60.0), // Scalar(1 / 120.0), // Scalar(1 / 120.0), // Scalar(1 / 120.0), // Scalar(1 / 60.0), // Scalar(1 / 120.0), // Scalar(1 / 120.0), // Scalar(1 / 120.0), // Scalar(1 / 60.0); if (!(vertices.get())) { std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " "not contain vertices." << std::endl; return C; } const std::vector& vertices_ = *vertices; if (!(vertices.get())) { std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " "not contain vertices." << std::endl; return C; } const std::vector& tri_indices_ = *tri_indices; for (unsigned int i = 0; i < num_tris; ++i) { const Triangle32& tri = tri_indices_[i]; const Vec3s& v1 = vertices_[tri[0]]; const Vec3s& v2 = vertices_[tri[1]]; const Vec3s& v3 = vertices_[tri[2]]; Matrix3s A; A << v1.transpose(), v2.transpose(), v3.transpose(); C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); } return C.trace() * Matrix3s::Identity() - C; } protected: virtual void deleteBVs() = 0; virtual bool allocateBVs() = 0; /// @brief Build the bounding volume hierarchy virtual int buildTree() = 0; /// @brief Refit the bounding volume hierarchy virtual int refitTree(bool bottomup) = 0; unsigned int num_tris_allocated; unsigned int num_vertices_allocated; unsigned int num_vertex_updated; /// for ccd vertex update protected: /// \brief Comparison operators virtual bool isEqual(const CollisionGeometry& other) const override; }; /// @brief A class describing the bounding hierarchy of a mesh model or a point /// cloud model (which is viewed as a degraded version of mesh) \tparam BV one /// of the bounding volume class in \ref Bounding_Volume. template class COAL_DLLAPI BVHModel : public BVHModelBase { typedef BVHModelBase Base; public: using bv_node_vector_t = std::vector, Eigen::aligned_allocator>>; /// @brief Split rule to split one BV node into two children shared_ptr> bv_splitter; /// @brief Fitting rule to fit a BV node to a set of geometry primitives shared_ptr> bv_fitter; /// @brief Default constructor to build an empty BVH BVHModel(); /// @brief Copy constructor from another BVH /// /// \param[in] other BVHModel to copy. /// BVHModel(const BVHModel& other); /// @brief Clone *this into a new BVHModel virtual BVHModel* clone() const override { return new BVHModel(*this); } /// @brief deconstruction, delete mesh data related. ~BVHModel() {} /// @brief We provide getBV() and getNumBVs() because BVH may be compressed /// (in future), so we must provide some flexibility here /// @brief Access the bv giving the its index const BVNode& getBV(unsigned int i) const { assert(i < num_bvs); return (*bvs)[i]; } /// @brief Access the bv giving the its index BVNode& getBV(unsigned int i) { assert(i < num_bvs); return (*bvs)[i]; } /// @brief Get the number of bv in the BVH unsigned int getNumBVs() const { return num_bvs; } /// @brief Get the BV type: default is unknown NODE_TYPE getNodeType() const override { return BV_UNKNOWN; } /// @brief Check the number of memory used int memUsage(const bool msg) const override; /// @brief This is a special acceleration: BVH_model default stores the BV's /// transform in world coordinate. However, we can also store each BV's /// transform related to its parent BV node. When traversing the BVH, this can /// save one matrix transformation. void makeParentRelative() override { Matrix3s I(Matrix3s::Identity()); makeParentRelativeRecurse(0, I, Vec3s::Zero()); } protected: void deleteBVs() override; bool allocateBVs() override; unsigned int num_bvs_allocated; std::shared_ptr> primitive_indices; /// @brief Bounding volume hierarchy std::shared_ptr bvs; /// @brief Number of BV nodes in bounding volume hierarchy unsigned int num_bvs; /// @brief Build the bounding volume hierarchy int buildTree() override; /// @brief Refit the bounding volume hierarchy int refitTree(bool bottomup) override; /// @brief Refit the bounding volume hierarchy in a top-down way (slow but /// more compact) int refitTree_topdown(); /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but /// less compact) int refitTree_bottomup(); /// @brief Recursive kernel for hierarchy construction int recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives); /// @brief Recursive kernel for bottomup refitting int recursiveRefitTree_bottomup(int bv_id); /// @ recursively compute each bv's transform related to its parent. For /// default BV, only the translation works. For oriented BV (OBB, RSS, /// OBBRSS), special implementation is provided. void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; if (!bvs_[static_cast(bv_id)].isLeaf()) { makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child, parent_axes, bvs_[static_cast(bv_id)].getCenter()); makeParentRelativeRecurse( bvs_[static_cast(bv_id)].first_child + 1, parent_axes, bvs_[static_cast(bv_id)].getCenter()); } bvs_[static_cast(bv_id)].bv = translate(bvs_[static_cast(bv_id)].bv, -parent_c); } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const BVHModel* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const BVHModel& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(Base::isEqual(_other)); COAL_EQUAL_OPERATOR_CHECK(shared_ptrs_are_equal(bvs, other.bvs)); return true; } }; /// @} template <> void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, const Vec3s& parent_c); template <> void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, const Vec3s& parent_c); template <> void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, const Vec3s& parent_c); /// @brief Specialization of getNodeType() for BVHModel with different BV types template <> NODE_TYPE BVHModel::getNodeType() const; template <> NODE_TYPE BVHModel::getNodeType() const; template <> NODE_TYPE BVHModel::getNodeType() const; template <> NODE_TYPE BVHModel::getNodeType() const; template <> NODE_TYPE BVHModel::getNodeType() const; template <> NODE_TYPE BVHModel>::getNodeType() const; template <> NODE_TYPE BVHModel>::getNodeType() const; template <> NODE_TYPE BVHModel>::getNodeType() const; } // namespace coal #endif ================================================ FILE: include/coal/BVH/BVH_utility.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_BVH_UTILITY_H #define COAL_BVH_UTILITY_H #include "coal/BVH/BVH_model.h" namespace coal { /// @brief Extract the part of the BVHModel that is inside an AABB. /// A triangle in collision with the AABB is considered inside. template COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, const Transform3s& pose, const AABB& aabb); /// @brief Compute the covariance matrix for a set or subset of points. if ts = /// null, then indices refer to points directly; otherwise refer to triangles COAL_DLLAPI void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, Matrix3s& M); /// @brief Compute the RSS bounding volume parameters: radius, rectangle size /// and the origin, given the BV axises. COAL_DLLAPI void getRadiusAndOriginAndRectangleSize( Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, const Matrix3s& axes, Vec3s& origin, Scalar l[2], Scalar& r); /// @brief Compute the bounding volume extent and center for a set or subset of /// points, given the BV axises. COAL_DLLAPI void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, Matrix3s& axes, Vec3s& center, Vec3s& extent); /// @brief Compute the center and radius for a triangle's circumcircle COAL_DLLAPI void circumCircleComputation(const Vec3s& a, const Vec3s& b, const Vec3s& c, Vec3s& center, Scalar& radius); /// @brief Compute the maximum distance from a given center point to a point /// cloud COAL_DLLAPI Scalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, const Vec3s& query); } // namespace coal #endif ================================================ FILE: include/coal/alloca.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2026, Inria * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** This file is heavily inspired by Pinocchio */ #ifndef COAL_ALLOCA_H #define COAL_ALLOCA_H #include "coal/logging.h" #include #include #ifdef WIN32 #include #else #include #endif /// @brief Maximum number of bytes that `COAL_MAKE_ALLOCA_BOOST_SPAN` will /// allocate on the stack via `alloca`. Requests that exceed this threshold fall /// back to a heap allocation managed by a `std::unique_ptr`. #define COAL_ALLOCA_MAX_STACK_BYTES 65536 #define COAL_ALLOCA EIGEN_ALLOCA #define COAL_ALLOCA_TYPED_PTR(Type, Size) \ reinterpret_cast(COAL_ALLOCA(sizeof(Type) * (Size))) /// @brief Macro to create a pointer of type Type, with Size * sizeof(Type) /// allocated elements, refered to by variable Name. /// /// If `Size * sizeof(Type) <= COAL_ALLOCA_MAX_STACK_BYTES`, the buffer is /// allocated on the stack via `alloca` (zero overhead, freed on function /// return). Otherwise a heap buffer is allocated and owned by a /// `std::unique_ptr` that is freed when it goes out of scope. #define COAL_MAKE_ALLOCA_TYPED_PTR(Type, Name, Size) \ const std::size_t _sz_##Name = static_cast(Size); \ const std::size_t _bytes_##Name = _sz_##Name * sizeof(Type); \ std::unique_ptr> _heap_buf_##Name; \ Type* ptr_##Name; \ if (_bytes_##Name <= COAL_ALLOCA_MAX_STACK_BYTES) { \ ptr_##Name = COAL_ALLOCA_TYPED_PTR(Type, _sz_##Name); \ } else { \ COAL_LOG_WARNING( \ "Exceeded COAL_ALLOCA_MAX_STACK_BYTES in " \ "COAL_MAKE_ALLOCA_TYPED_PTR. Switching to heap allocation."); \ _heap_buf_##Name.reset(new std::vector(_sz_##Name)); \ ptr_##Name = _heap_buf_##Name.get()->data(); \ } #define COAL_MAKE_ALLOCA_BOOST_SPAN(Type, Name, Size) \ COAL_MAKE_ALLOCA_TYPED_PTR(Type, Name, Size); \ boost::span Name(ptr_##Name, _sz_##Name); #endif // ifndef COAL_ALLOCA_H ================================================ FILE: include/coal/broadphase/broadphase.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2022, Inria * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef COAL_BROADPHASE_BROADPHASE_H #define COAL_BROADPHASE_BROADPHASE_H #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #include "coal/broadphase/broadphase_bruteforce.h" #include "coal/broadphase/broadphase_SaP.h" #include "coal/broadphase/broadphase_SSaP.h" #include "coal/broadphase/broadphase_interval_tree.h" #include "coal/broadphase/broadphase_spatialhash.h" #include "coal/broadphase/default_broadphase_callbacks.h" #endif // ifndef COAL_BROADPHASE_BROADPHASE_H ================================================ FILE: include/coal/broadphase/broadphase_SSaP.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROAD_PHASE_SSAP_H #define COAL_BROAD_PHASE_SSAP_H #include #include "coal/broadphase/broadphase_collision_manager.h" namespace coal { /// @brief Simple SAP collision manager class COAL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; SSaPCollisionManager(); /// @brief remove one object from the manager void registerObject(CollisionObject* obj); /// @brief add one object to the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager virtual void update(); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects /// belonging to the manager void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief perform distance computation between one object and all the objects /// belonging to the manager void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; /// @brief perform collision test for the objects belonging to the manager /// (i.e., N^2 self collision) void collide(CollisionCallBackBase* callback) const; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager size_t size() const; protected: /// @brief check collision between one object and a list of objects, return /// value is whether stop is possible bool checkColl( typename std::vector::const_iterator pos_start, typename std::vector::const_iterator pos_end, CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief check distance between one object and a list of objects, return /// value is whether stop is possible bool checkDis( typename std::vector::const_iterator pos_start, typename std::vector::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const; bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const; static int selectOptimalAxis( const std::vector& objs_x, const std::vector& objs_y, const std::vector& objs_z, typename std::vector::const_iterator& it_beg, typename std::vector::const_iterator& it_end); /// @brief Objects sorted according to lower x value std::vector objs_x; /// @brief Objects sorted according to lower y value std::vector objs_y; /// @brief Objects sorted according to lower z value std::vector objs_z; /// @brief tag about whether the environment is maintained suitably (i.e., the /// objs_x, objs_y, objs_z are sorted correctly bool setup_; }; } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_SaP.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROAD_PHASE_SAP_H #define COAL_BROAD_PHASE_SAP_H #include #include #include "coal/broadphase/broadphase_collision_manager.h" namespace coal { /// @brief Rigorous SAP collision manager class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; SaPCollisionManager(); ~SaPCollisionManager(); /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); /// @brief remove one object from the manager void registerObject(CollisionObject* obj); /// @brief add one object to the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager virtual void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects /// belonging to the manager void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief perform distance computation between one object and all the objects /// belonging to the manager void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; /// @brief perform collision test for the objects belonging to the manager /// (i.e., N^2 self collision) void collide(CollisionCallBackBase* callback) const; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager size_t size() const; protected: struct EndPoint; /// @brief SAP interval for one object struct SaPAABB { /// @brief object CollisionObject* obj; /// @brief lower bound end point of the interval EndPoint* lo; /// @brief higher bound end point of the interval EndPoint* hi; /// @brief cached AABB value AABB cached; }; /// @brief End point for an interval struct EndPoint { /// @brief tag for whether it is a lower bound or higher bound of an /// interval, 0 for lo, and 1 for hi char minmax; /// @brief back pointer to SAP interval SaPAABB* aabb; /// @brief the previous end point in the end point list EndPoint* prev[3]; /// @brief the next end point in the end point list EndPoint* next[3]; /// @brief get the value of the end point const Vec3s& getVal() const; /// @brief set the value of the end point Vec3s& getVal(); Scalar getVal(int i) const; Scalar& getVal(int i); }; /// @brief A pair of objects that are not culling away and should further /// check collision struct SaPPair { SaPPair(CollisionObject* a, CollisionObject* b); CollisionObject* obj1; CollisionObject* obj2; bool operator==(const SaPPair& other) const; }; /// @brief Functor to help unregister one object class COAL_DLLAPI isUnregistered { CollisionObject* obj; public: isUnregistered(CollisionObject* obj_); bool operator()(const SaPPair& pair) const; }; /// @brief Functor to help remove collision pairs no longer valid (i.e., /// should be culled away) class COAL_DLLAPI isNotValidPair { CollisionObject* obj1; CollisionObject* obj2; public: isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_); bool operator()(const SaPPair& pair); }; void update_(SaPAABB* updated_aabb); void updateVelist(); /// @brief End point list for x, y, z coordinates EndPoint* elist[3]; /// @brief vector version of elist, for acceleration std::vector velist[3]; /// @brief SAP interval list std::list AABB_arr; /// @brief The pair of objects that should further check for collision std::list overlap_pairs; int optimal_axis; std::map obj_aabb_map; bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const; bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; void addToOverlapPairs(const SaPPair& p); void removeFromOverlapPairs(const SaPPair& p); }; } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_bruteforce.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROAD_PHASE_BRUTE_FORCE_H #define COAL_BROAD_PHASE_BRUTE_FORCE_H #include #include "coal/broadphase/broadphase_collision_manager.h" namespace coal { /// @brief Brute force N-body collision manager class COAL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; NaiveCollisionManager(); /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); /// @brief add one object to the manager void registerObject(CollisionObject* obj); /// @brief remove one object from the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager virtual void update(); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects /// belonging to the manager void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief perform distance computation between one object and all the objects /// belonging to the manager void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; /// @brief perform collision test for the objects belonging to the manager /// (i.e., N^2 self collision) void collide(CollisionCallBackBase* callback) const; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager size_t size() const; protected: /// @brief objects belonging to the manager are stored in a list structure std::list objs; }; } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_callbacks.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2022, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Justin Carpentier (justin.carpentier@inria.fr) */ #ifndef COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H #define COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H #include "coal/fwd.hh" #include "coal/data_types.h" namespace coal { /// @brief Base callback class for collision queries. /// This class can be supersed by child classes to provide desired behaviors /// according to the application (e.g, only listing the potential /// CollisionObjects in collision). struct COAL_DLLAPI CollisionCallBackBase { /// @brief Initialization of the callback before running the collision /// broadphase manager. virtual void init() {}; /// @brief Collision evaluation between two objects in collision. /// This callback will cause the broadphase evaluation to stop if it /// returns true. /// /// @param[in] o1 Collision object #1. /// @param[in] o2 Collision object #2. virtual bool collide(CollisionObject* o1, CollisionObject* o2) = 0; /// @brief Functor call associated to the collide operation. virtual bool operator()(CollisionObject* o1, CollisionObject* o2) { return collide(o1, o2); } }; /// @brief Base callback class for distance queries. /// This class can be supersed by child classes to provide desired behaviors /// according to the application (e.g, only listing the potential /// CollisionObjects in collision). struct COAL_DLLAPI DistanceCallBackBase { /// @brief Initialization of the callback before running the collision /// broadphase manager. virtual void init() {}; /// @brief Distance evaluation between two objects in collision. /// This callback will cause the broadphase evaluation to stop if it /// returns true. /// /// @param[in] o1 Collision object #1. /// @param[in] o2 Collision object #2. /// @param[out] dist Distance between the two collision geometries. virtual bool distance(CollisionObject* o1, CollisionObject* o2, Scalar& dist) = 0; /// @brief Functor call associated to the distance operation. virtual bool operator()(CollisionObject* o1, CollisionObject* o2, Scalar& dist) { return distance(o1, o2, dist); } }; } // namespace coal #endif // COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H ================================================ FILE: include/coal/broadphase/broadphase_collision_manager.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H #define COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H #include #include #include #include "coal/collision_object.h" #include "coal/broadphase/broadphase_callbacks.h" namespace coal { using CollisionCallBackFunctor = std::function; using DistanceCallBackFunctor = std::function; /// @brief Base class for broad phase collision. It helps to accelerate the /// collision/distance between N objects. Also support self collision, self /// distance and collision/distance with another M objects. class COAL_DLLAPI BroadPhaseCollisionManager { public: BroadPhaseCollisionManager(); virtual ~BroadPhaseCollisionManager(); /// @brief add objects to the manager virtual void registerObjects(const std::vector& other_objs); /// @brief add one object to the manager virtual void registerObject(CollisionObject* obj) = 0; /// @brief remove one object from the manager virtual void unregisterObject(CollisionObject* obj) = 0; /// @brief initialize the manager, related with the specific type of manager virtual void setup() = 0; /// @brief update the condition of manager virtual void update() = 0; /// @brief update the manager by explicitly given the object updated virtual void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update virtual void update(const std::vector& updated_objs); /// @brief clear the manager virtual void clear() = 0; /// @brief return the objects managed by the manager virtual void getObjects(std::vector& objs) const = 0; /// @brief return the objects managed by the manager virtual std::vector getObjects() const { std::vector res(size()); getObjects(res); return res; }; /// @brief perform collision test between one object and all the objects /// belonging to the manager virtual void collide(CollisionObject* obj, CollisionCallBackBase* callback) const = 0; /// @copybrief collide(CollisionObject*, CollisionCallBackBase*) void collide(CollisionObject* obj, const CollisionCallBackFunctor& fn) const; /// @brief perform distance computation between one object and all the objects /// belonging to the manager virtual void distance(CollisionObject* obj, DistanceCallBackBase* callback) const = 0; /// @copybrief distance(CollisionObject*, DistanceCallBackBase*) void distance(CollisionObject* obj, const DistanceCallBackFunctor& fn) const; /// @brief perform collision test for the objects belonging to the manager /// (i.e., N^2 self collision) virtual void collide(CollisionCallBackBase* callback) const = 0; /// @copybrief collide(CollisionCallBackBase*) void collide(const CollisionCallBackFunctor& fn) const; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) virtual void distance(DistanceCallBackBase* callback) const = 0; /// @copybrief distance(DistanceCallBackBase*) void distance(const DistanceCallBackFunctor& fn) const; /// @brief perform collision test with objects belonging to another manager virtual void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase* callback) const = 0; /// @copybrief collide(BroadPhaseCollisionManager*, CollisionCallBackBase*) void collide(BroadPhaseCollisionManager* other_manager, const CollisionCallBackFunctor& fn) const; /// @brief perform distance test with objects belonging to another manager virtual void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase* callback) const = 0; /// @copybrief distance(BroadPhaseCollisionManager*, DistanceCallBackBase*) void distance(BroadPhaseCollisionManager* other_manager, const DistanceCallBackFunctor& fn) const; /// @brief whether the manager is empty virtual bool empty() const = 0; /// @brief the number of objects managed by the manager virtual size_t size() const = 0; protected: /// @brief tools help to avoid repeating collision or distance callback for /// the pairs of objects tested before. It can be useful for some of the /// broadphase algorithms. mutable std::set > tested_set; mutable bool enable_tested_set_; bool inTestedSet(CollisionObject* a, CollisionObject* b) const; void insertTestedSet(CollisionObject* a, CollisionObject* b) const; }; } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_continuous_collision_manager-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H #define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H #include "coal/broadphase/broadphase_continuous_collision_manager.h" #include namespace coal { //============================================================================== BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() { // Do nothing } //============================================================================== BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() { // Do nothing } //============================================================================== void BroadPhaseContinuousCollisionManager::registerObjects( const std::vector& other_objs) { for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); } //============================================================================== void BroadPhaseContinuousCollisionManager::update( ContinuousCollisionObject* updated_obj) { COAL_UNUSED_VARIABLE(updated_obj); update(); } //============================================================================== void BroadPhaseContinuousCollisionManager::update( const std::vector& updated_objs) { COAL_UNUSED_VARIABLE(updated_objs); update(); } } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_continuous_collision_manager.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H #define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H #include "coal/broadphase/broadphase_collision_manager.h" #include "coal/collision_object.h" #include "coal/narrowphase/continuous_collision_object.h" namespace coal { /// @brief Callback for continuous collision between two objects. Return value /// is whether can stop now. template using ContinuousCollisionCallBack = bool (*)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata); /// @brief Callback for continuous distance between two objects, Return value is /// whether can stop now, also return the minimum distance till now. template using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, S& dist); /// @brief Base class for broad phase continuous collision. It helps to /// accelerate the continuous collision/distance between N objects. Also support /// self collision, self distance and collision/distance with another M objects. template class COAL_DLLAPI BroadPhaseContinuousCollisionManager { public: BroadPhaseContinuousCollisionManager(); virtual ~BroadPhaseContinuousCollisionManager(); /// @brief add objects to the manager virtual void registerObjects( const std::vector& other_objs); /// @brief add one object to the manager virtual void registerObject(ContinuousCollisionObject* obj) = 0; /// @brief remove one object from the manager virtual void unregisterObject(ContinuousCollisionObject* obj) = 0; /// @brief initialize the manager, related with the specific type of manager virtual void setup() = 0; /// @brief update the condition of manager virtual void update() = 0; /// @brief update the manager by explicitly given the object updated virtual void update(ContinuousCollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update virtual void update( const std::vector& updated_objs); /// @brief clear the manager virtual void clear() = 0; /// @brief return the objects managed by the manager virtual void getObjects( std::vector& objs) const = 0; /// @brief perform collision test between one object and all the objects /// belonging to the manager virtual void collide(ContinuousCollisionObject* obj, CollisionCallBackBase* callback) const = 0; /// @brief perform distance computation between one object and all the objects /// belonging to the manager virtual void distance(ContinuousCollisionObject* obj, DistanceCallBackBase* callback) const = 0; /// @brief perform collision test for the objects belonging to the manager /// (i.e., N^2 self collision) virtual void collide(CollisionCallBackBase* callback) const = 0; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) virtual void distance(DistanceCallBackBase* callback) const = 0; /// @brief perform collision test with objects belonging to another manager virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, CollisionCallBackBase* callback) const = 0; /// @brief perform distance test with objects belonging to another manager virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, DistanceCallBackBase* callback) const = 0; /// @brief whether the manager is empty virtual bool empty() const = 0; /// @brief the number of objects managed by the manager virtual size_t size() const = 0; }; using BroadPhaseContinuousCollisionManagerf = BroadPhaseContinuousCollisionManager; using BroadPhaseContinuousCollisionManagerd = BroadPhaseContinuousCollisionManager; } // namespace coal #include "coal/broadphase/broadphase_continuous_collision_manager-inl.h" #endif ================================================ FILE: include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H #define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include #if COAL_HAVE_OCTOMAP #include "coal/octree.h" #endif #include "coal/BV/BV.h" #include "coal/shape/geometric_shapes_utility.h" namespace coal { namespace detail { namespace dynamic_AABB_tree { #if COAL_HAVE_OCTOMAP //============================================================================== template bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Eigen::MatrixBase& translation2, CollisionCallBackBase* callback) { if (!root2) { if (root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); if (!obj1->collisionGeometry()->isFree()) { const AABB& root2_bv_t = translate(root2_bv, translation2); if (root1->bv.overlap(root2_bv_t)) { Box* box = new Box(); Transform3s box_tf; Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } } } else { if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, translation2, callback)) return true; if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, translation2, callback)) return true; } return false; } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { const AABB& root2_bv_t = translate(root2_bv, translation2); if (root1->bv.overlap(root2_bv_t)) { Box* box = new Box(); Transform3s box_tf; Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = Scalar(root2->getOccupancy()); box->threshold_occupied = tree2->getOccupancyThres(); CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } else return false; } else return false; } const AABB& root2_bv_t = translate(root2_bv, translation2); if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, callback)) return true; if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, callback)) return true; } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); if (collisionRecurse_(root1, tree2, child, child_bv, translation2, callback)) return true; } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2, callback)) return true; } } } return false; } //============================================================================== template bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Eigen::MatrixBase& translation2, DistanceCallBackBase* callback, Scalar& min_dist) { if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3s box_tf; Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); return (*callback)(static_cast(root1->data), &obj, min_dist); } else return false; } if (!tree2->isNodeOccupied(root2)) return false; if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, translation2); Scalar d1 = aabb2.distance(root1->children[0]->bv); Scalar d2 = aabb2.distance(root1->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, callback, min_dist)) return true; } } } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); const AABB& aabb2 = translate(child_bv, translation2); Scalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(root1, tree2, child, child_bv, translation2, callback, min_dist)) return true; } } } } return false; } #endif } // namespace dynamic_AABB_tree } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_dynamic_AABB_tree.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H #define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H #include #include #include "coal/fwd.hh" // #include "coal/BV/utility.h" #include "coal/shape/geometric_shapes.h" // #include "coal/geometry/shape/utility.h" #include "coal/broadphase/broadphase_collision_manager.h" #include "coal/broadphase/detail/hierarchy_tree.h" namespace coal { class COAL_DLLAPI DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; using DynamicAABBNode = detail::NodeBase; using DynamicAABBTable = std::unordered_map; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; int* tree_topdown_balance_threshold{nullptr}; int* tree_topdown_level{nullptr}; int tree_init_level; bool octree_as_geometry_collide; bool octree_as_geometry_distance; DynamicAABBTreeCollisionManager(); /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); /// @brief add one object to the manager void registerObject(CollisionObject* obj); /// @brief remove one object from the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager virtual void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects /// belonging to the manager void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief perform distance computation between one object and all the objects /// belonging to the manager void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; /// @brief perform collision test for the objects belonging to the manager /// (i.e., N^2 self collision) void collide(CollisionCallBackBase* callback) const; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager size_t size() const; /// @brief returns the AABB tree structure. const detail::HierarchyTree& getTree() const; /// @brief returns the AABB tree structure. detail::HierarchyTree& getTree(); private: detail::HierarchyTree dtree{}; std::unordered_map table; bool setup_; void update_(CollisionObject* updated_obj); }; } // namespace coal #include "coal/broadphase/broadphase_dynamic_AABB_tree-inl.h" #endif ================================================ FILE: include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H #define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #include "coal/shape/geometric_shapes_utility.h" #if COAL_HAVE_OCTOMAP #include "coal/octree.h" #endif namespace coal { namespace detail { namespace dynamic_AABB_tree_array { #if COAL_HAVE_OCTOMAP //============================================================================== template bool collisionRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Eigen::MatrixBase& translation2, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; if (!root2) { if (root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); if (!obj1->collisionGeometry()->isFree()) { const AABB& root_bv_t = translate(root2_bv, translation2); if (root1->bv.overlap(root_bv_t)) { Box* box = new Box(); Transform3s box_tf; Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } } } else { if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, root2_bv, translation2, callback)) return true; if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, root2_bv, translation2, callback)) return true; } return false; } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { const AABB& root_bv_t = translate(root2_bv, translation2); if (root1->bv.overlap(root_bv_t)) { Box* box = new Box(); Transform3s box_tf; Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = Scalar(root2->getOccupancy()); box->threshold_occupied = tree2->getOccupancyThres(); CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } else return false; } else return false; } const AABB& root_bv_t = translate(root2_bv, translation2); if (tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, callback)) return true; if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, callback)) return true; } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, callback)) return true; } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, translation2, callback)) return true; } } } return false; } //============================================================================== template bool distanceRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Eigen::MatrixBase& translation2, DistanceCallBackBase* callback, Scalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3s box_tf; Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); return (*callback)(static_cast(root1->data), &obj, min_dist); } else return false; } if (!tree2->isNodeOccupied(root2)) return false; if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, translation2); Scalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv); Scalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, callback, min_dist)) return true; } } } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); const AABB& aabb2 = translate(child_bv, translation2); Scalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, callback, min_dist)) return true; } } } } return false; } #endif } // namespace dynamic_AABB_tree_array } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H #define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H #include #include #include #include "coal/fwd.hh" // #include "coal/BV/utility.h" #include "coal/shape/geometric_shapes.h" // #include "coal/geometry/shape/utility.h" #include "coal/broadphase/broadphase_collision_manager.h" #include "coal/broadphase/detail/hierarchy_tree_array.h" namespace coal { class COAL_DLLAPI DynamicAABBTreeArrayCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; using DynamicAABBNode = detail::implementation_array::NodeBase; using DynamicAABBTable = std::unordered_map; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; int* tree_topdown_balance_threshold{nullptr}; int* tree_topdown_level{nullptr}; int tree_init_level; bool octree_as_geometry_collide; bool octree_as_geometry_distance; DynamicAABBTreeArrayCollisionManager(); /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); /// @brief add one object to the manager void registerObject(CollisionObject* obj); /// @brief remove one object from the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager virtual void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects /// belonging to the manager void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief perform distance computation between one object and all the objects /// belonging to the manager void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; /// @brief perform collision test for the objects belonging to the manager /// (i.e., N^2 self collision) void collide(CollisionCallBackBase* callback) const; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager size_t size() const; const detail::implementation_array::HierarchyTree& getTree() const; private: detail::implementation_array::HierarchyTree dtree{}; std::unordered_map table; bool setup_; void update_(CollisionObject* updated_obj); }; } // namespace coal #include "coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h" #endif ================================================ FILE: include/coal/broadphase/broadphase_interval_tree.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROAD_PHASE_INTERVAL_TREE_H #define COAL_BROAD_PHASE_INTERVAL_TREE_H #include #include #include "coal/broadphase/broadphase_collision_manager.h" #include "coal/broadphase/detail/interval_tree.h" namespace coal { /// @brief Collision manager based on interval tree class COAL_DLLAPI IntervalTreeCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; IntervalTreeCollisionManager(); ~IntervalTreeCollisionManager(); /// @brief remove one object from the manager void registerObject(CollisionObject* obj); /// @brief add one object to the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager virtual void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects /// belonging to the manager void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief perform distance computation between one object and all the objects /// belonging to the manager void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; /// @brief perform collision test for the objects belonging to the manager /// (i.e., N^2 self collision) void collide(CollisionCallBackBase* callback) const; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager size_t size() const; protected: /// @brief SAP end point /// @brief SAP end point struct COAL_DLLAPI EndPoint { /// @brief object related with the end point CollisionObject* obj; /// @brief end point value Scalar value; /// @brief tag for whether it is a lower bound or higher bound of an /// interval, 0 for lo, and 1 for hi char minmax; bool operator<(const EndPoint& p) const; }; /// @brief Extention interval tree's interval to SAP interval, adding more /// information struct COAL_DLLAPI SAPInterval : public detail::SimpleInterval { CollisionObject* obj; SAPInterval(Scalar low_, Scalar high_, CollisionObject* obj_); }; bool checkColl( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, CollisionObject* obj, CollisionCallBackBase* callback) const; bool checkDist( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const; bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const; /// @brief vector stores all the end points std::vector endpoints[3]; /// @brief interval tree manages the intervals detail::IntervalTree* interval_trees[3]; std::map obj_interval_maps[3]; /// @brief tag for whether the interval tree is maintained suitably bool setup_; }; } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_spatialhash-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H #define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H #include "coal/broadphase/broadphase_spatialhash.h" namespace coal { //============================================================================== template SpatialHashingCollisionManager::SpatialHashingCollisionManager( Scalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max, unsigned int default_table_size) : scene_limit(AABB(scene_min, scene_max)), hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) { hash_table->init(default_table_size); } //============================================================================== template SpatialHashingCollisionManager::~SpatialHashingCollisionManager() { delete hash_table; } //============================================================================== template void SpatialHashingCollisionManager::registerObject( CollisionObject* obj) { objs.push_back(obj); const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; if (scene_limit.overlap(obj_aabb, overlap_aabb)) { if (!scene_limit.contain(obj_aabb)) objs_partially_penetrating_scene_limit.push_back(obj); hash_table->insert(overlap_aabb, obj); } else { objs_outside_scene_limit.push_back(obj); } obj_aabb_map[obj] = obj_aabb; } //============================================================================== template void SpatialHashingCollisionManager::unregisterObject( CollisionObject* obj) { objs.remove(obj); const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; if (scene_limit.overlap(obj_aabb, overlap_aabb)) { if (!scene_limit.contain(obj_aabb)) objs_partially_penetrating_scene_limit.remove(obj); hash_table->remove(overlap_aabb, obj); } else { objs_outside_scene_limit.remove(obj); } obj_aabb_map.erase(obj); } //============================================================================== template void SpatialHashingCollisionManager::setup() { // Do nothing } //============================================================================== template void SpatialHashingCollisionManager::update() { hash_table->clear(); objs_partially_penetrating_scene_limit.clear(); objs_outside_scene_limit.clear(); for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) { CollisionObject* obj = *it; const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; if (scene_limit.overlap(obj_aabb, overlap_aabb)) { if (!scene_limit.contain(obj_aabb)) objs_partially_penetrating_scene_limit.push_back(obj); hash_table->insert(overlap_aabb, obj); } else { objs_outside_scene_limit.push_back(obj); } obj_aabb_map[obj] = obj_aabb; } } //============================================================================== template void SpatialHashingCollisionManager::update( CollisionObject* updated_obj) { const AABB& new_aabb = updated_obj->getAABB(); const AABB& old_aabb = obj_aabb_map[updated_obj]; AABB old_overlap_aabb; const auto is_old_aabb_overlapping = scene_limit.overlap(old_aabb, old_overlap_aabb); if (is_old_aabb_overlapping) hash_table->remove(old_overlap_aabb, updated_obj); AABB new_overlap_aabb; const auto is_new_aabb_overlapping = scene_limit.overlap(new_aabb, new_overlap_aabb); if (is_new_aabb_overlapping) hash_table->insert(new_overlap_aabb, updated_obj); ObjectStatus old_status; if (is_old_aabb_overlapping) { if (scene_limit.contain(old_aabb)) old_status = Inside; else old_status = PartiallyPenetrating; } else { old_status = Outside; } if (is_new_aabb_overlapping) { if (scene_limit.contain(new_aabb)) { if (old_status == PartiallyPenetrating) { // Status change: PartiallyPenetrating --> Inside // Required action(s): // - remove object from "objs_partially_penetrating_scene_limit" auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(), objs_partially_penetrating_scene_limit.end(), updated_obj); objs_partially_penetrating_scene_limit.erase(find_it); } else if (old_status == Outside) { // Status change: Outside --> Inside // Required action(s): // - remove object from "objs_outside_scene_limit" auto find_it = std::find(objs_outside_scene_limit.begin(), objs_outside_scene_limit.end(), updated_obj); objs_outside_scene_limit.erase(find_it); } } else { if (old_status == Inside) { // Status change: Inside --> PartiallyPenetrating // Required action(s): // - add object to "objs_partially_penetrating_scene_limit" objs_partially_penetrating_scene_limit.push_back(updated_obj); } else if (old_status == Outside) { // Status change: Outside --> PartiallyPenetrating // Required action(s): // - remove object from "objs_outside_scene_limit" // - add object to "objs_partially_penetrating_scene_limit" auto find_it = std::find(objs_outside_scene_limit.begin(), objs_outside_scene_limit.end(), updated_obj); objs_outside_scene_limit.erase(find_it); objs_partially_penetrating_scene_limit.push_back(updated_obj); } } } else { if (old_status == Inside) { // Status change: Inside --> Outside // Required action(s): // - add object to "objs_outside_scene_limit" objs_outside_scene_limit.push_back(updated_obj); } else if (old_status == PartiallyPenetrating) { // Status change: PartiallyPenetrating --> Outside // Required action(s): // - remove object from "objs_partially_penetrating_scene_limit" // - add object to "objs_outside_scene_limit" auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(), objs_partially_penetrating_scene_limit.end(), updated_obj); objs_partially_penetrating_scene_limit.erase(find_it); objs_outside_scene_limit.push_back(updated_obj); } } obj_aabb_map[updated_obj] = new_aabb; } //============================================================================== template void SpatialHashingCollisionManager::update( const std::vector& updated_objs) { for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); } //============================================================================== template void SpatialHashingCollisionManager::clear() { objs.clear(); hash_table->clear(); objs_outside_scene_limit.clear(); obj_aabb_map.clear(); } //============================================================================== template void SpatialHashingCollisionManager::getObjects( std::vector& objs_) const { objs_.resize(objs.size()); std::copy(objs.begin(), objs.end(), objs_.begin()); } //============================================================================== template void SpatialHashingCollisionManager::collide( CollisionObject* obj, CollisionCallBackBase* callback) const { if (size() == 0) return; collide_(obj, callback); } //============================================================================== template void SpatialHashingCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== template bool SpatialHashingCollisionManager::collide_( CollisionObject* obj, CollisionCallBackBase* callback) const { const auto& obj_aabb = obj->getAABB(); AABB overlap_aabb; if (scene_limit.overlap(obj_aabb, overlap_aabb)) { const auto query_result = hash_table->query(overlap_aabb); for (const auto& obj2 : query_result) { if (obj == obj2) continue; if ((*callback)(obj, obj2)) return true; } if (!scene_limit.contain(obj_aabb)) { for (const auto& obj2 : objs_outside_scene_limit) { if (obj == obj2) continue; if ((*callback)(obj, obj2)) return true; } } } else { for (const auto& obj2 : objs_partially_penetrating_scene_limit) { if (obj == obj2) continue; if ((*callback)(obj, obj2)) return true; } for (const auto& obj2 : objs_outside_scene_limit) { if (obj == obj2) continue; if ((*callback)(obj, obj2)) return true; } } return false; } //============================================================================== template bool SpatialHashingCollisionManager::distance_( CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const { auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; auto aabb = obj->getAABB(); if (min_dist < (std::numeric_limits::max)()) { Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } AABB overlap_aabb; auto status = 1; Scalar old_min_distance; while (1) { old_min_distance = min_dist; if (scene_limit.overlap(aabb, overlap_aabb)) { if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb), callback, min_dist)) { return true; } if (!scene_limit.contain(aabb)) { if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, min_dist)) { return true; } } } else { if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit, callback, min_dist)) { return true; } if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, min_dist)) { return true; } } if (status == 1) { if (old_min_distance < (std::numeric_limits::max)()) { break; } else { if (min_dist < old_min_distance) { Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { if (aabb == obj->getAABB()) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } } else if (status == 0) { break; } } return false; } //============================================================================== template void SpatialHashingCollisionManager::collide( CollisionCallBackBase* callback) const { if (size() == 0) return; for (const auto& obj1 : objs) { const auto& obj_aabb = obj1->getAABB(); AABB overlap_aabb; if (scene_limit.overlap(obj_aabb, overlap_aabb)) { auto query_result = hash_table->query(overlap_aabb); for (const auto& obj2 : query_result) { if (obj1 < obj2) { if ((*callback)(obj1, obj2)) return; } } if (!scene_limit.contain(obj_aabb)) { for (const auto& obj2 : objs_outside_scene_limit) { if (obj1 < obj2) { if ((*callback)(obj1, obj2)) return; } } } } else { for (const auto& obj2 : objs_partially_penetrating_scene_limit) { if (obj1 < obj2) { if ((*callback)(obj1, obj2)) return; } } for (const auto& obj2 : objs_outside_scene_limit) { if (obj1 < obj2) { if ((*callback)(obj1, obj2)) return; } } } } } //============================================================================== template void SpatialHashingCollisionManager::distance( DistanceCallBackBase* callback) const { if (size() == 0) return; this->enable_tested_set_ = true; this->tested_set.clear(); Scalar min_dist = (std::numeric_limits::max)(); for (const auto& obj : objs) { if (distance_(obj, callback, min_dist)) break; } this->enable_tested_set_ = false; this->tested_set.clear(); } //============================================================================== template void SpatialHashingCollisionManager::collide( BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const { auto* other_manager = static_cast*>(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { collide(callback); return; } if (this->size() < other_manager->size()) { for (const auto& obj : objs) { if (other_manager->collide_(obj, callback)) return; } } else { for (const auto& obj : other_manager->objs) { if (collide_(obj, callback)) return; } } } //============================================================================== template void SpatialHashingCollisionManager::distance( BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const { auto* other_manager = static_cast*>(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { distance(callback); return; } Scalar min_dist = (std::numeric_limits::max)(); if (this->size() < other_manager->size()) { for (const auto& obj : objs) if (other_manager->distance_(obj, callback, min_dist)) return; } else { for (const auto& obj : other_manager->objs) if (distance_(obj, callback, min_dist)) return; } } //============================================================================== template bool SpatialHashingCollisionManager::empty() const { return objs.empty(); } //============================================================================== template size_t SpatialHashingCollisionManager::size() const { return objs.size(); } //============================================================================== template void SpatialHashingCollisionManager::computeBound( std::vector& objs, Vec3s& l, Vec3s& u) { AABB bound; for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB(); l = bound.min_; u = bound.max_; } //============================================================================== template template bool SpatialHashingCollisionManager::distanceObjectToObjects( CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback, Scalar& min_dist) const { for (auto& obj2 : objs) { if (obj == obj2) continue; if (!this->enable_tested_set_) { if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { if ((*callback)(obj, obj2, min_dist)) return true; } } else { if (!this->inTestedSet(obj, obj2)) { if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { if ((*callback)(obj, obj2, min_dist)) return true; } this->insertTestedSet(obj, obj2); } } } return false; } } // namespace coal #endif ================================================ FILE: include/coal/broadphase/broadphase_spatialhash.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_H #define COAL_BROADPHASE_BROADPAHSESPATIALHASH_H #include #include #include "coal/BV/AABB.h" #include "coal/broadphase/broadphase_collision_manager.h" #include "coal/broadphase/detail/simple_hash_table.h" #include "coal/broadphase/detail/sparse_hash_table.h" #include "coal/broadphase/detail/spatial_hash.h" namespace coal { /// @brief spatial hashing collision mananger template > class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; SpatialHashingCollisionManager(Scalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max, unsigned int default_table_size = 1000); ~SpatialHashingCollisionManager(); /// @brief add one object to the manager void registerObject(CollisionObject* obj); /// @brief remove one object from the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager virtual void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects /// belonging to the manager void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief perform distance computation between one object and all the objects /// belonging ot the manager void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; /// @brief perform collision test for the objects belonging to the manager /// (i.e, N^2 self collision) void collide(CollisionCallBackBase* callback) const; /// @brief perform distance test for the objects belonging to the manager /// (i.e., N^2 self distance) void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager size_t size() const; /// @brief compute the bound for the environent static void computeBound(std::vector& objs, Vec3s& l, Vec3s& u); protected: /// @brief perform collision test between one object and all the objects /// belonging to the manager bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; /// @brief perform distance computation between one object and all the objects /// belonging ot the manager bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const; /// @brief all objects in the scene std::list objs; /// @brief objects partially penetrating (not totally inside nor outside) the /// scene limit are in another list std::list objs_partially_penetrating_scene_limit; /// @brief objects outside the scene limit are in another list std::list objs_outside_scene_limit; /// @brief the size of the scene AABB scene_limit; /// @brief store the map between objects and their aabbs. will make update /// more convenient std::map obj_aabb_map; /// @brief objects in the scene limit (given by scene_min and scene_max) are /// in the spatial hash table HashTable* hash_table; private: enum ObjectStatus { Inside, PartiallyPenetrating, Outside }; template bool distanceObjectToObjects(CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback, Scalar& min_dist) const; }; } // namespace coal #include "coal/broadphase/broadphase_spatialhash-inl.h" #endif ================================================ FILE: include/coal/broadphase/default_broadphase_callbacks.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2020, Toyota Research Institute * Copyright (c) 2022-2023, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Sean Curtis (sean@tri.global) */ /** @author Justin Carpentier (justin.carpentier@inria.fr) */ #ifndef COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H #define COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H #include "coal/broadphase/broadphase_callbacks.h" #include "coal/collision.h" #include "coal/distance.h" // #include "coal/narrowphase/continuous_collision.h" // #include "coal/narrowphase/continuous_collision_request.h" // #include "coal/narrowphase/continuous_collision_result.h" // #include "coal/narrowphase/distance_request.h" // #include "coal/narrowphase/distance_result.h" namespace coal { /// @brief Collision data stores the collision request and the result given by /// collision algorithm. struct CollisionData { CollisionData() { done = false; } /// @brief Collision request CollisionRequest request; /// @brief Collision result CollisionResult result; /// @brief Whether the collision iteration can stop bool done; /// @brief Clears the CollisionData void clear() { result.clear(); done = false; } }; /// @brief Distance data stores the distance request and the result given by /// distance algorithm. struct DistanceData { DistanceData() { done = false; } /// @brief Distance request DistanceRequest request; /// @brief Distance result DistanceResult result; /// @brief Whether the distance iteration can stop bool done; /// @brief Clears the DistanceData void clear() { result.clear(); done = false; } }; /// @brief Provides a simple callback for the collision query in the /// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and /// points to an instance of CollisionData. It simply invokes the /// `collide()` method on the culled pair of geometries and stores the results /// in the data's CollisionResult instance. /// /// This callback will cause the broadphase evaluation to stop if: /// - the collision requests _disables_ cost _and_ /// - the collide() reports a collision for the culled pair, _and_ /// - we've reported the number of contacts requested in the CollisionRequest. /// /// For a given instance of CollisionData, if broadphase evaluation has /// already terminated (i.e., defaultCollisionFunction() returned `true`), /// subsequent invocations with the same instance of CollisionData will /// return immediately, requesting termination of broadphase evaluation (i.e., /// return `true`). /// /// @param o1 The first object in the culled pair. /// @param o2 The second object in the culled pair. /// @param data A non-null pointer to a CollisionData instance. /// @return `true` if the broadphase evaluation should stop. /// @tparam S The scalar type with which the computation will be performed. bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* data); /// @brief Collision data for use with the DefaultContinuousCollisionFunction. /// It stores the collision request and the result given by the collision /// algorithm (and stores the conclusion of whether further evaluation of the /// broadphase collision manager has been deemed unnecessary). // struct DefaultContinuousCollisionData { // ContinuousCollisionRequest request; // ContinuousCollisionResult result; // // /// If `true`, requests that the broadphase evaluation stop. // bool done{false}; // }; /// @brief Provides a simple callback for the continuous collision query in the /// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and /// points to an instance of DefaultContinuousCollisionData. It simply invokes /// the `collide()` method on the culled pair of geometries and stores the /// results in the data's ContinuousCollisionResult instance. /// /// This callback will never cause the broadphase evaluation to terminate early. /// However, if the `done` member of the DefaultContinuousCollisionData is set /// to true, this method will simply return without doing any computation. /// /// For a given instance of DefaultContinuousCollisionData, if broadphase /// evaluation has already terminated (i.e., /// DefaultContinuousCollisionFunction() returned `true`), subsequent /// invocations with the same instance of CollisionData will return /// immediately, requesting termination of broadphase evaluation (i.e., return /// `true`). /// /// @param o1 The first object in the culled pair. /// @param o2 The second object in the culled pair. /// @param data A non-null pointer to a CollisionData instance. /// @return True if the broadphase evaluation should stop. /// @tparam S The scalar type with which the computation will be performed. // bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1, // ContinuousCollisionObject* o2, // void* data) { // assert(data != nullptr); // auto* cdata = static_cast(data); // // if (cdata->done) return true; // // const ContinuousCollisionRequest& request = cdata->request; // ContinuousCollisionResult& result = cdata->result; // collide(o1, o2, request, result); // // return cdata->done; // } /// @brief Provides a simple callback for the distance query in the /// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and /// points to an instance of DistanceData. It simply invokes the /// `distance()` method on the culled pair of geometries and stores the results /// in the data's DistanceResult instance. /// /// This callback will cause the broadphase evaluation to stop if: /// - The distance is less than or equal to zero (i.e., the pair is in /// contact). /// /// For a given instance of DistanceData, if broadphase evaluation has /// already terminated (i.e., defaultDistanceFunction() returned `true`), /// subsequent invocations with the same instance of DistanceData will /// simply report the previously reported minimum distance and request /// termination of broadphase evaluation (i.e., return `true`). /// /// @param o1 The first object in the culled pair. /// @param o2 The second object in the culled pair. /// @param data A non-null pointer to a DistanceData instance. /// @param dist The distance computed by distance(). /// @return `true` if the broadphase evaluation should stop. /// @tparam S The scalar type with which the computation will be performed. bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* data, Scalar& dist); /// @brief Default collision callback to check collision between collision /// objects. struct COAL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase { /// @brief Initialize the callback. /// Clears the collision result and sets the done boolean to false. void init() { data.clear(); } bool collide(CollisionObject* o1, CollisionObject* o2); CollisionData data; virtual ~CollisionCallBackDefault() {}; }; /// @brief Default distance callback to check collision between collision /// objects. struct COAL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { /// @brief Initialize the callback. /// Clears the distance result and sets the done boolean to false. void init() { data.clear(); } bool distance(CollisionObject* o1, CollisionObject* o2, Scalar& dist); DistanceData data; virtual ~DistanceCallBackDefault() {}; }; /// @brief Collision callback to collect collision pairs potentially in contacts struct COAL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase { typedef std::pair CollisionPair; /// @brief Default constructor. CollisionCallBackCollect(const size_t max_size); bool collide(CollisionObject* o1, CollisionObject* o2); /// @brief Returns the number of registered collision pairs size_t numCollisionPairs() const; /// @brief Returns a const reference to the active collision_pairs to check const std::vector& getCollisionPairs() const; /// @brief Reset the callback void init(); /// @brief Check whether a collision pair exists bool exist(const CollisionPair& pair) const; /// @brief Check whether a collision pair exists bool exist(CollisionObject* o1, CollisionObject* o2) const; virtual ~CollisionCallBackCollect() {}; protected: std::vector collision_pairs; size_t max_size; }; } // namespace coal #endif // COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H ================================================ FILE: include/coal/broadphase/detail/hierarchy_tree-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_HIERARCHY_TREE_INL_H #define COAL_HIERARCHY_TREE_INL_H #include "coal/broadphase/detail/hierarchy_tree.h" namespace coal { namespace detail { //============================================================================== template HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { root_node = nullptr; n_leaves = 0; free_node = nullptr; max_lookahead_level = -1; opath = 0; bu_threshold = bu_threshold_; topdown_level = topdown_level_; } //============================================================================== template HierarchyTree::~HierarchyTree() { clear(); } //============================================================================== template void HierarchyTree::init(std::vector& leaves, int level) { switch (level) { case 0: init_0(leaves); break; case 1: init_1(leaves); break; case 2: init_2(leaves); break; case 3: init_3(leaves); break; default: init_0(leaves); } } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::insert(const BV& bv, void* data) { Node* leaf = createNode(nullptr, bv, data); insertLeaf(root_node, leaf); ++n_leaves; return leaf; } //============================================================================== template void HierarchyTree::remove(Node* leaf) { removeLeaf(leaf); deleteNode(leaf); --n_leaves; } //============================================================================== template void HierarchyTree::clear() { if (root_node) recurseDeleteNode(root_node); n_leaves = 0; delete free_node; free_node = nullptr; max_lookahead_level = -1; opath = 0; } //============================================================================== template bool HierarchyTree::empty() const { return (nullptr == root_node); } //============================================================================== template void HierarchyTree::update(Node* leaf, int lookahead_level) { // TODO(DamrongGuoy): Since we update a leaf node by removing and // inserting the same leaf node, it is likely to change the structure of // the tree even if no object's pose has changed. In the future, // find a way to preserve the structure of the tree to solve this issue: // https://github.com/flexible-collision-library/fcl/issues/368 // First we remove the leaf node pointed by `leaf` variable from the tree. // The `sub_root` variable is the root of the subtree containing nodes // whose BVs were adjusted due to node removal. typename HierarchyTree::Node* sub_root = removeLeaf(leaf); if (sub_root) { if (lookahead_level > 0) { // For positive `lookahead_level`, we move the `sub_root` variable up. for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i) sub_root = sub_root->parent; } else // By default, lookahead_level = -1, and we reset the `sub_root` variable // to the root of the entire tree. sub_root = root_node; } // Then we insert the node pointed by `leaf` variable back into the // subtree rooted at `sub_root` variable. insertLeaf(sub_root, leaf); } //============================================================================== template bool HierarchyTree::update(Node* leaf, const BV& bv) { if (leaf->bv.contain(bv)) return false; update_(leaf, bv); return true; } //============================================================================== template struct UpdateImpl { static bool run(const HierarchyTree& tree, typename HierarchyTree::Node* leaf, const BV& bv, const Vec3s& /*vel*/, Scalar /*margin*/) { if (leaf->bv.contain(bv)) return false; tree.update_(leaf, bv); return true; } static bool run(const HierarchyTree& tree, typename HierarchyTree::Node* leaf, const BV& bv, const Vec3s& /*vel*/) { if (leaf->bv.contain(bv)) return false; tree.update_(leaf, bv); return true; } }; //============================================================================== template bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3s& vel, Scalar margin) { return UpdateImpl::run(*this, leaf, bv, vel, margin); } //============================================================================== template bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3s& vel) { return UpdateImpl::run(*this, leaf, bv, vel); } //============================================================================== template size_t HierarchyTree::getMaxHeight() const { if (!root_node) return 0; return getMaxHeight(root_node); } //============================================================================== template size_t HierarchyTree::getMaxDepth() const { if (!root_node) return 0; size_t max_depth; getMaxDepth(root_node, 0, max_depth); return max_depth; } //============================================================================== template void HierarchyTree::balanceBottomup() { if (root_node) { std::vector leaves; leaves.reserve(n_leaves); fetchLeaves(root_node, leaves); bottomup(leaves.begin(), leaves.end()); root_node = leaves[0]; } } //============================================================================== template void HierarchyTree::balanceTopdown() { if (root_node) { std::vector leaves; leaves.reserve(n_leaves); fetchLeaves(root_node, leaves); root_node = topdown(leaves.begin(), leaves.end()); } } //============================================================================== template void HierarchyTree::balanceIncremental(int iterations) { if (iterations < 0) iterations = (int)n_leaves; if (root_node && (iterations > 0)) { for (int i = 0; i < iterations; ++i) { Node* node = root_node; unsigned int bit = 0; while (!node->isLeaf()) { node = sort(node, root_node)->children[(opath >> bit) & 1]; bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); } update(node); ++opath; } } } //============================================================================== template void HierarchyTree::refit() { if (root_node) recurseRefit(root_node); } //============================================================================== template void HierarchyTree::extractLeaves(const Node* root, std::vector& leaves) const { if (!root->isLeaf()) { extractLeaves(root->children[0], leaves); extractLeaves(root->children[1], leaves); } else leaves.push_back(root); } //============================================================================== template size_t HierarchyTree::size() const { return n_leaves; } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::getRoot() const { return root_node; } //============================================================================== template typename HierarchyTree::Node*& HierarchyTree::getRoot() { return root_node; } //============================================================================== template void HierarchyTree::print(Node* root, int depth) { for (int i = 0; i < depth; ++i) std::cout << " "; std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; if (root->isLeaf()) { } else { print(root->children[0], depth + 1); print(root->children[1], depth + 1); } } //============================================================================== template void HierarchyTree::bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend) { NodeVecIterator lcur_end = lend; while (lbeg < lcur_end - 1) { NodeVecIterator min_it1 = lbeg; NodeVecIterator min_it2 = lbeg + 1; Scalar min_size = (std::numeric_limits::max)(); for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) { for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) { Scalar cur_size = ((*it1)->bv + (*it2)->bv).size(); if (cur_size < min_size) { min_size = cur_size; min_it1 = it1; min_it2 = it2; } } } Node* n[2] = {*min_it1, *min_it2}; Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr); p->children[0] = n[0]; p->children[1] = n[1]; n[0]->parent = p; n[1]->parent = p; *min_it1 = p; Node* tmp = *min_it2; lcur_end--; *min_it2 = *lcur_end; *lcur_end = tmp; } } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::topdown( const NodeVecIterator lbeg, const NodeVecIterator lend) { switch (topdown_level) { case 0: return topdown_0(lbeg, lend); break; case 1: return topdown_1(lbeg, lend); break; default: return topdown_0(lbeg, lend); } } //============================================================================== template size_t HierarchyTree::getMaxHeight(Node* node) const { if (!node->isLeaf()) { size_t height1 = getMaxHeight(node->children[0]); size_t height2 = getMaxHeight(node->children[1]); return std::max(height1, height2) + 1; } else return 0; } //============================================================================== template void HierarchyTree::getMaxDepth(Node* node, size_t depth, size_t& max_depth) const { if (!node->isLeaf()) { getMaxDepth(node->children[0], depth + 1, max_depth); getMaxDepth(node->children[1], depth + 1, max_depth); } else max_depth = std::max(max_depth, depth); } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::topdown_0( const NodeVecIterator lbeg, const NodeVecIterator lend) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (num_leaves > bu_threshold) { BV vol = (*lbeg)->bv; for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv; int best_axis = 0; Scalar extent[3] = {vol.width(), vol.height(), vol.depth()}; if (extent[1] > extent[0]) best_axis = 1; if (extent[2] > extent[best_axis]) best_axis = 2; // compute median NodeVecIterator lcenter = lbeg + num_leaves / 2; std::nth_element(lbeg, lcenter, lend, std::bind(&nodeBaseLess, std::placeholders::_1, std::placeholders::_2, std::ref(best_axis))); Node* node = createNode(nullptr, vol, nullptr); node->children[0] = topdown_0(lbeg, lcenter); node->children[1] = topdown_0(lcenter, lend); node->children[0]->parent = node; node->children[1]->parent = node; return node; } else { bottomup(lbeg, lend); return *lbeg; } } return *lbeg; } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::topdown_1( const NodeVecIterator lbeg, const NodeVecIterator lend) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (num_leaves > bu_threshold) { Vec3s split_p = (*lbeg)->bv.center(); BV vol = (*lbeg)->bv; NodeVecIterator it; for (it = lbeg + 1; it < lend; ++it) { split_p += (*it)->bv.center(); vol += (*it)->bv; } split_p /= static_cast(num_leaves); int best_axis = -1; long bestmidp = num_leaves; int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; for (it = lbeg; it < lend; ++it) { Vec3s x = (*it)->bv.center() - split_p; for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; } for (int i = 0; i < 3; ++i) { if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { long midp = std::abs(splitcount[i][0] - splitcount[i][1]); if (midp < bestmidp) { best_axis = i; bestmidp = midp; } } } if (best_axis < 0) best_axis = 0; Scalar split_value = split_p[best_axis]; NodeVecIterator lcenter = lbeg; for (it = lbeg; it < lend; ++it) { if ((*it)->bv.center()[best_axis] < split_value) { Node* temp = *it; *it = *lcenter; *lcenter = temp; ++lcenter; } } Node* node = createNode(nullptr, vol, nullptr); node->children[0] = topdown_1(lbeg, lcenter); node->children[1] = topdown_1(lcenter, lend); node->children[0]->parent = node; node->children[1]->parent = node; return node; } else { bottomup(lbeg, lend); return *lbeg; } } return *lbeg; } //============================================================================== template void HierarchyTree::init_0(std::vector& leaves) { clear(); root_node = topdown(leaves.begin(), leaves.end()); n_leaves = leaves.size(); max_lookahead_level = -1; opath = 0; } //============================================================================== template void HierarchyTree::init_1(std::vector& leaves) { clear(); BV bound_bv; if (leaves.size() > 0) bound_bv = leaves[0]->bv; for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits() - 1)), coder.bits() - 1); refit(); n_leaves = leaves.size(); max_lookahead_level = -1; opath = 0; } //============================================================================== template void HierarchyTree::init_2(std::vector& leaves) { clear(); BV bound_bv; if (leaves.size() > 0) bound_bv = leaves[0]->bv; for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits() - 1)), coder.bits() - 1); refit(); n_leaves = leaves.size(); max_lookahead_level = -1; opath = 0; } //============================================================================== template void HierarchyTree::init_3(std::vector& leaves) { clear(); BV bound_bv; if (leaves.size() > 0) bound_bv = leaves[0]->bv; for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); root_node = mortonRecurse_2(leaves.begin(), leaves.end()); refit(); n_leaves = leaves.size(); max_lookahead_level = -1; opath = 0; } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::mortonRecurse_0( const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (bits > 0) { Node dummy; dummy.code = split; NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); if (lcenter == lbeg) { uint32_t split2 = split | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split2, bits - 1); } else if (lcenter == lend) { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split1, bits - 1); } else { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); uint32_t split2 = split | (1 << (bits - 1)); Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); Node* node = createNode(nullptr, nullptr); node->children[0] = child1; node->children[1] = child2; child1->parent = node; child2->parent = node; return node; } } else { Node* node = topdown(lbeg, lend); return node; } } else return *lbeg; } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::mortonRecurse_1( const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (bits > 0) { Node dummy; dummy.code = split; NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); if (lcenter == lbeg) { uint32_t split2 = split | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split2, bits - 1); } else if (lcenter == lend) { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split1, bits - 1); } else { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); uint32_t split2 = split | (1 << (bits - 1)); Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); Node* node = createNode(nullptr, nullptr); node->children[0] = child1; node->children[1] = child2; child1->parent = node; child2->parent = node; return node; } } else { Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); Node* node = createNode(nullptr, nullptr); node->children[0] = child1; node->children[1] = child2; child1->parent = node; child2->parent = node; return node; } } else return *lbeg; } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::mortonRecurse_2( const NodeVecIterator lbeg, const NodeVecIterator lend) { long num_leaves = lend - lbeg; if (num_leaves > 1) { Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); Node* node = createNode(nullptr, nullptr); node->children[0] = child1; node->children[1] = child2; child1->parent = node; child2->parent = node; return node; } else return *lbeg; } //============================================================================== template void HierarchyTree::update_(Node* leaf, const BV& bv) { Node* root = removeLeaf(leaf); if (root) { if (max_lookahead_level >= 0) { for (int i = 0; (i < max_lookahead_level) && root->parent; ++i) root = root->parent; } else root = root_node; } leaf->bv = bv; insertLeaf(root, leaf); } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::sort(Node* n, Node*& r) { Node* p = n->parent; if (p > n) { size_t i = indexOf(n); size_t j = 1 - i; Node* s = p->children[j]; Node* q = p->parent; if (q) q->children[indexOf(p)] = n; else r = n; s->parent = n; p->parent = n; n->parent = q; p->children[0] = n->children[0]; p->children[1] = n->children[1]; n->children[0]->parent = p; n->children[1]->parent = p; n->children[i] = p; n->children[j] = s; std::swap(p->bv, n->bv); return p; } return n; } //============================================================================== template void HierarchyTree::insertLeaf(Node* const sub_root, Node* const leaf) // Attempts to insert `leaf` into a subtree rooted at `sub_root`. // 1. If the whole tree is empty, then `leaf` simply becomes the tree. // 2. Otherwise, a leaf node called `sibling` of the subtree rooted at // `sub_root` is selected (the selection criteria is a black box for this // algorithm), and the `sibling` leaf is replaced by an internal node with // two children, `sibling` and `leaf`. The bounding volumes are updated as // necessary. { if (!root_node) { // If the entire tree is empty, the node pointed by `leaf` variable will // become the root of the tree. root_node = leaf; leaf->parent = nullptr; return; } // Traverse the tree from the given `sub_root` down to an existing leaf // node that we call `sibling`. The `sibling` node will eventually become // the sibling of the given `leaf` node. Node* sibling = sub_root; while (!sibling->isLeaf()) { sibling = sibling->children[select(*leaf, *(sibling->children[0]), *(sibling->children[1]))]; } Node* prev = sibling->parent; // Create a new `node` that later will become the new parent of both the // `sibling` and the given `leaf`. Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr); if (prev) // If the parent `prev` of the `sibling` is an interior node, we will // replace the `sibling` with the subtree {node {`sibling`, leaf}} like // this: // Before After // // ╱ ╱ // prev prev // ╱ ╲ ─> ╱ ╲ // sibling ... node ... // ╱ ╲ // sibling leaf { prev->children[indexOf(sibling)] = node; node->children[0] = sibling; sibling->parent = node; node->children[1] = leaf; leaf->parent = node; // Now that we've inserted `leaf` some of the existing bounding // volumes might not fully enclose their children. Walk up the tree // looking for parents that don't already enclose their children // and create a new tight-fitting bounding volume for those. do { if (!prev->bv.contain(node->bv)) prev->bv = prev->children[0]->bv + prev->children[1]->bv; else break; node = prev; } while (nullptr != (prev = node->parent)); } else // If the `sibling` has no parent, i.e., the tree is a singleton, // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like // this: // // node // ╱ ╲ // sibling leaf { node->children[0] = sibling; sibling->parent = node; node->children[1] = leaf; leaf->parent = node; root_node = node; } // Note that the above algorithm always adds the new `leaf` node as the right // child, i.e., children[1]. Calling removeLeaf(l) followed by calling // this function insertLeaf(l) where l is a left child will result in // switching l and its sibling even if no object's pose has changed. } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::removeLeaf( Node* const leaf) { // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling, // and its parent with just its sibling. It then "tightens" the ancestor // bounding volumes. Returns a pointer to the parent of the highest node whose // BV changed due to the removal. if (leaf == root_node) { // If the `leaf` node is the only node in the tree, the tree becomes empty. root_node = nullptr; return nullptr; } Node* parent = leaf->parent; Node* prev = parent->parent; Node* sibling = parent->children[1 - indexOf(leaf)]; if (prev) { // If the parent node is not the root node, the sibling node will // replace the parent node like this: // // Before After // ... ... // ╱ ╱ // prev prev // ╱ ╲ ╱ ╲ // parent ... ─> sibling ... // ╱ ╲ ╱╲ // leaf sibling ... // ╱╲ // ... // // Step 1: replace the subtree {parent {leaf, sibling {...}}} with // {sibling {...}}. prev->children[indexOf(parent)] = sibling; sibling->parent = prev; deleteNode(parent); // Step 2: tighten up the BVs of the ancestor nodes. while (prev) { BV new_bv = prev->children[0]->bv + prev->children[1]->bv; if (!(new_bv == prev->bv)) { prev->bv = new_bv; prev = prev->parent; } else break; } return prev ? prev : root_node; } else { // If the parent node is the root node, the sibling node will become the // root of the whole tree like this: // // Before After // // parent // ╱ ╲ // leaf sibling ─> sibling // ╱╲ ╱╲ // ... ... root_node = sibling; sibling->parent = nullptr; deleteNode(parent); return root_node; } } //============================================================================== template void HierarchyTree::fetchLeaves(Node* root, std::vector& leaves, int depth) { if ((!root->isLeaf()) && depth) { fetchLeaves(root->children[0], leaves, depth - 1); fetchLeaves(root->children[1], leaves, depth - 1); deleteNode(root); } else { leaves.push_back(root); } } //============================================================================== template size_t HierarchyTree::indexOf(Node* node) { // node cannot be nullptr return (node->parent->children[1] == node); } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, const BV& bv, void* data) { Node* node = createNode(parent, data); node->bv = bv; return node; } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, const BV& bv1, const BV& bv2, void* data) { Node* node = createNode(parent, data); node->bv = bv1 + bv2; return node; } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, void* data) { Node* node = nullptr; if (free_node) { node = free_node; free_node = nullptr; } else node = new Node(); node->parent = parent; node->data = data; node->children[1] = 0; return node; } //============================================================================== template void HierarchyTree::deleteNode(Node* node) { if (free_node != node) { delete free_node; free_node = node; } } //============================================================================== template void HierarchyTree::recurseDeleteNode(Node* node) { if (!node->isLeaf()) { recurseDeleteNode(node->children[0]); recurseDeleteNode(node->children[1]); } if (node == root_node) root_node = nullptr; deleteNode(node); } //============================================================================== template void HierarchyTree::recurseRefit(Node* node) { if (!node->isLeaf()) { recurseRefit(node->children[0]); recurseRefit(node->children[1]); node->bv = node->children[0]->bv + node->children[1]->bv; } else return; } //============================================================================== template bool nodeBaseLess(NodeBase* a, NodeBase* b, int d) { if (a->bv.center()[d] < b->bv.center()[d]) return true; return false; } //============================================================================== template struct SelectImpl { static std::size_t run(const NodeBase& /*query*/, const NodeBase& /*node1*/, const NodeBase& /*node2*/) { return 0; } static std::size_t run(const BV& /*query*/, const NodeBase& /*node1*/, const NodeBase& /*node2*/) { return 0; } }; //============================================================================== template size_t select(const NodeBase& query, const NodeBase& node1, const NodeBase& node2) { return SelectImpl::run(query, node1, node2); } //============================================================================== template size_t select(const BV& query, const NodeBase& node1, const NodeBase& node2) { return SelectImpl::run(query, node1, node2); } //============================================================================== template struct SelectImpl { static std::size_t run(const NodeBase& node, const NodeBase& node1, const NodeBase& node2) { const AABB& bv = node.bv; const AABB& bv1 = node1.bv; const AABB& bv2 = node2.bv; Vec3s v = bv.min_ + bv.max_; Vec3s v1 = v - (bv1.min_ + bv1.max_); Vec3s v2 = v - (bv2.min_ + bv2.max_); Scalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); Scalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } static std::size_t run(const AABB& query, const NodeBase& node1, const NodeBase& node2) { const AABB& bv = query; const AABB& bv1 = node1.bv; const AABB& bv2 = node2.bv; Vec3s v = bv.min_ + bv.max_; Vec3s v1 = v - (bv1.min_ + bv1.max_); Vec3s v2 = v - (bv2.min_ + bv2.max_); Scalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); Scalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } }; } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/hierarchy_tree.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_HIERARCHY_TREE_H #define COAL_HIERARCHY_TREE_H #include #include #include #include #include "coal/warning.hh" #include "coal/BV/AABB.h" #include "coal/broadphase/detail/morton.h" #include "coal/broadphase/detail/node_base.h" namespace coal { namespace detail { /// @brief Class for hierarchy tree structure template class HierarchyTree { public: typedef NodeBase Node; /// @brief Create hierarchy tree with suitable setting. /// bu_threshold decides the height of tree node to start bottom-up /// construction / optimization; topdown_level decides different methods to /// construct tree in topdown manner. lower level method constructs tree with /// better quality but is slower. HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); /// @brief Initialize the tree by a set of leaves using algorithm with a given /// level. void init(std::vector& leaves, int level = 0); /// @brief Insest a node Node* insert(const BV& bv, void* data); /// @brief Remove a leaf node void remove(Node* leaf); /// @brief Clear the tree void clear(); /// @brief Whether the tree is empty bool empty() const; /// @brief Updates a `leaf` node. A use case is when the bounding volume /// of an object changes. Ensure every parent node has its bounding volume /// expand or shrink to fit the bounding volumes of its children. /// @note Strangely the structure of the tree may change even if the /// bounding volume of the `leaf` node does not change. It is just /// another valid tree of the same set of objects. This is because /// update() works by first removing `leaf` and then inserting `leaf` /// back. The structural change could be as simple as switching the /// order of two leaves if the sibling of the `leaf` is also a leaf. /// Or it could be more complicated if the sibling is an internal /// node. void update(Node* leaf, int lookahead_level = -1); /// @brief update the tree when the bounding volume of a given leaf has /// changed bool update(Node* leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction bool update(Node* leaf, const BV& bv, const Vec3s& vel, Scalar margin); /// @brief update one leaf's bounding volume, with prediction bool update(Node* leaf, const BV& bv, const Vec3s& vel); /// @brief get the max height of the tree size_t getMaxHeight() const; /// @brief get the max depth of the tree size_t getMaxDepth() const; /// @brief balance the tree from bottom void balanceBottomup(); /// @brief balance the tree from top void balanceTopdown(); /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, /// update the entire tree in a bottom-up manner void refit(); /// @brief extract all the leaves of the tree void extractLeaves(const Node* root, std::vector& leaves) const; /// @brief number of leaves in the tree size_t size() const; /// @brief get the root of the tree Node* getRoot() const; Node*& getRoot(); /// @brief print the tree in a recursive way void print(Node* root, int depth); private: typedef typename std::vector*>::iterator NodeVecIterator; typedef typename std::vector*>::const_iterator NodeVecConstIterator; struct SortByMorton { bool operator()(const Node* a, const Node* b) const { return a->code < b->code; } }; /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief construct a tree for a set of leaves from top Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief compute the maximum height of a subtree rooted from a given node size_t getMaxHeight(Node* node) const; /// @brief compute the maximum depth of a subtree rooted from a given node void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const; /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a /// topdown manner. During construction, first compute the best split axis as /// the axis along with the longest AABB edge. Then compute the median of all /// nodes' center projection onto the axis and using it as the split /// threshold. Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a /// topdown manner. During construction, first compute the best split /// thresholds for different axes as the average of all nodes' center. Then /// choose the split axis as the axis whose threshold can divide the nodes /// into two parts with almost similar size. This construction is more /// expensive then topdown_0, but also can provide tree with better quality. Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief init tree from leaves in the topdown manner (topdown_0 or /// topdown_1) void init_0(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., /// for nodes which is of depth more than the maximum bits of the morton code, /// we use bottomup method to construct the subtree, which is slow but can /// construct tree with high quality. void init_1(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., /// for nodes which is of depth more than the maximum bits of the morton code, /// we split the leaves into two parts with the same size simply using the /// node index. void init_2(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., /// for all nodes, we simply divide the leaves into parts with the same size /// simply using the node index. void init_3(std::vector& leaves); Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits); Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits); Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief update one leaf node's bounding volume void update_(Node* leaf, const BV& bv); /// @brief sort node n and its parent according to their memory position Node* sort(Node* n, Node*& r); /// @brief Insert a leaf node and also update its ancestors. Maintain the /// tree as a full binary tree (every interior node has exactly two children). /// Furthermore, adjust the BV of interior nodes so that each parent's BV /// tightly fits its children's BVs. /// @param sub_root The root of the subtree into which we will insert the // leaf node. void insertLeaf(Node* const sub_root, Node* const leaf); /// @brief Remove a leaf. Maintain the tree as a full binary tree (every /// interior node has exactly two children). Furthermore, adjust the BV of /// interior nodes so that each parent's BV tightly fits its children's BVs. /// @note The leaf node itself is not deleted yet, but all the unnecessary /// internal nodes are deleted. /// @returns the root of the subtree containing the nodes whose BVs were // adjusted. Node* removeLeaf(Node* const leaf); /// @brief Delete all internal nodes and return all leaves nodes with given /// depth from root void fetchLeaves(Node* root, std::vector& leaves, int depth = -1); static size_t indexOf(Node* node); /// @brief create one node (leaf or internal) Node* createNode(Node* parent, const BV& bv, void* data); Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data); Node* createNode(Node* parent, void* data); void deleteNode(Node* node); void recurseDeleteNode(Node* node); void recurseRefit(Node* node); protected: Node* root_node; size_t n_leaves; unsigned int opath; /// This is a one Node cache, the reason is that we need to remove a node and /// then add it again frequently. Node* free_node; int max_lookahead_level; public: /// @brief decide which topdown algorithm to use int topdown_level; /// @brief decide the depth to use expensive bottom-up algorithm int bu_threshold; }; /// @brief Compare two nodes accoording to the d-th dimension of node center template bool nodeBaseLess(NodeBase* a, NodeBase* b, int d); /// @brief select from node1 and node2 which is close to a given query. 0 for /// node1 and 1 for node2 template size_t select(const NodeBase& query, const NodeBase& node1, const NodeBase& node2); /// @brief select from node1 and node2 which is close to a given query bounding /// volume. 0 for node1 and 1 for node2 template size_t select(const BV& query, const NodeBase& node1, const NodeBase& node2); } // namespace detail } // namespace coal #include "coal/broadphase/detail/hierarchy_tree-inl.h" #endif ================================================ FILE: include/coal/broadphase/detail/hierarchy_tree_array-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_HIERARCHY_TREE_ARRAY_INL_H #define COAL_HIERARCHY_TREE_ARRAY_INL_H #include "coal/broadphase/detail/hierarchy_tree_array.h" #include #include namespace coal { namespace detail { namespace implementation_array { //============================================================================== template HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { root_node = NULL_NODE; n_nodes = 0; n_nodes_alloc = 16; nodes = new Node[n_nodes_alloc]; for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; n_leaves = 0; freelist = 0; opath = 0; max_lookahead_level = -1; bu_threshold = bu_threshold_; topdown_level = topdown_level_; } //============================================================================== template HierarchyTree::~HierarchyTree() { delete[] nodes; } //============================================================================== template void HierarchyTree::init(Node* leaves, int n_leaves_, int level) { switch (level) { case 0: init_0(leaves, n_leaves_); break; case 1: init_1(leaves, n_leaves_); break; case 2: init_2(leaves, n_leaves_); break; case 3: init_3(leaves, n_leaves_); break; default: init_0(leaves, n_leaves_); } } //============================================================================== template void HierarchyTree::init_0(Node* leaves, int n_leaves_) { clear(); n_leaves = (size_t)n_leaves_; root_node = NULL_NODE; nodes = new Node[n_leaves * 2]; std::copy(leaves, leaves + n_leaves, nodes); freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; size_t* ids = new size_t[n_leaves]; for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; root_node = topdown(ids, ids + n_leaves); delete[] ids; opath = 0; max_lookahead_level = -1; } //============================================================================== template void HierarchyTree::init_1(Node* leaves, int n_leaves_) { clear(); n_leaves = (size_t)n_leaves_; root_node = NULL_NODE; nodes = new Node[n_leaves * 2]; std::copy(leaves, leaves + n_leaves, nodes); freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; BV bound_bv; if (n_leaves > 0) bound_bv = nodes[0].bv; for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); size_t* ids = new size_t[n_leaves]; for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; const SortByMorton comp{nodes}; std::sort(ids, ids + n_leaves, comp); root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)), coder.bits() - 1); delete[] ids; refit(); opath = 0; max_lookahead_level = -1; } //============================================================================== template void HierarchyTree::init_2(Node* leaves, int n_leaves_) { clear(); n_leaves = (size_t)n_leaves_; root_node = NULL_NODE; nodes = new Node[n_leaves * 2]; std::copy(leaves, leaves + n_leaves, nodes); freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; BV bound_bv; if (n_leaves > 0) bound_bv = nodes[0].bv; for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); size_t* ids = new size_t[n_leaves]; for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; const SortByMorton comp{nodes}; std::sort(ids, ids + n_leaves, comp); root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)), coder.bits() - 1); delete[] ids; refit(); opath = 0; max_lookahead_level = -1; } //============================================================================== template void HierarchyTree::init_3(Node* leaves, int n_leaves_) { clear(); n_leaves = (size_t)n_leaves_; root_node = NULL_NODE; nodes = new Node[n_leaves * 2]; std::copy(leaves, leaves + n_leaves, nodes); freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; BV bound_bv; if (n_leaves > 0) bound_bv = nodes[0].bv; for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); size_t* ids = new size_t[n_leaves]; for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; const SortByMorton comp{nodes}; std::sort(ids, ids + n_leaves, comp); root_node = mortonRecurse_2(ids, ids + n_leaves); delete[] ids; refit(); opath = 0; max_lookahead_level = -1; } //============================================================================== template size_t HierarchyTree::insert(const BV& bv, void* data) { size_t node = createNode(NULL_NODE, bv, data); insertLeaf(root_node, node); ++n_leaves; return node; } //============================================================================== template void HierarchyTree::remove(size_t leaf) { removeLeaf(leaf); deleteNode(leaf); --n_leaves; } //============================================================================== template void HierarchyTree::clear() { delete[] nodes; root_node = NULL_NODE; n_nodes = 0; n_nodes_alloc = 16; nodes = new Node[n_nodes_alloc]; for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; n_leaves = 0; freelist = 0; opath = 0; max_lookahead_level = -1; } //============================================================================== template bool HierarchyTree::empty() const { return (n_nodes == 0); } //============================================================================== template void HierarchyTree::update(size_t leaf, int lookahead_level) { size_t root = removeLeaf(leaf); if (root != NULL_NODE) { if (lookahead_level > 0) { for (int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) root = nodes[root].parent; } else root = root_node; } insertLeaf(root, leaf); } //============================================================================== template bool HierarchyTree::update(size_t leaf, const BV& bv) { if (nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } //============================================================================== template bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3s& vel, Scalar margin) { COAL_UNUSED_VARIABLE(bv); COAL_UNUSED_VARIABLE(vel); COAL_UNUSED_VARIABLE(margin); if (nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } //============================================================================== template bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3s& vel) { COAL_UNUSED_VARIABLE(vel); if (nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } //============================================================================== template size_t HierarchyTree::getMaxHeight() const { if (root_node == NULL_NODE) return 0; return getMaxHeight(root_node); } //============================================================================== template size_t HierarchyTree::getMaxDepth() const { if (root_node == NULL_NODE) return 0; size_t max_depth; getMaxDepth(root_node, 0, max_depth); return max_depth; } //============================================================================== template void HierarchyTree::balanceBottomup() { if (root_node != NULL_NODE) { Node* leaves = new Node[n_leaves]; Node* leaves_ = leaves; extractLeaves(root_node, leaves_); root_node = NULL_NODE; std::copy(leaves, leaves + n_leaves, nodes); freelist = n_leaves; n_nodes = n_leaves; for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; size_t* ids = new size_t[n_leaves]; for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; bottomup(ids, ids + n_leaves); root_node = *ids; delete[] ids; } } //============================================================================== template void HierarchyTree::balanceTopdown() { if (root_node != NULL_NODE) { Node* leaves = new Node[n_leaves]; Node* leaves_ = leaves; extractLeaves(root_node, leaves_); root_node = NULL_NODE; std::copy(leaves, leaves + n_leaves, nodes); freelist = n_leaves; n_nodes = n_leaves; for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; size_t* ids = new size_t[n_leaves]; for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; root_node = topdown(ids, ids + n_leaves); delete[] ids; } } //============================================================================== template void HierarchyTree::balanceIncremental(int iterations) { if (iterations < 0) iterations = (int)n_leaves; if ((root_node != NULL_NODE) && (iterations > 0)) { for (int i = 0; i < iterations; ++i) { size_t node = root_node; unsigned int bit = 0; while (!nodes[node].isLeaf()) { node = nodes[node].children[(opath >> bit) & 1]; bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); } update(node); ++opath; } } } //============================================================================== template void HierarchyTree::refit() { if (root_node != NULL_NODE) recurseRefit(root_node); } //============================================================================== template void HierarchyTree::extractLeaves(size_t root, Node*& leaves) const { if (!nodes[root].isLeaf()) { extractLeaves(nodes[root].children[0], leaves); extractLeaves(nodes[root].children[1], leaves); } else { *leaves = nodes[root]; leaves++; } } //============================================================================== template size_t HierarchyTree::size() const { return n_leaves; } //============================================================================== template size_t HierarchyTree::getRoot() const { return root_node; } //============================================================================== template typename HierarchyTree::Node* HierarchyTree::getNodes() const { return nodes; } //============================================================================== template void HierarchyTree::print(size_t root, int depth) { for (int i = 0; i < depth; ++i) std::cout << " "; Node* n = nodes + root; std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl; if (n->isLeaf()) { } else { print(n->children[0], depth + 1); print(n->children[1], depth + 1); } } //============================================================================== template size_t HierarchyTree::getMaxHeight(size_t node) const { if (!nodes[node].isLeaf()) { size_t height1 = getMaxHeight(nodes[node].children[0]); size_t height2 = getMaxHeight(nodes[node].children[1]); return std::max(height1, height2) + 1; } else return 0; } //============================================================================== template void HierarchyTree::getMaxDepth(size_t node, size_t depth, size_t& max_depth) const { if (!nodes[node].isLeaf()) { getMaxDepth(nodes[node].children[0], depth + 1, max_depth); getmaxDepth(nodes[node].children[1], depth + 1, max_depth); } else max_depth = std::max(max_depth, depth); } //============================================================================== template void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) { size_t* lcur_end = lend; while (lbeg < lcur_end - 1) { size_t *min_it1 = nullptr, *min_it2 = nullptr; Scalar min_size = (std::numeric_limits::max)(); for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) { for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) { Scalar cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); if (cur_size < min_size) { min_size = cur_size; min_it1 = it1; min_it2 = it2; } } } size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr); nodes[p].children[0] = *min_it1; nodes[p].children[1] = *min_it2; nodes[*min_it1].parent = p; nodes[*min_it2].parent = p; *min_it1 = p; size_t tmp = *min_it2; lcur_end--; *min_it2 = *lcur_end; *lcur_end = tmp; } } //============================================================================== template size_t HierarchyTree::topdown(size_t* lbeg, size_t* lend) { switch (topdown_level) { case 0: return topdown_0(lbeg, lend); break; case 1: return topdown_1(lbeg, lend); break; default: return topdown_0(lbeg, lend); } } //============================================================================== template size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (num_leaves > bu_threshold) { BV vol = nodes[*lbeg].bv; for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv; size_t best_axis = 0; Scalar extent[3] = {vol.width(), vol.height(), vol.depth()}; if (extent[1] > extent[0]) best_axis = 1; if (extent[2] > extent[best_axis]) best_axis = 2; nodeBaseLess comp(nodes, best_axis); size_t* lcenter = lbeg + num_leaves / 2; std::nth_element(lbeg, lcenter, lend, comp); size_t node = createNode(NULL_NODE, vol, nullptr); nodes[node].children[0] = topdown_0(lbeg, lcenter); nodes[node].children[1] = topdown_0(lcenter, lend); nodes[nodes[node].children[0]].parent = node; nodes[nodes[node].children[1]].parent = node; return node; } else { bottomup(lbeg, lend); return *lbeg; } } return *lbeg; } //============================================================================== template size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (num_leaves > bu_threshold) { Vec3s split_p = nodes[*lbeg].bv.center(); BV vol = nodes[*lbeg].bv; for (size_t* i = lbeg + 1; i < lend; ++i) { split_p += nodes[*i].bv.center(); vol += nodes[*i].bv; } split_p /= static_cast(num_leaves); int best_axis = -1; int bestmidp = (int)num_leaves; int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; for (size_t* i = lbeg; i < lend; ++i) { Vec3s x = nodes[*i].bv.center() - split_p; for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; } for (size_t i = 0; i < 3; ++i) { if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { int midp = std::abs(splitcount[i][0] - splitcount[i][1]); if (midp < bestmidp) { best_axis = (int)i; bestmidp = midp; } } } if (best_axis < 0) best_axis = 0; Scalar split_value = split_p[best_axis]; size_t* lcenter = lbeg; for (size_t* i = lbeg; i < lend; ++i) { if (nodes[*i].bv.center()[best_axis] < split_value) { size_t temp = *i; *i = *lcenter; *lcenter = temp; ++lcenter; } } size_t node = createNode(NULL_NODE, vol, nullptr); nodes[node].children[0] = topdown_1(lbeg, lcenter); nodes[node].children[1] = topdown_1(lcenter, lend); nodes[nodes[node].children[0]].parent = node; nodes[nodes[node].children[1]].parent = node; return node; } else { bottomup(lbeg, lend); return *lbeg; } } return *lbeg; } //============================================================================== template size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split, int bits) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (bits > 0) { const SortByMorton comp{nodes, split}; size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); if (lcenter == lbeg) { uint32_t split2 = split | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split2, bits - 1); } else if (lcenter == lend) { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split1, bits - 1); } else { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); uint32_t split2 = split | (1 << (bits - 1)); size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); size_t node = createNode(NULL_NODE, nullptr); nodes[node].children[0] = child1; nodes[node].children[1] = child2; nodes[child1].parent = node; nodes[child2].parent = node; return node; } } else { size_t node = topdown(lbeg, lend); return node; } } else return *lbeg; } //============================================================================== template size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split, int bits) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (bits > 0) { const SortByMorton comp{nodes, split}; size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); if (lcenter == lbeg) { uint32_t split2 = split | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split2, bits - 1); } else if (lcenter == lend) { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split1, bits - 1); } else { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); uint32_t split2 = split | (1 << (bits - 1)); size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); size_t node = createNode(NULL_NODE, nullptr); nodes[node].children[0] = child1; nodes[node].children[1] = child2; nodes[child1].parent = node; nodes[child2].parent = node; return node; } } else { size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); size_t node = createNode(NULL_NODE, nullptr); nodes[node].children[0] = child1; nodes[node].children[1] = child2; nodes[child1].parent = node; nodes[child2].parent = node; return node; } } else return *lbeg; } //============================================================================== template size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) { long num_leaves = lend - lbeg; if (num_leaves > 1) { size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); size_t node = createNode(NULL_NODE, nullptr); nodes[node].children[0] = child1; nodes[node].children[1] = child2; nodes[child1].parent = node; nodes[child2].parent = node; return node; } else return *lbeg; } //============================================================================== template void HierarchyTree::insertLeaf(size_t root, size_t leaf) { if (root_node == NULL_NODE) { root_node = leaf; nodes[leaf].parent = NULL_NODE; } else { if (!nodes[root].isLeaf()) { do { root = nodes[root].children[select(leaf, nodes[root].children[0], nodes[root].children[1], nodes)]; } while (!nodes[root].isLeaf()); } size_t prev = nodes[root].parent; size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr); if (prev != NULL_NODE) { nodes[prev].children[indexOf(root)] = node; nodes[node].children[0] = root; nodes[root].parent = node; nodes[node].children[1] = leaf; nodes[leaf].parent = node; do { if (!nodes[prev].bv.contain(nodes[node].bv)) nodes[prev].bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; else break; node = prev; } while (NULL_NODE != (prev = nodes[node].parent)); } else { nodes[node].children[0] = root; nodes[root].parent = node; nodes[node].children[1] = leaf; nodes[leaf].parent = node; root_node = node; } } } //============================================================================== template size_t HierarchyTree::removeLeaf(size_t leaf) { if (leaf == root_node) { root_node = NULL_NODE; return NULL_NODE; } else { size_t parent = nodes[leaf].parent; size_t prev = nodes[parent].parent; size_t sibling = nodes[parent].children[1 - indexOf(leaf)]; if (prev != NULL_NODE) { nodes[prev].children[indexOf(parent)] = sibling; nodes[sibling].parent = prev; deleteNode(parent); while (prev != NULL_NODE) { BV new_bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; if (!(new_bv == nodes[prev].bv)) { nodes[prev].bv = new_bv; prev = nodes[prev].parent; } else break; } return (prev != NULL_NODE) ? prev : root_node; } else { root_node = sibling; nodes[sibling].parent = NULL_NODE; deleteNode(parent); return root_node; } } } //============================================================================== template void HierarchyTree::update_(size_t leaf, const BV& bv) { size_t root = removeLeaf(leaf); if (root != NULL_NODE) { if (max_lookahead_level >= 0) { for (int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) root = nodes[root].parent; } nodes[leaf].bv = bv; insertLeaf(root, leaf); } } //============================================================================== template size_t HierarchyTree::indexOf(size_t node) { return (nodes[nodes[node].parent].children[1] == node); } //============================================================================== template size_t HierarchyTree::allocateNode() { if (freelist == NULL_NODE) { Node* old_nodes = nodes; n_nodes_alloc *= 2; nodes = new Node[n_nodes_alloc]; std::copy(old_nodes, old_nodes + n_nodes, nodes); delete[] old_nodes; for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; freelist = n_nodes; } size_t node_id = freelist; freelist = nodes[node_id].next; nodes[node_id].parent = NULL_NODE; nodes[node_id].children[0] = NULL_NODE; nodes[node_id].children[1] = NULL_NODE; ++n_nodes; return node_id; } //============================================================================== template size_t HierarchyTree::createNode(size_t parent, const BV& bv1, const BV& bv2, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; nodes[node].bv = bv1 + bv2; return node; } //============================================================================== template size_t HierarchyTree::createNode(size_t parent, const BV& bv, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; nodes[node].bv = bv; return node; } //============================================================================== template size_t HierarchyTree::createNode(size_t parent, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; return node; } //============================================================================== template void HierarchyTree::deleteNode(size_t node) { nodes[node].next = freelist; freelist = node; --n_nodes; } //============================================================================== template void HierarchyTree::recurseRefit(size_t node) { if (!nodes[node].isLeaf()) { recurseRefit(nodes[node].children[0]); recurseRefit(nodes[node].children[1]); nodes[node].bv = nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; } else return; } //============================================================================== template void HierarchyTree::fetchLeaves(size_t root, Node*& leaves, int depth) { if ((!nodes[root].isLeaf()) && depth) { fetchLeaves(nodes[root].children[0], leaves, depth - 1); fetchLeaves(nodes[root].children[1], leaves, depth - 1); deleteNode(root); } else { *leaves = nodes[root]; leaves++; } } //============================================================================== template nodeBaseLess::nodeBaseLess(const NodeBase* nodes_, size_t d_) : nodes(nodes_), d(d_) { // Do nothing } //============================================================================== template bool nodeBaseLess::operator()(size_t i, size_t j) const { if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true; return false; } //============================================================================== template struct SelectImpl { static bool run(size_t query, size_t node1, size_t node2, NodeBase* nodes) { COAL_UNUSED_VARIABLE(query); COAL_UNUSED_VARIABLE(node1); COAL_UNUSED_VARIABLE(node2); COAL_UNUSED_VARIABLE(nodes); return 0; } static bool run(const BV& query, size_t node1, size_t node2, NodeBase* nodes) { COAL_UNUSED_VARIABLE(query); COAL_UNUSED_VARIABLE(node1); COAL_UNUSED_VARIABLE(node2); COAL_UNUSED_VARIABLE(nodes); return 0; } }; //============================================================================== template size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) { return SelectImpl::run(query, node1, node2, nodes); } //============================================================================== template size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes) { return SelectImpl::run(query, node1, node2, nodes); } //============================================================================== template struct SelectImpl { static bool run(size_t query, size_t node1, size_t node2, NodeBase* nodes) { const AABB& bv = nodes[query].bv; const AABB& bv1 = nodes[node1].bv; const AABB& bv2 = nodes[node2].bv; Vec3s v = bv.min_ + bv.max_; Vec3s v1 = v - (bv1.min_ + bv1.max_); Vec3s v2 = v - (bv2.min_ + bv2.max_); Scalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); Scalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } static bool run(const AABB& query, size_t node1, size_t node2, NodeBase* nodes) { const AABB& bv = query; const AABB& bv1 = nodes[node1].bv; const AABB& bv2 = nodes[node2].bv; Vec3s v = bv.min_ + bv.max_; Vec3s v1 = v - (bv1.min_ + bv1.max_); Vec3s v2 = v - (bv2.min_ + bv2.max_); Scalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); Scalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } }; } // namespace implementation_array } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/hierarchy_tree_array.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_HIERARCHY_TREE_ARRAY_H #define COAL_HIERARCHY_TREE_ARRAY_H #include #include #include #include "coal/fwd.hh" #include "coal/BV/AABB.h" #include "coal/broadphase/detail/morton.h" #include "coal/broadphase/detail/node_base_array.h" namespace coal { namespace detail { namespace implementation_array { /// @brief Class for hierarchy tree structure template class HierarchyTree { public: typedef NodeBase Node; private: struct SortByMorton { SortByMorton(Node* nodes_in) : nodes(nodes_in) {} SortByMorton(Node* nodes_in, uint32_t split_in) : nodes(nodes_in), split(split_in) {} bool operator()(size_t a, size_t b) const { if ((a != NULL_NODE) && (b != NULL_NODE)) return nodes[a].code < nodes[b].code; else if (a == NULL_NODE) return split < nodes[b].code; else if (b == NULL_NODE) return nodes[a].code < split; return false; } Node* nodes{}; uint32_t split{}; }; public: /// @brief Create hierarchy tree with suitable setting. /// bu_threshold decides the height of tree node to start bottom-up /// construction / optimization; topdown_level decides different methods to /// construct tree in topdown manner. lower level method constructs tree with /// better quality but is slower. HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); /// @brief Initialize the tree by a set of leaves using algorithm with a given /// level. void init(Node* leaves, int n_leaves_, int level = 0); /// @brief Initialize the tree by a set of leaves using algorithm with a given /// level. size_t insert(const BV& bv, void* data); /// @brief Remove a leaf node void remove(size_t leaf); /// @brief Clear the tree void clear(); /// @brief Whether the tree is empty bool empty() const; /// @brief update one leaf node void update(size_t leaf, int lookahead_level = -1); /// @brief update the tree when the bounding volume of a given leaf has /// changed bool update(size_t leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vec3s& vel, Scalar margin); /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vec3s& vel); /// @brief get the max height of the tree size_t getMaxHeight() const; /// @brief get the max depth of the tree size_t getMaxDepth() const; /// @brief balance the tree from bottom void balanceBottomup(); /// @brief balance the tree from top void balanceTopdown(); /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, /// update the entire tree in a bottom-up manner void refit(); /// @brief extract all the leaves of the tree void extractLeaves(size_t root, Node*& leaves) const; /// @brief number of leaves in the tree size_t size() const; /// @brief get the root of the tree size_t getRoot() const; /// @brief get the pointer to the nodes array Node* getNodes() const; /// @brief print the tree in a recursive way void print(size_t root, int depth); private: /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(size_t* lbeg, size_t* lend); /// @brief construct a tree for a set of leaves from top size_t topdown(size_t* lbeg, size_t* lend); /// @brief compute the maximum height of a subtree rooted from a given node size_t getMaxHeight(size_t node) const; /// @brief compute the maximum depth of a subtree rooted from a given node void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const; /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a /// topdown manner. During construction, first compute the best split axis as /// the axis along with the longest AABB edge. Then compute the median of all /// nodes' center projection onto the axis and using it as the split /// threshold. size_t topdown_0(size_t* lbeg, size_t* lend); /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a /// topdown manner. During construction, first compute the best split /// thresholds for different axes as the average of all nodes' center. Then /// choose the split axis as the axis whose threshold can divide the nodes /// into two parts with almost similar size. This construction is more /// expensive then topdown_0, but also can provide tree with better quality. size_t topdown_1(size_t* lbeg, size_t* lend); /// @brief init tree from leaves in the topdown manner (topdown_0 or /// topdown_1) void init_0(Node* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., /// for nodes which is of depth more than the maximum bits of the morton code, /// we use bottomup method to construct the subtree, which is slow but can /// construct tree with high quality. void init_1(Node* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., /// for nodes which is of depth more than the maximum bits of the morton code, /// we split the leaves into two parts with the same size simply using the /// node index. void init_2(Node* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., /// for all nodes, we simply divide the leaves into parts with the same size /// simply using the node index. void init_3(Node* leaves, int n_leaves_); size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split, int bits); size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split, int bits); size_t mortonRecurse_2(size_t* lbeg, size_t* lend); /// @brief update one leaf node's bounding volume void update_(size_t leaf, const BV& bv); /// @brief Insert a leaf node and also update its ancestors void insertLeaf(size_t root, size_t leaf); /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the /// unnecessary internal nodes are deleted. return the node with the smallest /// depth and is influenced by the remove operation size_t removeLeaf(size_t leaf); /// @brief Delete all internal nodes and return all leaves nodes with given /// depth from root void fetchLeaves(size_t root, Node*& leaves, int depth = -1); size_t indexOf(size_t node); size_t allocateNode(); /// @brief create one node (leaf or internal) size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data); size_t createNode(size_t parent, const BV& bv, void* data); size_t createNode(size_t parent, void* data); void deleteNode(size_t node); void recurseRefit(size_t node); protected: size_t root_node; Node* nodes; size_t n_nodes; size_t n_nodes_alloc; size_t n_leaves; size_t freelist; unsigned int opath; int max_lookahead_level; public: /// @brief decide which topdown algorithm to use int topdown_level; /// @brief decide the depth to use expensive bottom-up algorithm int bu_threshold; public: static const size_t NULL_NODE = std::numeric_limits::max(); }; template const size_t HierarchyTree::NULL_NODE; /// @brief Functor comparing two nodes template struct nodeBaseLess { nodeBaseLess(const NodeBase* nodes_, size_t d_); bool operator()(size_t i, size_t j) const; private: /// @brief the nodes array const NodeBase* nodes; /// @brief the dimension to compare size_t d; }; /// @brief select the node from node1 and node2 which is close to the query-th /// node in the nodes. 0 for node1 and 1 for node2. template size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes); /// @brief select the node from node1 and node2 which is close to the query /// AABB. 0 for node1 and 1 for node2. template size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes); } // namespace implementation_array } // namespace detail } // namespace coal #include "coal/broadphase/detail/hierarchy_tree_array-inl.h" #endif ================================================ FILE: include/coal/broadphase/detail/interval_tree.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_INTERVAL_TREE_H #define COAL_INTERVAL_TREE_H #include #include #include #include "coal/broadphase/detail/interval_tree_node.h" namespace coal { namespace detail { /// @brief Class describes the information needed when we take the /// right branch in searching for intervals but possibly come back /// and check the left branch as well. struct COAL_DLLAPI it_recursion_node { public: IntervalTreeNode* start_node; unsigned int parent_index; bool try_right_branch; }; /// @brief Interval tree class COAL_DLLAPI IntervalTree { public: IntervalTree(); ~IntervalTree(); /// @brief Print the whole interval tree void print() const; /// @brief Delete one node of the interval tree SimpleInterval* deleteNode(IntervalTreeNode* node); /// @brief delete node stored a given interval void deleteNode(SimpleInterval* ivl); /// @brief Insert one node of the interval tree IntervalTreeNode* insert(SimpleInterval* new_interval); /// @brief get the predecessor of a given node IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const; /// @brief Get the successor of a given node IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const; /// @brief Return result for a given query std::deque query(Scalar low, Scalar high); protected: IntervalTreeNode* root; IntervalTreeNode* invalid_node; /// @brief left rotation of tree node void leftRotate(IntervalTreeNode* node); /// @brief right rotation of tree node void rightRotate(IntervalTreeNode* node); /// @brief Inserts node into the tree as if it were a regular binary tree void recursiveInsert(IntervalTreeNode* node); /// @brief recursively print a subtree void recursivePrint(IntervalTreeNode* node) const; /// @brief recursively find the node corresponding to the interval IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const; /// @brief Travels up to the root fixing the max_high fields after an /// insertion or deletion void fixupMaxHigh(IntervalTreeNode* node); void deleteFixup(IntervalTreeNode* node); private: unsigned int recursion_node_stack_size; it_recursion_node* recursion_node_stack; unsigned int current_parent; unsigned int recursion_node_stack_top; }; } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/interval_tree_node-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H #define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H #include "coal/broadphase/detail/interval_tree_node.h" #include #include namespace coal { namespace detail { //============================================================================== IntervalTreeNode::IntervalTreeNode() { // Do nothing } //============================================================================== IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) : stored_interval(new_interval), key(new_interval->low), high(new_interval->high), max_high(high) { // Do nothing } //============================================================================== IntervalTreeNode::~IntervalTreeNode() { // Do nothing } //============================================================================== void IntervalTreeNode::print(IntervalTreeNode* nil, IntervalTreeNode* root) const { stored_interval->print(); std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high; std::cout << " l->key = "; if (left == nil) std::cout << "nullptr"; else std::cout << left->key; std::cout << " r->key = "; if (right == nil) std::cout << "nullptr"; else std::cout << right->key; std::cout << " p->key = "; if (parent == root) std::cout << "nullptr"; else std::cout << parent->key; std::cout << " red = " << (int)red << std::endl; } } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/interval_tree_node.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H #define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H #include "coal/broadphase/detail/simple_interval.h" #include "coal/fwd.hh" namespace coal { namespace detail { class COAL_DLLAPI IntervalTree; /// @brief The node for interval tree class COAL_DLLAPI IntervalTreeNode { public: friend class IntervalTree; /// @brief Create an empty node IntervalTreeNode(); /// @brief Create an node storing the interval IntervalTreeNode(SimpleInterval* new_interval); ~IntervalTreeNode(); /// @brief Print the interval node information: set left = invalid_node and /// right = root void print(IntervalTreeNode* left, IntervalTreeNode* right) const; protected: /// @brief interval stored in the node SimpleInterval* stored_interval; Scalar key; Scalar high; Scalar max_high; /// @brief red or black node: if red = false then the node is black bool red; IntervalTreeNode* left; IntervalTreeNode* right; IntervalTreeNode* parent; }; } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/morton-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * Copyright (c) 2016, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_MORTON_INL_H #define COAL_MORTON_INL_H #include "coal/broadphase/detail/morton.h" namespace coal { /// @cond IGNORE namespace detail { //============================================================================== template uint32_t quantize(S x, uint32_t n) { return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0)); } //============================================================================== template morton_functor::morton_functor(const AABB& bbox) : base(bbox.min_), inv(Scalar(1) / (bbox.max_[0] - bbox.min_[0]), Scalar(1) / (bbox.max_[1] - bbox.min_[1]), Scalar(1) / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== template uint32_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); return detail::morton_code(x, y, z); } //============================================================================== template morton_functor::morton_functor(const AABB& bbox) : base(bbox.min_), inv(Scalar(1) / (bbox.max_[0] - bbox.min_[0]), Scalar(1) / (bbox.max_[1] - bbox.min_[1]), Scalar(1) / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== template uint64_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); return detail::morton_code60(x, y, z); } //============================================================================== template constexpr size_t morton_functor::bits() { return 60; } //============================================================================== template constexpr size_t morton_functor::bits() { return 30; } //============================================================================== template morton_functor>::morton_functor(const AABB& bbox) : base(bbox.min_), inv(Scalar(1) / (bbox.max_[0] - bbox.min_[0]), Scalar(1) / (bbox.max_[1] - bbox.min_[1]), Scalar(1) / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== template std::bitset morton_functor>::operator()( const Vec3s& point) const { S x = (point[0] - base[0]) * inv[0]; S y = (point[1] - base[1]) * inv[1]; S z = (point[2] - base[2]) * inv[2]; int start_bit = bits() - 1; std::bitset bset; x *= 2; y *= 2; z *= 2; for (size_t i = 0; i < bits() / 3; ++i) { bset[start_bit--] = ((z < 1) ? 0 : 1); bset[start_bit--] = ((y < 1) ? 0 : 1); bset[start_bit--] = ((x < 1) ? 0 : 1); x = ((x >= 1) ? 2 * (x - 1) : 2 * x); y = ((y >= 1) ? 2 * (y - 1) : 2 * y); z = ((z >= 1) ? 2 * (z - 1) : 2 * z); } return bset; } //============================================================================== template constexpr size_t morton_functor>::bits() { return N; } } // namespace detail /// @endcond } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/morton.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * Copyright (c) 2016, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_MORTON_H #define COAL_MORTON_H #include "coal/BV/AABB.h" #include #include namespace coal { /// @cond IGNORE namespace detail { template uint32_t quantize(S x, uint32_t n); /// @brief compute 30 bit morton code COAL_DLLAPI uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z); /// @brief compute 60 bit morton code COAL_DLLAPI uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z); /// @brief Functor to compute the morton code for a given AABB /// This is specialized for 32- and 64-bit unsigned integers giving /// a 30- or 60-bit code, respectively, and for `std::bitset` where /// N is the length of the code and must be a multiple of 3. template struct morton_functor {}; /// @brief Functor to compute 30 bit morton code for a given AABB template struct morton_functor { morton_functor(const AABB& bbox); uint32_t operator()(const Vec3s& point) const; const Vec3s base; const Vec3s inv; static constexpr size_t bits(); }; using morton_functoru32f = morton_functor; using morton_functoru32d = morton_functor; /// @brief Functor to compute 60 bit morton code for a given AABB template struct morton_functor { morton_functor(const AABB& bbox); uint64_t operator()(const Vec3s& point) const; const Vec3s base; const Vec3s inv; static constexpr size_t bits(); }; /// @brief Functor to compute N bit morton code for a given AABB /// N must be a multiple of 3. template struct morton_functor> { static_assert(N % 3 == 0, "Number of bits must be a multiple of 3"); morton_functor(const AABB& bbox); std::bitset operator()(const Vec3s& point) const; const Vec3s base; const Vec3s inv; static constexpr size_t bits(); }; } // namespace detail /// @endcond } // namespace coal #include "coal/broadphase/detail/morton-inl.h" #endif ================================================ FILE: include/coal/broadphase/detail/node_base-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_NODEBASE_INL_H #define COAL_BROADPHASE_DETAIL_NODEBASE_INL_H #include "coal/broadphase/detail/node_base.h" namespace coal { namespace detail { //============================================================================// // // // Implementations // // // //============================================================================// //============================================================================== template bool NodeBase::isLeaf() const { return (children[1] == nullptr); } //============================================================================== template bool NodeBase::isInternal() const { return !isLeaf(); } //============================================================================== template NodeBase::NodeBase() { parent = nullptr; children[0] = nullptr; children[1] = nullptr; } } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/node_base.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_NODEBASE_H #define COAL_BROADPHASE_DETAIL_NODEBASE_H #include "coal/data_types.h" namespace coal { namespace detail { /// @brief dynamic AABB tree node template struct NodeBase { /// @brief the bounding volume for the node BV bv; /// @brief pointer to parent node NodeBase* parent; /// @brief whether is a leaf bool isLeaf() const; /// @brief whether is internal node bool isInternal() const; union { /// @brief for leaf node, children nodes NodeBase* children[2]; void* data; }; /// @brief morton code for current BV uint32_t code; NodeBase(); }; } // namespace detail } // namespace coal #include "coal/broadphase/detail/node_base-inl.h" #endif ================================================ FILE: include/coal/broadphase/detail/node_base_array-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H #define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H #include "coal/broadphase/detail/node_base_array.h" namespace coal { namespace detail { namespace implementation_array { //============================================================================== template bool NodeBase::isLeaf() const { return (children[1] == (size_t)(-1)); } //============================================================================== template bool NodeBase::isInternal() const { return !isLeaf(); } } // namespace implementation_array } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/node_base_array.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H #define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H #include "coal/data_types.h" namespace coal { namespace detail { namespace implementation_array { template struct NodeBase { BV bv; union { size_t parent; size_t next; }; union { size_t children[2]; void* data; }; uint32_t code; bool isLeaf() const; bool isInternal() const; }; } // namespace implementation_array } // namespace detail } // namespace coal #include "coal/broadphase/detail/node_base_array-inl.h" #endif ================================================ FILE: include/coal/broadphase/detail/simple_hash_table-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H #define COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H #include "coal/broadphase/detail/simple_hash_table.h" #include namespace coal { namespace detail { //============================================================================== template SimpleHashTable::SimpleHashTable(const HashFnc& h) : h_(h) { // Do nothing } //============================================================================== template void SimpleHashTable::init(size_t size) { if (size == 0) { COAL_THROW_PRETTY("SimpleHashTable must have non-zero size.", std::logic_error); } table_.resize(size); table_size_ = size; } //============================================================================== template void SimpleHashTable::insert(Key key, Data value) { std::vector indices = h_(key); size_t range = table_.size(); for (size_t i = 0; i < indices.size(); ++i) table_[indices[i] % range].push_back(value); } //============================================================================== template std::vector SimpleHashTable::query(Key key) const { size_t range = table_.size(); std::vector indices = h_(key); std::set result; for (size_t i = 0; i < indices.size(); ++i) { size_t index = indices[i] % range; std::copy(table_[index].begin(), table_[index].end(), std::inserter(result, result.end())); } return std::vector(result.begin(), result.end()); } //============================================================================== template void SimpleHashTable::remove(Key key, Data value) { size_t range = table_.size(); std::vector indices = h_(key); for (size_t i = 0; i < indices.size(); ++i) { size_t index = indices[i] % range; table_[index].remove(value); } } //============================================================================== template void SimpleHashTable::clear() { table_.clear(); table_.resize(table_size_); } } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/simple_hash_table.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_H #define COAL_BROADPHASE_SIMPLEHASHTABLE_H #include #include #include namespace coal { namespace detail { /// @brief A simple hash table implemented as multiple buckets. HashFnc is any /// extended hash function: HashFnc(key) = {index1, index2, ..., } template class SimpleHashTable { protected: typedef std::list Bin; std::vector table_; HashFnc h_; size_t table_size_; public: SimpleHashTable(const HashFnc& h); /// @brief Init the number of bins in the hash table void init(size_t size); //// @brief Insert a key-value pair into the table void insert(Key key, Data value); /// @brief Find the elements in the hash table whose key is the same as query /// key. std::vector query(Key key) const; /// @brief remove the key-value pair from the table void remove(Key key, Data value); /// @brief clear the hash table void clear(); }; } // namespace detail } // namespace coal #include "coal/broadphase/detail/simple_hash_table-inl.h" #endif ================================================ FILE: include/coal/broadphase/detail/simple_interval-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H #define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H #include "coal/broadphase/detail/simple_interval.h" #include namespace coal { namespace detail { //============================================================================== SimpleInterval::~SimpleInterval() { // Do nothing } //============================================================================== void SimpleInterval::print() { // Do nothing } } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/simple_interval.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H #define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H #include "coal/fwd.hh" #include "coal/data_types.h" namespace coal { namespace detail { /// @brief Interval trees implemented using red-black-trees as described in /// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest. struct COAL_DLLAPI SimpleInterval { public: virtual ~SimpleInterval(); virtual void print(); /// @brief interval is defined as [low, high] Scalar low, high; }; } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/sparse_hash_table-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_SPARSEHASHTABLE_INL_H #define COAL_BROADPHASE_SPARSEHASHTABLE_INL_H #include "coal/broadphase/detail/sparse_hash_table.h" namespace coal { namespace detail { //============================================================================== template class TableT> SparseHashTable::SparseHashTable(const HashFnc& h) : h_(h) { // Do nothing } //============================================================================== template class TableT> void SparseHashTable::init(size_t) { table_.clear(); } //============================================================================== template class TableT> void SparseHashTable::insert(Key key, Data value) { std::vector indices = h_(key); for (size_t i = 0; i < indices.size(); ++i) table_[indices[i]].push_back(value); } //============================================================================== template class TableT> std::vector SparseHashTable::query( Key key) const { std::vector indices = h_(key); std::set result; for (size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i]; typename Table::const_iterator p = table_.find(index); if (p != table_.end()) { std::copy((*p).second.begin(), (*p).second.end(), std ::inserter(result, result.end())); } } return std::vector(result.begin(), result.end()); } //============================================================================== template class TableT> void SparseHashTable::remove(Key key, Data value) { std::vector indices = h_(key); for (size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i]; table_[index].remove(value); } } //============================================================================== template class TableT> void SparseHashTable::clear() { table_.clear(); } } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/sparse_hash_table.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_SPARSEHASHTABLE_H #define COAL_BROADPHASE_SPARSEHASHTABLE_H #include #include #include #include namespace coal { namespace detail { template class unordered_map_hash_table : public std::unordered_map { typedef std::unordered_map Base; public: unordered_map_hash_table() : Base() {}; }; /// @brief A hash table implemented using unordered_map template class TableT = unordered_map_hash_table> class SparseHashTable { protected: HashFnc h_; typedef std::list Bin; typedef TableT Table; Table table_; public: SparseHashTable(const HashFnc& h); /// @brief Init the hash table. The bucket size is dynamically decided. void init(size_t); /// @brief insert one key-value pair into the hash table void insert(Key key, Data value); /// @brief find the elements whose key is the same as the query std::vector query(Key key) const; /// @brief remove one key-value pair from the hash table void remove(Key key, Data value); /// @brief clear the hash table void clear(); }; } // namespace detail } // namespace coal #include "coal/broadphase/detail/sparse_hash_table-inl.h" #endif ================================================ FILE: include/coal/broadphase/detail/spatial_hash-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_SPATIALHASH_INL_H #define COAL_BROADPHASE_SPATIALHASH_INL_H #include "coal/broadphase/detail/spatial_hash.h" #include namespace coal { namespace detail { //============================================================================== SpatialHash::SpatialHash(const AABB& scene_limit_, Scalar cell_size_) : cell_size(cell_size_), scene_limit(scene_limit_) { width[0] = std::ceil(scene_limit.width() / cell_size); width[1] = std::ceil(scene_limit.height() / cell_size); width[2] = std::ceil(scene_limit.depth() / cell_size); } //============================================================================== std::vector SpatialHash::operator()(const AABB& aabb) const { int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size); int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size); int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size); int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size); int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size); int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size); std::vector keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z)); int id = 0; for (int x = min_x; x < max_x; ++x) { for (int y = min_y; y < max_y; ++y) { for (int z = min_z; z < max_z; ++z) { keys[id++] = x + y * width[0] + z * width[0] * width[1]; } } } return keys; } } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/broadphase/detail/spatial_hash.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_SPATIALHASH_H #define COAL_BROADPHASE_SPATIALHASH_H #include "coal/BV/AABB.h" #include namespace coal { namespace detail { /// @brief Spatial hash function: hash an AABB to a set of integer values struct COAL_DLLAPI SpatialHash { SpatialHash(const AABB& scene_limit_, Scalar cell_size_); std::vector operator()(const AABB& aabb) const; private: Scalar cell_size; AABB scene_limit; unsigned int width[3]; }; } // namespace detail } // namespace coal #endif ================================================ FILE: include/coal/collision.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2021, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_COLLISION_H #define COAL_COLLISION_H #include "coal/data_types.h" #include "coal/collision_object.h" #include "coal/collision_data.h" #include "coal/collision_func_matrix.h" #include "coal/timings.h" namespace coal { /// @brief Main collision interface: given two collision objects, and the /// requirements for contacts, including num of max contacts, whether perform /// exhaustive collision (i.e., returning returning all the contact points), /// whether return detailed contact information (i.e., normal, contact point, /// depth; otherwise only contact primitive id is returned), this function /// performs the collision between them. /// Return value is the number of contacts generated between the two objects. COAL_DLLAPI std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const CollisionRequest& request, CollisionResult& result); /// @copydoc collide(const CollisionObject*, const CollisionObject*, const /// CollisionRequest&, CollisionResult&) COAL_DLLAPI std::size_t collide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result); /// @brief This class reduces the cost of identifying the geometry pair. /// This is mostly useful for repeated shape-shape queries. /// /// \code /// ComputeCollision calc_collision (o1, o2); /// std::size_t ncontacts = calc_collision(tf1, tf2, request, result); /// \endcode class COAL_DLLAPI ComputeCollision { public: /// @brief Default constructor from two Collision Geometries. ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2); std::size_t operator()(const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const; bool operator==(const ComputeCollision& other) const { COAL_EQUAL_OPERATOR_CHECK(((o1 == nullptr && other.o1 == nullptr) || (o1 != nullptr && other.o1 != nullptr))); COAL_EQUAL_OPERATOR_CHECK(((o2 == nullptr && other.o2 == nullptr) || (o2 != nullptr && other.o2 != nullptr))); if ((o1 && other.o1) && (o1 != other.o1)) COAL_EQUAL_OPERATOR_CHECK(*o1 == *other.o1); if ((o2 && other.o2) && (o2 != other.o2)) COAL_EQUAL_OPERATOR_CHECK(*o2 == *other.o2); COAL_EQUAL_OPERATOR_CHECK(solver == other.solver); COAL_EQUAL_OPERATOR_CHECK(func == other.func); return true; } bool operator!=(const ComputeCollision& other) const { return !(*this == other); } virtual ~ComputeCollision() {}; protected: // These pointers are made mutable to let the derived classes to update // their values when updating the collision geometry (e.g. creating a new // one). This feature should be used carefully to avoid any mis usage (e.g, // changing the type of the collision geometry should be avoided). mutable const CollisionGeometry* o1; mutable const CollisionGeometry* o2; mutable GJKSolver solver; CollisionFunctionMatrix::CollisionFunc func; bool swap_geoms; virtual std::size_t run(const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace coal #endif ================================================ FILE: include/coal/collision_data.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_COLLISION_DATA_H #define COAL_COLLISION_DATA_H #include #include #include #include #include #include "coal/collision_object.h" #include "coal/config.hh" #include "coal/data_types.h" #include "coal/timings.h" #include "coal/narrowphase/narrowphase_defaults.h" #include "coal/logging.h" #include "coal/collision_utility.h" namespace coal { /// @brief Contact information returned by collision struct COAL_DLLAPI Contact { /// @brief collision object 1 const CollisionGeometry* o1; /// @brief collision object 2 const CollisionGeometry* o2; /// @brief contact primitive in object 1 /// if object 1 is mesh or point cloud, it is the triangle or point id /// if object 1 is geometry shape, it is NONE (-1), /// if object 1 is octree, it is the id of the cell int b1; /// @brief contact primitive in object 2 /// if object 2 is mesh or point cloud, it is the triangle or point id /// if object 2 is geometry shape, it is NONE (-1), /// if object 2 is octree, it is the id of the cell int b2; /// @brief contact normal, pointing from o1 to o2. /// The normal defined as the normalized separation vector: /// normal = (p2 - p1) / dist(o1, o2), where p1 = nearest_points[0] /// belongs to o1 and p2 = nearest_points[1] belongs to o2 and dist(o1, o2) is /// the **signed** distance between o1 and o2. The normal always points from /// o1 to o2. /// @note The separation vector is the smallest vector such that if o1 is /// translated by it, o1 and o2 are in touching contact (they share at least /// one contact point but have a zero intersection volume). If the shapes /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) = /// (p2-p1).norm(). Vec3s normal; /// @brief nearest points associated to this contact. /// @note Also referred as "witness points" in other collision libraries. /// The points p1 = nearest_points[0] and p2 = nearest_points[1] verify the /// property that dist(o1, o2) * (p1 - p2) is the separation vector between o1 /// and o2, with dist(o1, o2) being the **signed** distance separating o1 from /// o2. See \ref DistanceResult::normal for the definition of the separation /// vector. If o1 and o2 have multiple contacts, the nearest_points are /// associated with the contact which has the greatest penetration depth. /// TODO (louis): rename `nearest_points` to `witness_points`. std::array nearest_points; /// @brief contact position, in world space Vec3s pos; /// @brief penetration depth Scalar penetration_depth; /// @brief invalid contact primitive information static const int NONE = -1; /// @brief Default constructor Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) { penetration_depth = (std::numeric_limits::max)(); nearest_points[0] = nearest_points[1] = normal = pos = Vec3s::Constant(std::numeric_limits::max()); } /// @brief Copy constructor. /// This constructor does a raw copy of the contact information, including the /// pointers to the collision geometries. Contact(const Contact& other) = default; Contact& operator=(const Contact& other) = default; /// @brief Copy constructor using an other contact and a pair of /// CollisionGeometry pointers. This constructor allows to copy the contact /// information from an other contact, but remap the pointers to the collision /// geometries to the provided ones. This is usefull in a deep-copy context, /// when the collision geometries are also copied and the pointers in the /// contact need to be remapped to the new collision geometries. Contact( const Contact& other, std::pair new_o1_o2) : Contact(other) { resolveReferences(new_o1_o2); } Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_), o2(o2_), b1(b1_), b2(b2_) { penetration_depth = (std::numeric_limits::max)(); nearest_points[0] = nearest_points[1] = normal = pos = Vec3s::Constant(std::numeric_limits::max()); } Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& pos_, const Vec3s& normal_, Scalar depth_) : o1(o1_), o2(o2_), b1(b1_), b2(b2_), normal(normal_), nearest_points{pos_ - (depth_ * normal_ / 2), pos_ + (depth_ * normal_ / 2)}, pos(pos_), penetration_depth(depth_) {} Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_, Scalar depth_) : o1(o1_), o2(o2_), b1(b1_), b2(b2_), normal(normal_), nearest_points{p1, p2}, pos((p1 + p2) / 2), penetration_depth(depth_) {} bool operator<(const Contact& other) const { if (b1 == other.b1) return b2 < other.b2; return b1 < other.b1; } /// @brief Equality operator. Two contacts are considered equal if they have /// the same normal, the same nearest points and the same penetration depth /// and if the shapes they point to are equal. /// Note: two contacts may be equal even if they point to different collision /// geometries o1/o2, as long as these geometries are equal. bool operator==(const Contact& other) const { if (this == &other) return true; COAL_EQUAL_OPERATOR_CHECK(((o1 == nullptr && other.o1 == nullptr) || (o1 != nullptr && other.o1 != nullptr))); COAL_EQUAL_OPERATOR_CHECK(((o2 == nullptr && other.o2 == nullptr) || (o2 != nullptr && other.o2 != nullptr))); if ((o1 && other.o1) && (o1 != other.o1)) COAL_EQUAL_OPERATOR_CHECK(*o1 == *other.o1); if ((o2 && other.o2) && (o2 != other.o2)) COAL_EQUAL_OPERATOR_CHECK(*o2 == *other.o2); COAL_EQUAL_OPERATOR_CHECK(b1 == other.b1); COAL_EQUAL_OPERATOR_CHECK(b2 == other.b2); COAL_EQUAL_OPERATOR_CHECK(normal == other.normal); COAL_EQUAL_OPERATOR_CHECK(pos == other.pos); COAL_EQUAL_OPERATOR_CHECK(nearest_points[0] == other.nearest_points[0]); COAL_EQUAL_OPERATOR_CHECK(nearest_points[1] == other.nearest_points[1]); COAL_EQUAL_OPERATOR_CHECK(penetration_depth == other.penetration_depth); return true; } /// @brief Inequality operator. Negation of the equality operator. bool operator!=(const Contact& other) const { return !(*this == other); } /// @brief Returns the distance to collision: penetration depth - security /// margin. Scalar getDistanceToCollision(const CollisionRequest& request) const; /// @brief Resolve internal references to the pair of collision geometries. /// This is useful when deserializing a contact, as the pointers to the /// collision objects are not valid anymore and need to be remapped to the /// collision objects in the current context. void resolveReferences( std::pair new_o1_o2) { o1 = new_o1_o2.first; o2 = new_o1_o2.second; } /// \brief Prints the contact to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; os << prefix << " o1 type: " << (o1 ? get_node_type_name(o1->getNodeType()) : "null") << ",\n"; os << prefix << " o2 type: " << (o2 ? get_node_type_name(o2->getNodeType()) : "null") << ",\n"; os << prefix << " b1: " << b1 << ",\n"; os << prefix << " b2: " << b2 << ",\n"; os << prefix << " normal: " << normal.transpose() << ",\n"; os << prefix << " nearest_points[0]: " << nearest_points[0].transpose() << ",\n"; os << prefix << " nearest_points[1]: " << nearest_points[1].transpose() << ",\n"; os << prefix << " pos: " << pos.transpose() << ",\n"; os << prefix << " penetration_depth: " << penetration_depth << ",\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const Contact& contact) { os << "Contact: {" << "\n"; contact.disp(os); os << "}" << "\n"; return os; } }; struct QueryResult; /// @brief base class for all query requests struct COAL_DLLAPI QueryRequest { // @brief Initial guess to use for the GJK algorithm GJKInitialGuess gjk_initial_guess; /// @brief whether enable gjk initial guess /// @Deprecated Use gjk_initial_guess instead COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) bool enable_cached_gjk_guess; /// @brief the gjk initial guess set by user mutable Vec3s cached_gjk_guess; /// @brief the support function initial guess set by user mutable support_func_guess_t cached_support_func_guess; /// @brief maximum iteration for the GJK algorithm size_t gjk_max_iterations; /// @brief tolerance for the GJK algorithm. /// Note: This tolerance determines the precision on the estimated distance /// between two geometries which are not in collision. /// It is recommended to not set this tolerance to less than 1e-6. Scalar gjk_tolerance; /// @brief whether to enable the Nesterov accleration of GJK GJKVariant gjk_variant; /// @brief convergence criterion used to stop GJK GJKConvergenceCriterion gjk_convergence_criterion; /// @brief convergence criterion used to stop GJK GJKConvergenceCriterionType gjk_convergence_criterion_type; /// @brief max number of iterations for EPA size_t epa_max_iterations; /// @brief tolerance for EPA. /// Note: This tolerance determines the precision on the estimated distance /// between two geometries which are in collision. /// It is recommended to not set this tolerance to less than 1e-6. /// Also, setting EPA's tolerance to less than GJK's is not recommended. Scalar epa_tolerance; /// @brief enable timings when performing collision/distance request bool enable_timings; /// @brief threshold below which a collision is considered. Scalar collision_distance_threshold; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS /// @brief Default constructor. QueryRequest() : gjk_initial_guess(GJKInitialGuess::DefaultGuess), enable_cached_gjk_guess(false), cached_gjk_guess(1, 0, 0), cached_support_func_guess(support_func_guess_t::Zero()), gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS), gjk_tolerance(GJK_DEFAULT_TOLERANCE), gjk_variant(GJKVariant::DefaultGJK), gjk_convergence_criterion(GJKConvergenceCriterion::Default), gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative), epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS), epa_tolerance(EPA_DEFAULT_TOLERANCE), enable_timings(false), collision_distance_threshold( Eigen::NumTraits::dummy_precision()) {} /// @brief Copy constructor. QueryRequest(const QueryRequest& other) = default; /// @brief Copy assignment operator. QueryRequest& operator=(const QueryRequest& other) = default; COAL_COMPILER_DIAGNOSTIC_POP /// @brief Updates the guess for the internal GJK algorithm in order to /// warm-start it when reusing this collision request on the same collision /// pair. /// @note The option `gjk_initial_guess` must be set to /// `GJKInitialGuess::CachedGuess` for this to work. void updateGuess(const QueryResult& result) const; /// @brief whether two QueryRequest are the same or not inline bool operator==(const QueryRequest& other) const { if (this == &other) return true; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS COAL_EQUAL_OPERATOR_CHECK(gjk_initial_guess == other.gjk_initial_guess); COAL_EQUAL_OPERATOR_CHECK(enable_cached_gjk_guess == other.enable_cached_gjk_guess); COAL_EQUAL_OPERATOR_CHECK(gjk_variant == other.gjk_variant); COAL_EQUAL_OPERATOR_CHECK(gjk_convergence_criterion == other.gjk_convergence_criterion); COAL_EQUAL_OPERATOR_CHECK(gjk_convergence_criterion_type == other.gjk_convergence_criterion_type); COAL_EQUAL_OPERATOR_CHECK(gjk_tolerance == other.gjk_tolerance); COAL_EQUAL_OPERATOR_CHECK(gjk_max_iterations == other.gjk_max_iterations); COAL_EQUAL_OPERATOR_CHECK(cached_gjk_guess == other.cached_gjk_guess); COAL_EQUAL_OPERATOR_CHECK(cached_support_func_guess == other.cached_support_func_guess); COAL_EQUAL_OPERATOR_CHECK(epa_max_iterations == other.epa_max_iterations); COAL_EQUAL_OPERATOR_CHECK(epa_tolerance == other.epa_tolerance); COAL_EQUAL_OPERATOR_CHECK(enable_timings == other.enable_timings); COAL_EQUAL_OPERATOR_CHECK(collision_distance_threshold == other.collision_distance_threshold); COAL_COMPILER_DIAGNOSTIC_POP return true; } /// @brief whether two QueryRequest are different or not inline bool operator!=(const QueryRequest& other) const { return !(*this == other); } /// @brief Prints the query request to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; os << prefix << " gjk_initial_guess: " << (gjk_initial_guess == GJKInitialGuess::DefaultGuess ? "DefaultGuess" : (GJKInitialGuess::CachedGuess ? "CachedGuess" : "BoundingVolumeGuess")) << ",\n"; os << prefix << " cached_gjk_guess: " << cached_gjk_guess.transpose() << ",\n"; os << prefix << " cached_support_func_guess: " << cached_support_func_guess.transpose() << ",\n"; os << prefix << " gjk_max_iterations: " << gjk_max_iterations << ",\n"; os << prefix << " gjk_tolerance: " << gjk_tolerance << ",\n"; os << prefix << " gjk_variant: " << static_cast(gjk_variant) << ",\n"; os << prefix << " gjk_convergence_criterion: " << static_cast(gjk_convergence_criterion) << ",\n"; os << prefix << " gjk_convergence_criterion_type: " << static_cast(gjk_convergence_criterion_type) << ",\n"; os << prefix << " epa_max_iterations: " << epa_max_iterations << ",\n"; os << prefix << " epa_tolerance: " << epa_tolerance << ",\n"; os << prefix << " enable_timings: " << enable_timings << ",\n"; os << prefix << " collision_distance_threshold: " << collision_distance_threshold << ",\n"; } }; /// @brief base class for all query results struct COAL_DLLAPI QueryResult { /// @brief stores the last GJK ray when relevant. Vec3s cached_gjk_guess; /// @brief stores the last support function vertex index, when relevant. support_func_guess_t cached_support_func_guess; /// @brief timings for the given request CPUTimes timings; QueryResult() : cached_gjk_guess(Vec3s::Zero()), cached_support_func_guess(support_func_guess_t::Constant(-1)) {} /// @brief Comparison operator. inline bool operator==(const QueryResult& other) const { if (this == &other) return true; COAL_EQUAL_OPERATOR_CHECK(cached_gjk_guess == other.cached_gjk_guess); COAL_EQUAL_OPERATOR_CHECK(cached_support_func_guess == other.cached_support_func_guess); COAL_EQUAL_OPERATOR_CHECK(timings == other.timings); return true; } /// @brief Prints the query result to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; os << prefix << " cached_gjk_guess: " << cached_gjk_guess.transpose() << ",\n"; os << prefix << " cached_support_func_guess: " << cached_support_func_guess.transpose() << ",\n"; os << prefix << " timings: " << timings.user << ",\n"; } }; inline void QueryRequest::updateGuess(const QueryResult& result) const { COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (gjk_initial_guess == GJKInitialGuess::CachedGuess || enable_cached_gjk_guess) { cached_gjk_guess = result.cached_gjk_guess; cached_support_func_guess = result.cached_support_func_guess; } COAL_COMPILER_DIAGNOSTIC_POP } struct CollisionResult; /// @brief flag declaration for specifying required params in CollisionResult enum CollisionRequestFlag { CONTACT = 0x00001, DISTANCE_LOWER_BOUND = 0x00002, NO_REQUEST = 0x01000 }; /// @brief request to the collision algorithm struct COAL_DLLAPI CollisionRequest : QueryRequest { /// @brief The maximum number of contacts that can be returned size_t num_max_contacts; /// @brief whether the contact information (normal, penetration depth and /// contact position) will return. bool enable_contact; /// Whether a lower bound on distance is returned when objects are disjoint COAL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.) bool enable_distance_lower_bound; /// @brief Distance below which objects are considered in collision. /// See \ref coal_collision_and_distance_lower_bound_computation /// @note If set to -inf, the objects tested for collision are considered /// as collision free and no test is actually performed by functions /// coal::collide of class coal::ComputeCollision. Scalar security_margin; /// @brief Distance below which bounding volumes are broken down. /// See \ref coal_collision_and_distance_lower_bound_computation Scalar break_distance; /// @brief Distance above which GJK solver makes an early stopping. /// GJK stops searching for the closest points when it proves that the /// distance between two geometries is above this threshold. /// /// @remarks Consequently, the closest points might be incorrect, but allows /// to save computational resources. Scalar distance_upper_bound; /// @brief Constructor from a flag and a maximal number of contacts. /// /// @param[in] flag Collision request flag /// @param[in] num_max_contacts Maximal number of allowed contacts /// COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) : num_max_contacts(num_max_contacts_), enable_contact(flag & CONTACT), enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND), security_margin(0), break_distance(Scalar(1e-3)), distance_upper_bound((std::numeric_limits::max)()) {} /// @brief Default constructor. CollisionRequest() : num_max_contacts(1), enable_contact(true), enable_distance_lower_bound(false), security_margin(0), break_distance(Scalar(1e-3)), distance_upper_bound((std::numeric_limits::max)()) {} COAL_COMPILER_DIAGNOSTIC_POP bool isSatisfied(const CollisionResult& result) const; /// @brief whether two CollisionRequest are the same or not inline bool operator==(const CollisionRequest& other) const { if (this == &other) return true; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS COAL_EQUAL_OPERATOR_CHECK((QueryRequest::operator==(other))); COAL_EQUAL_OPERATOR_CHECK(num_max_contacts == other.num_max_contacts); COAL_EQUAL_OPERATOR_CHECK(enable_contact == other.enable_contact); COAL_EQUAL_OPERATOR_CHECK(enable_distance_lower_bound == other.enable_distance_lower_bound); COAL_EQUAL_OPERATOR_CHECK(security_margin == other.security_margin); COAL_EQUAL_OPERATOR_CHECK(break_distance == other.break_distance); COAL_EQUAL_OPERATOR_CHECK(distance_upper_bound == other.distance_upper_bound); COAL_COMPILER_DIAGNOSTIC_POP return true; } /// @brief whether two CollisionRequest are different or not inline bool operator!=(const CollisionRequest& other) const { return !(*this == other); } /// @brief Prints the collision request to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; QueryRequest::disp(os, prefix); os << prefix << " num_max_contacts: " << num_max_contacts << ",\n"; os << prefix << " enable_contact: " << enable_contact << ",\n"; os << prefix << " security_margin: " << security_margin << ",\n"; os << prefix << " break_distance: " << break_distance << ",\n"; os << prefix << " distance_upper_bound: " << distance_upper_bound << ",\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const CollisionRequest& request) { os << "CollisionRequest: {" << "\n"; request.disp(os); os << "}" << "\n"; return os; } }; inline Scalar Contact::getDistanceToCollision( const CollisionRequest& request) const { return penetration_depth - request.security_margin; } /// @brief collision result struct COAL_DLLAPI CollisionResult : QueryResult { private: /// @brief contact information std::vector contacts; public: /// Lower bound on distance between objects if they are disjoint. /// See \ref coal_collision_and_distance_lower_bound_computation /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is /// set to infinity, distance_lower_bound is the actual distance between the /// shapes. Scalar distance_lower_bound; /// @brief normal associated to nearest_points. /// Same as `CollisionResult::nearest_points` but for the normal. Vec3s normal; /// @brief nearest points. /// A `CollisionResult` can have multiple contacts. /// The nearest points in `CollisionResults` correspond to the witness points /// associated with the smallest distance i.e the `distance_lower_bound`. /// For bounding volumes and BVHs, these nearest points are available /// only when distance_lower_bound is inferior to /// CollisionRequest::break_distance. std::array nearest_points; public: /// @brief Default constructor. CollisionResult() : distance_lower_bound((std::numeric_limits::max)()) { nearest_points[0] = nearest_points[1] = normal = Vec3s::Constant(std::numeric_limits::max()); } /// @brief Copy constructor. CollisionResult(const CollisionResult& other) = default; CollisionResult& operator=(const CollisionResult& other) = default; /// @brief Copy constructor from an other CollisionResult and two vectors of /// CollisionGeometry pointers. /// This constructor allows to copy the collision result information from an /// other collision result, but remap the pointers to the collision geometries /// to the provided ones. This is usefull in a deep-copy context, when the /// collision geometries are also copied and the pointers in the collision /// result need to be remapped to the new collision geometries. CollisionResult( const CollisionResult& other, const std::vector>& new_geometries) : CollisionResult(other) { resolveReferences(new_geometries); } /// @brief Resolve the internal points the pointers in the contacts to the /// provided pairs of collision geometries. This is useful when deep-copying /// or deserializing a collision result, as the pointers to the collision /// objects are not valid anymore and need to be remapped to the collision /// objects in the current context. void resolveReferences( const std::vector>& new_geometries) { COAL_THROW_PRETTY_IF( contacts.size() != new_geometries.size(), std::invalid_argument, "The size of the vectors of collision geometries must be equal to " "the number of contacts in the collision result."); for (std::size_t i = 0; i < contacts.size(); ++i) { contacts[i].resolveReferences(new_geometries[i]); } } /// @brief Update the lower bound only if the distance is inferior. inline void updateDistanceLowerBound(const Scalar& distance_lower_bound_) { if (distance_lower_bound_ < distance_lower_bound) distance_lower_bound = distance_lower_bound_; } /// @brief add one contact into result structure inline void addContact(const Contact& c) { contacts.push_back(c); } /// @brief whether two CollisionResult are the same or not inline bool operator==(const CollisionResult& other) const { if (this == &other) return true; COAL_EQUAL_OPERATOR_CHECK((QueryResult::operator==(other))); COAL_EQUAL_OPERATOR_CHECK(contacts == other.contacts); COAL_EQUAL_OPERATOR_CHECK(distance_lower_bound == other.distance_lower_bound); COAL_EQUAL_OPERATOR_CHECK(nearest_points[0] == other.nearest_points[0]); COAL_EQUAL_OPERATOR_CHECK(nearest_points[1] == other.nearest_points[1]); COAL_EQUAL_OPERATOR_CHECK(normal == other.normal); return true; } /// @brief whether two CollisionResult are the same or not bool operator!=(const CollisionResult& other) const { return !(*this == other); } /// @brief return binary collision result bool isCollision() const { return contacts.size() > 0; } /// @brief number of contacts found size_t numContacts() const { return contacts.size(); } /// @brief get the i-th contact calculated const Contact& getContact(size_t i) const { if (contacts.size() == 0) COAL_THROW_PRETTY( "The number of contacts is zero. No Contact can be returned.", std::invalid_argument); if (i < contacts.size()) return contacts[i]; else return contacts.back(); } /// @brief set the i-th contact calculated void setContact(size_t i, const Contact& c) { if (contacts.size() == 0) COAL_THROW_PRETTY( "The number of contacts is zero. No Contact can be returned.", std::invalid_argument); if (i < contacts.size()) contacts[i] = c; else contacts.back() = c; } /// @brief get all the contacts void getContacts(std::vector& contacts_) const { contacts_.resize(contacts.size()); std::copy(contacts.begin(), contacts.end(), contacts_.begin()); } const std::vector& getContacts() const { return contacts; } /// @brief clear the results obtained void clear() { distance_lower_bound = (std::numeric_limits::max)(); contacts.clear(); timings.clear(); nearest_points[0] = nearest_points[1] = normal = Vec3s::Constant(std::numeric_limits::max()); } /// @brief reposition Contact objects when fcl inverts them /// during their construction. void swapObjects(); /// \brief Prints the result to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; QueryResult::disp(os, prefix); os << prefix << " distance_lower_bound: " << distance_lower_bound << ",\n"; os << prefix << " normal: " << normal.transpose() << ",\n"; os << prefix << " nearest_points[0]: " << nearest_points[0].transpose() << ",\n"; os << prefix << " nearest_points[1]: " << nearest_points[1].transpose() << ",\n"; os << prefix << " contacts: {" << "\n"; for (std::size_t i = 0; i < contacts.size(); ++i) { os << prefix << " Contact " << i << ": {" << "\n"; const Contact& contact = contacts[i]; contact.disp(os, prefix + " "); os << prefix << " }\n"; } os << prefix << " }\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const CollisionResult& res) { os << "CollisionResult:" << "\n"; res.disp(os); os << "}" << "\n"; return os; } }; /// @brief This structure allows to encode contact patches. /// A contact patch is defined by a set of points belonging to a subset of a /// plane passing by `p` and supported by `n`, where `n = Contact::normal` and /// `p = Contact::pos`. If we denote by P this plane and by S1 and S2 the first /// and second shape of a collision pair, a contact patch is represented as a /// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the /// set-intersection. Since a contact patch is a subset of a plane supported by /// `n`, it has a preferred direction. In Coal, the `Contact::normal` points /// from S1 to S2. In the same way, a contact patch points by default from S1 /// to S2. /// /// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope), /// so the points of the set, forming the convex-hull of the polytope, are /// stored in a counter-clockwise fashion. /// @note If needed, the internal algorithms of Coal can easily be extended /// to compute a contact volume instead of a contact patch. struct COAL_DLLAPI ContactPatch { public: using Polygon = std::vector; /// @brief Frame of the set, expressed in the world coordinates. /// The z-axis of the frame's rotation is the contact patch normal. Transform3s tf; /// @brief Direction of ContactPatch. /// When doing collision detection, the convention of Coal is that the /// normal always points from the first to the second shape of the collision /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`. /// The PatchDirection enum allows to identify if the patch points from /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted /// type). The Inverted type should only be used for internal Coal /// computations (it allows to properly define two separate contact patches in /// the same frame). enum PatchDirection { DEFAULT = 0, INVERTED = 1 }; /// @brief Direction of this contact patch. PatchDirection direction; /// @brief Penetration depth of the contact patch. This value corresponds to /// the signed distance `d` between the shapes. /// @note For each contact point `p` in the patch of normal `n`, `p1 = p - /// 0.5*d*n` and `p2 = p + 0.5*d*n` define a pair of witness points. `p1` /// belongs to the surface of the first shape and `p2` belongs to the surface /// of the second shape. For any pair of witness points, we always have `p2 - /// p1 = d * n`. The vector `d * n` is called a minimum separation vector: /// if S1 is translated by it, S1 and S2 are not in collision anymore. /// @note Although there may exist multiple minimum separation vectors between /// two shapes, the term "minimum" comes from the fact that it's impossible to /// find a different separation vector which has a smaller norm than `d * n`. Scalar penetration_depth; /// @brief Default maximum size of the polygon representing the contact patch. /// Used to pre-allocate memory for the patch. static constexpr size_t default_preallocated_size = 12; protected: /// @brief Container for the vertices of the set. std::vector m_points; public: /// @brief Default constructor. /// Note: the preallocated size does not determine the maximum number of /// points in the patch, it only serves as preallocation if the maximum size /// of the patch is known in advance. Coal will automatically expand/shrink /// the contact patch if needed. explicit ContactPatch(size_t preallocated_size = default_preallocated_size) : tf(Transform3s::Identity()), direction(PatchDirection::DEFAULT), penetration_depth(0) { this->m_points.reserve(preallocated_size); } /// @brief Normal of the contact patch, expressed in the WORLD frame. Vec3s getNormal() const { if (this->direction == PatchDirection::INVERTED) { return -this->tf.rotation().col(2); } return this->tf.rotation().col(2); } /// @brief Returns the number of points in the contact patch. size_t size() const { return this->m_points.size(); } /// @brief Returns the area of the 2D surface represented by the contact /// patch. Scalar computeArea() const { const size_t n = this->m_points.size(); if (n < 3) { return Scalar(0); } // Shoelace formula for polygon area // See: https://en.wikipedia.org/wiki/Shoelace_formula Scalar area = Scalar(0); for (size_t i = 0; i < n; ++i) { const size_t j = (i + 1) % n; area += this->m_points[i](0) * this->m_points[j](1); area -= this->m_points[j](0) * this->m_points[i](1); } return std::abs(area) / Scalar(2); } /// @brief Add a 3D point to the set, expressed in the world frame. /// @note This function takes a 3D point and expresses it in the local frame /// of the set. It then takes only the x and y components of the vector, /// effectively doing a projection onto the plane to which the set belongs. /// TODO(louis): if necessary, we can store the offset to the plane (x, y). void addPoint(const Vec3s& point_3d) { const Vec3s point = this->tf.inverseTransform(point_3d); this->m_points.emplace_back(point.template head<2>()); } /// @brief Get the i-th point of the set, expressed in the 3D world frame. Vec3s getPoint(const size_t i) const { Vec3s point(0, 0, 0); point.head<2>() = this->point(i); point = tf.transform(point); return point; } /// @brief Get the i-th point of the contact patch, projected back onto the /// first shape of the collision pair. This point is expressed in the 3D /// world frame. Vec3s getPointShape1(const size_t i) const { Vec3s point = this->getPoint(i); point -= (this->penetration_depth / 2) * this->getNormal(); return point; } /// @brief Get the i-th point of the contact patch, projected back onto the /// first shape of the collision pair. This 3D point is expressed in the world /// frame. Vec3s getPointShape2(const size_t i) const { Vec3s point = this->getPoint(i); point += (this->penetration_depth / 2) * this->getNormal(); return point; } /// @brief Getter for the 2D points in the set. std::vector& points() { return this->m_points; } /// @brief Const getter for the 2D points in the set. const std::vector& points() const { return this->m_points; } /// @brief Getter for the i-th 2D point in the set. Vec2s& point(const size_t i) { COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error); if (i < this->m_points.size()) { return this->m_points[i]; } return this->m_points.back(); } /// @brief Const getter for the i-th 2D point in the set. const Vec2s& point(const size_t i) const { COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error); if (i < this->m_points.size()) { return this->m_points[i]; } return this->m_points.back(); } /// @brief Clear the set. void clear() { this->m_points.clear(); this->tf.setIdentity(); this->penetration_depth = 0; } /// @brief Whether two contact patches are the same or not. /// @note This compares the two sets terms by terms. /// However, two contact patches can be identical, but have a different /// order for their points. Use `isEqual` in this case. bool operator==(const ContactPatch& other) const { if (this == &other) return true; COAL_EQUAL_OPERATOR_CHECK(tf == other.tf); COAL_EQUAL_OPERATOR_CHECK(direction == other.direction); COAL_EQUAL_OPERATOR_CHECK(penetration_depth == other.penetration_depth); COAL_EQUAL_OPERATOR_CHECK(points() == other.points()); return true; } /// @brief Whether two contact patches are different or not. bool operator!=(const ContactPatch& other) const { return !(*this == other); } /// @brief Whether two contact patches are the same or not. /// Checks for different order of the points. bool isSame( const ContactPatch& other, const Scalar tol = Eigen::NumTraits::dummy_precision()) const { // The x and y axis of the set are arbitrary, but the z axis is // always the normal. The position of the origin of the frame is also // arbitrary. So we only check if the normals are the same. if (!this->getNormal().isApprox(other.getNormal(), tol)) { return false; } if (std::abs(this->penetration_depth - other.penetration_depth) > tol) { return false; } if (this->direction != other.direction) { return false; } if (this->size() != other.size()) { return false; } // Check all points of the contact patch. for (size_t i = 0; i < this->size(); ++i) { bool found = false; const Vec3s pi = this->getPoint(i); for (size_t j = 0; j < other.size(); ++j) { const Vec3s other_pj = other.getPoint(j); if (pi.isApprox(other_pj, tol)) { found = true; } } if (!found) { return false; } } return true; } /// @brief Prints the contact patch to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; os << prefix << " tf: {" << "\n"; tf.disp(os, prefix + " "); os << prefix << " },\n"; os << prefix << " direction: " << (direction == PatchDirection::DEFAULT ? "DEFAULT" : "INVERTED") << ",\n"; os << prefix << " penetration_depth: " << penetration_depth << ",\n"; os << prefix << " points: {" << "\n"; for (size_t i = 0; i < this->size(); ++i) { os << prefix << " Point " << i << ": " << this->getPoint(i).transpose() << ",\n"; } os << prefix << " }\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const ContactPatch& patch) { os << "ContactPatch: {" << "\n"; patch.disp(os); os << "}" << "\n"; return os; } }; /// @brief Construct a frame from a `Contact`'s position and normal. /// Because both `Contact`'s position and normal are expressed in the world /// frame, this frame is also expressed w.r.t the world frame. /// The origin of the frame is `contact.pos` and the z-axis of the frame is /// `contact.normal`. inline void constructContactPatchFrameFromContact(const Contact& contact, ContactPatch& contact_patch) { contact_patch.penetration_depth = contact.penetration_depth; contact_patch.tf.translation() = contact.pos; contact_patch.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); contact_patch.direction = ContactPatch::PatchDirection::DEFAULT; } /// @brief Structure used for internal computations. A support set and a /// contact patch can be represented by the same structure. In fact, a contact /// patch is the intersection of two support sets, one with /// `PatchDirection::DEFAULT` and one with `PatchDirection::INVERTED`. /// @note A support set with `DEFAULT` direction is the support set of a shape /// in the direction given by `+n`, where `n` is the z-axis of the frame's /// patch rotation. An `INVERTED` support set is the support set of a shape in /// the direction `-n`. using SupportSet = ContactPatch; /// @brief Request for a contact patch computation. struct COAL_DLLAPI ContactPatchRequest { /// @brief Maximum number of contact patches that will be computed. size_t max_num_patch; protected: /// @brief Maximum samples to compute the support sets of curved shapes, /// i.e. when the normal is perpendicular to the base of a cylinder. For /// now, only relevant for Cone and Cylinder. In the future this might be /// extended to Sphere and Ellipsoid. size_t m_num_samples_curved_shapes; /// @brief Tolerance below which points are added to a contact patch. /// In details, given two shapes S1 and S2, a contact patch is the triple /// intersection between the separating plane (P) (passing by `Contact::pos` /// and supported by `Contact::normal`), S1 and S2; i.e. a contact patch is /// `P & S1 & S2` if we denote `&` the set intersection operator. If a point /// p1 of S1 is at a distance below `patch_tolerance` from the separating /// plane, it is taken into account in the computation of the contact patch. /// Otherwise, it is not used for the computation. /// @note Needs to be positive. Scalar m_patch_tolerance; public: /// @brief Default constructor. /// @param max_num_patch maximum number of contact patches per collision pair. /// @param max_sub_patch_size maximum size of each sub contact patch. Each /// contact patch contains an internal representation for an inscribed sub /// contact patch. This allows physics simulation to always work with a /// predetermined maximum size for each contact patch. A sub contact patch is /// simply a subset of the vertices of a contact patch. /// @param num_samples_curved_shapes for shapes like cones and cylinders, /// which have smooth basis (circles in this case), we need to sample a /// certain amount of point of this basis. /// @param patch_tolerance the tolerance below which a point of a shape is /// considered to belong to the support set of this shape in the direction of /// the normal. Said otherwise, `patch_tolerance` determines the "thickness" /// of the separating plane between shapes of a collision pair. explicit ContactPatchRequest(size_t max_num_patch = 1, size_t num_samples_curved_shapes = ContactPatch::default_preallocated_size, Scalar patch_tolerance = Scalar(1e-3)) : max_num_patch(max_num_patch) { this->setNumSamplesCurvedShapes(num_samples_curved_shapes); this->setPatchTolerance(patch_tolerance); } /// @brief Construct a contact patch request from a collision request. explicit ContactPatchRequest(const CollisionRequest& collision_request, size_t num_samples_curved_shapes = ContactPatch::default_preallocated_size, Scalar patch_tolerance = Scalar(1e-3)) : max_num_patch(collision_request.num_max_contacts) { this->setNumSamplesCurvedShapes(num_samples_curved_shapes); this->setPatchTolerance(patch_tolerance); } /// @copydoc m_num_samples_curved_shapes void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) { if (num_samples_curved_shapes < 3) { COAL_LOG_WARNING( "`num_samples_curved_shapes` cannot be lower than 3. Setting it to " "3 to prevent bugs."); this->m_num_samples_curved_shapes = 3; } else { this->m_num_samples_curved_shapes = num_samples_curved_shapes; } } /// @copydoc m_num_samples_curved_shapes size_t getNumSamplesCurvedShapes() const { return this->m_num_samples_curved_shapes; } /// @copydoc m_patch_tolerance void setPatchTolerance(const Scalar patch_tolerance) { if (patch_tolerance < 0) { COAL_LOG_WARNING( "`patch_tolerance` cannot be negative. Setting it to 0 to prevent " "bugs."); this->m_patch_tolerance = Eigen::NumTraits::dummy_precision(); } else { this->m_patch_tolerance = patch_tolerance; } } /// @copydoc m_patch_tolerance Scalar getPatchTolerance() const { return this->m_patch_tolerance; } /// @brief Whether two ContactPatchRequest are identical or not. bool operator==(const ContactPatchRequest& other) const { if (this == &other) return true; COAL_EQUAL_OPERATOR_CHECK(max_num_patch == other.max_num_patch); COAL_EQUAL_OPERATOR_CHECK(getNumSamplesCurvedShapes() == other.getNumSamplesCurvedShapes()); COAL_EQUAL_OPERATOR_CHECK(getPatchTolerance() == other.getPatchTolerance()); return true; } /// @brief Whether two ContactPatchRequest are different or not. bool operator!=(const ContactPatchRequest& other) const { return !(*this == other); } /// @brief Prints the contact patch request to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; os << prefix << " max_num_patch: " << max_num_patch << ",\n"; os << prefix << " num_samples_curved_shapes: " << getNumSamplesCurvedShapes() << ",\n"; os << prefix << " patch_tolerance: " << getPatchTolerance() << ",\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const ContactPatchRequest& request) { os << "ContactPatchRequest: {" << "\n"; request.disp(os); os << "}" << "\n"; return os; } }; /// @brief Result for a contact patch computation. struct COAL_DLLAPI ContactPatchResult { using ContactPatchVector = std::vector; using ContactPatchRef = std::reference_wrapper; using ContactPatchRefVector = std::vector; protected: /// @brief Data container for the vector of contact patches. /// @note Contrary to `CollisionResult` or `DistanceResult`, which have a /// very small memory footprint, contact patches can contain relatively /// large polytopes. In order to reuse a `ContactPatchResult` while avoiding /// successive mallocs, we have a data container and a vector which points /// to the currently active patches in this data container. ContactPatchVector m_contact_patches_data; /// @brief Contact patches in `m_contact_patches_data` can have two /// statuses: used or unused. This index tracks the first unused patch in /// the `m_contact_patches_data` vector. size_t m_id_available_patch; /// @brief Vector of contact patches of the result. ContactPatchRefVector m_contact_patches; public: /// @brief Default constructor. ContactPatchResult() : m_id_available_patch(0) { const size_t max_num_patch = 1; const ContactPatchRequest request(max_num_patch); this->set(request); } /// @brief Constructor using a `ContactPatchRequest`. explicit ContactPatchResult(const ContactPatchRequest& request) : m_id_available_patch(0) { this->set(request); }; /// @brief Copy constructor. ContactPatchResult(const ContactPatchResult& other) { *this = other; } /// @brief Move constructor. ContactPatchResult(ContactPatchResult&& other) noexcept { *this = std::move(other); } /// @brief Copy assignment operator. ContactPatchResult& operator=(const ContactPatchResult& other) { if (this != &other) { this->m_contact_patches_data = other.m_contact_patches_data; this->m_id_available_patch = other.m_id_available_patch; this->m_contact_patches.clear(); for (size_t i = 0; i < this->m_id_available_patch; ++i) { this->m_contact_patches.emplace_back(this->m_contact_patches_data[i]); } } return *this; } /// @brief Move assignment operator. ContactPatchResult& operator=(ContactPatchResult&& other) noexcept { if (this != &other) { this->m_contact_patches_data = std::move(other.m_contact_patches_data); this->m_id_available_patch = other.m_id_available_patch; this->m_contact_patches.clear(); for (size_t i = 0; i < this->m_id_available_patch; ++i) { this->m_contact_patches.emplace_back(this->m_contact_patches_data[i]); } } return *this; } /// @brief Number of contact patches in the result. size_t numContactPatches() const { return this->m_contact_patches.size(); } /// @brief Returns a new unused contact patch from the internal data vector. ContactPatch& getUnusedContactPatch() { if (this->m_id_available_patch >= this->m_contact_patches_data.size()) { COAL_LOG_WARNING( "Trying to get an unused contact patch but all contact patches are " "used. Increasing size of contact patches vector, at the cost of a " "copy. You should increase `max_num_patch` in the " "`ContactPatchRequest` when setting up `ContactPatchResult`."); bool need_to_reset_references = (this->m_contact_patches_data.size() == this->m_contact_patches_data.capacity()); this->m_contact_patches_data.emplace_back(ContactPatch()); if (need_to_reset_references) { // If we have reached the capacity of the data vector, all references // in `m_contact_patches` are invalidated. We need to reset them. this->m_contact_patches.clear(); for (size_t i = 0; i < this->m_id_available_patch; ++i) { this->m_contact_patches.emplace_back(this->m_contact_patches_data[i]); } } } ContactPatch& contact_patch = this->m_contact_patches_data[this->m_id_available_patch]; contact_patch.clear(); this->m_contact_patches.emplace_back(contact_patch); ++(this->m_id_available_patch); return this->m_contact_patches.back(); } /// @brief Const getter for the i-th contact patch of the result. const ContactPatch& getContactPatch(const size_t i) const { if (this->m_contact_patches.empty()) { COAL_THROW_PRETTY( "The number of contact patches is zero. No ContactPatch can be " "returned.", std::invalid_argument); } if (i < this->m_contact_patches.size()) { return this->m_contact_patches[i]; } return this->m_contact_patches.back(); } /// @brief Getter for the i-th contact patch of the result. ContactPatch& contactPatch(const size_t i) { if (this->m_contact_patches.empty()) { COAL_THROW_PRETTY( "The number of contact patches is zero. No ContactPatch can be " "returned.", std::invalid_argument); } if (i < this->m_contact_patches.size()) { return this->m_contact_patches[i]; } return this->m_contact_patches.back(); } /// @brief Clears the contact patch result. void clear() { this->m_contact_patches.clear(); this->m_id_available_patch = 0; for (ContactPatch& patch : this->m_contact_patches_data) { patch.clear(); } } /// @brief Set up a `ContactPatchResult` from a `ContactPatchRequest` void set(const ContactPatchRequest& request) { if (this->m_contact_patches_data.size() < request.max_num_patch) { this->m_contact_patches_data.resize(request.max_num_patch); } for (ContactPatch& patch : this->m_contact_patches_data) { patch.points().reserve(request.getNumSamplesCurvedShapes()); } this->clear(); } /// @brief Return true if this `ContactPatchResult` is aligned with the /// `ContactPatchRequest` given as input. bool check(const ContactPatchRequest& request) const { assert(this->m_contact_patches_data.size() >= request.max_num_patch); if (this->m_contact_patches_data.size() < request.max_num_patch) { return false; } for (const ContactPatch& patch : this->m_contact_patches_data) { if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) { assert(patch.points().capacity() >= request.getNumSamplesCurvedShapes()); return false; } } return true; } /// @brief Whether two ContactPatchResult are identical or not. bool operator==(const ContactPatchResult& other) const { if (this == &other) return true; COAL_EQUAL_OPERATOR_CHECK(numContactPatches() == other.numContactPatches()); for (size_t i = 0; i < this->numContactPatches(); ++i) { const ContactPatch& patch = this->getContactPatch(i); const ContactPatch& other_patch = other.getContactPatch(i); COAL_EQUAL_OPERATOR_CHECK(patch == other_patch); } return true; } /// @brief Whether two ContactPatchResult are different or not. bool operator!=(const ContactPatchResult& other) const { return !(*this == other); } /// @brief Repositions the ContactPatch when they get inverted during their /// construction. void swapObjects() { // Create new transform: it's the reflection of `tf` along the z-axis. // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis. for (size_t i = 0; i < this->numContactPatches(); ++i) { ContactPatch& patch = this->contactPatch(i); patch.tf.rotation().col(0) *= -1.0; patch.tf.rotation().col(2) *= -1.0; for (size_t j = 0; j < patch.size(); ++j) { patch.point(i)(0) *= Scalar(-1); // only invert the x-axis } } } /// @brief Prints the contact patch result to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; os << prefix << " numContactPatches: " << this->numContactPatches() << ",\n"; os << prefix << " contact_patches: {" << "\n"; for (size_t i = 0; i < this->numContactPatches(); ++i) { os << prefix << " ContactPatch " << i << ": {" << "\n"; const ContactPatch& patch = this->getContactPatch(i); patch.disp(os, prefix + " "); os << prefix << " }\n"; } os << prefix << " }\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const ContactPatchResult& result) { os << "ContactPatchResult: {" << "\n"; result.disp(os); os << "}" << "\n"; return os; } }; struct DistanceResult; /// @brief request to the distance computation struct COAL_DLLAPI DistanceRequest : QueryRequest { /// @brief whether to return the nearest points. /// Nearest points are always computed and are the points of the shapes that /// achieve a distance of `DistanceResult::min_distance`. COAL_DEPRECATED_MESSAGE( Nearest points are always computed : they are the points of the shapes that achieve a distance of `DistanceResult::min_distance` .\n Use `enable_signed_distance` if you want to compute a signed minimum distance(and thus its corresponding nearest points) .) bool enable_nearest_points; /// @brief whether to compute the penetration depth when objects are in /// collision. /// Turning this off can save computation time if only the distance /// when objects are disjoint is needed. /// @note The minimum distance between the shapes is stored in /// `DistanceResult::min_distance`. /// If `enable_signed_distance` is off, `DistanceResult::min_distance` /// is always positive. /// If `enable_signed_distance` is on, `DistanceResult::min_distance` /// can be positive or negative. /// The nearest points are the points of the shapes that achieve /// a distance of `DistanceResult::min_distance`. bool enable_signed_distance; /// @brief error threshold for approximate distance Scalar rel_err; // relative error, between 0 and 1 Scalar abs_err; // absolute error /// \param enable_nearest_points_ enables the nearest points computation. /// \param enable_signed_distance_ allows to compute the penetration depth /// \param rel_err_ /// \param abs_err_ COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS DistanceRequest(bool enable_nearest_points_ = true, bool enable_signed_distance_ = true, Scalar rel_err_ = 0.0, Scalar abs_err_ = 0.0) : enable_nearest_points(enable_nearest_points_), enable_signed_distance(enable_signed_distance_), rel_err(rel_err_), abs_err(abs_err_) {} COAL_COMPILER_DIAGNOSTIC_POP bool isSatisfied(const DistanceResult& result) const; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS DistanceRequest(const DistanceRequest& other) = default; DistanceRequest& operator=(const DistanceRequest& other) = default; COAL_COMPILER_DIAGNOSTIC_POP /// @brief whether two DistanceRequest are the same or not inline bool operator==(const DistanceRequest& other) const { if (this == &other) return true; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS COAL_EQUAL_OPERATOR_CHECK((QueryRequest::operator==(other))); COAL_EQUAL_OPERATOR_CHECK(enable_nearest_points == other.enable_nearest_points); COAL_EQUAL_OPERATOR_CHECK(enable_signed_distance == other.enable_signed_distance); COAL_EQUAL_OPERATOR_CHECK(rel_err == other.rel_err); COAL_EQUAL_OPERATOR_CHECK(abs_err == other.abs_err); COAL_COMPILER_DIAGNOSTIC_POP return true; } /// @brief whether two DistanceRequest are different or not inline bool operator!=(const DistanceRequest& other) const { return !(*this == other); } /// @brief Prints the distance request to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; QueryRequest::disp(os, prefix); os << prefix << " enable_signed_distance: " << enable_signed_distance << ",\n"; os << prefix << " rel_err: " << rel_err << ",\n"; os << prefix << " abs_err: " << abs_err << ",\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const DistanceRequest& request) { os << "DistanceRequest: {" << "\n"; request.disp(os); os << "}" << "\n"; return os; } }; /// @brief distance result struct COAL_DLLAPI DistanceResult : QueryResult { public: /// @brief minimum distance between two objects. /// If two objects are in collision and /// DistanceRequest::enable_signed_distance is activated, min_distance <= 0. /// @note The nearest points are the points of the shapes that achieve a /// distance of `DistanceResult::min_distance`. Scalar min_distance; /// @brief normal. Vec3s normal; /// @brief nearest points. /// See CollisionResult::nearest_points. std::array nearest_points; /// @brief collision object 1 const CollisionGeometry* o1; /// @brief collision object 2 const CollisionGeometry* o2; /// @brief information about the nearest point in object 1 /// if object 1 is mesh or point cloud, it is the triangle or point id /// if object 1 is geometry shape, it is NONE (-1), /// if object 1 is octree, it is the id of the cell int b1; /// @brief information about the nearest point in object 2 /// if object 2 is mesh or point cloud, it is the triangle or point id /// if object 2 is geometry shape, it is NONE (-1), /// if object 2 is octree, it is the id of the cell int b2; /// @brief invalid contact primitive information static const int NONE = -1; /// @brief Default constructor. DistanceResult(Scalar min_distance_ = (std::numeric_limits::max)()) : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) { const Vec3s inf(Vec3s::Constant(std::numeric_limits::max())); nearest_points[0] = nearest_points[1] = normal = inf; } /// @brief Copy constructor. /// This constructor does a raw copy of the contact information, including the /// pointers to the collision geometries. DistanceResult(const DistanceResult& other) = default; DistanceResult& operator=(const DistanceResult& other) = default; /// @brief Copy constructor using another result and a pair of /// CollisionGeometry pointers. This constructor allows to copy the contact /// information from an other contact, but remap the pointers to the collision /// geometries to the provided ones. This is usefull in a deep-copy context, /// when the collision geometries are also copied and the pointers in the /// contact need to be remapped to the new collision geometries. DistanceResult( const DistanceResult& other, std::pair new_o1_o2) : DistanceResult(other) { resolveReferences(new_o1_o2); } /// @brief (re)Map the pointers o1/o2 to the provided collision objects. /// This is useful when deserializing a result, as the pointers to the /// collision objects are not valid anymore and need to be remapped to the /// collision objects in the current context. void resolveReferences( std::pair new_o1_o2) { o1 = new_o1_o2.first; o2 = new_o1_o2.second; } /// @brief add distance information into the result void update(Scalar distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) { if (min_distance > distance) { min_distance = distance; o1 = o1_; o2 = o2_; b1 = b1_; b2 = b2_; } } /// @brief add distance information into the result void update(Scalar distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_) { if (min_distance > distance) { min_distance = distance; o1 = o1_; o2 = o2_; b1 = b1_; b2 = b2_; nearest_points[0] = p1; nearest_points[1] = p2; normal = normal_; } } /// @brief add distance information into the result void update(const DistanceResult& other_result) { if (min_distance > other_result.min_distance) { min_distance = other_result.min_distance; o1 = other_result.o1; o2 = other_result.o2; b1 = other_result.b1; b2 = other_result.b2; nearest_points[0] = other_result.nearest_points[0]; nearest_points[1] = other_result.nearest_points[1]; normal = other_result.normal; } } /// @brief clear the result void clear() { const Vec3s inf(Vec3s::Constant(std::numeric_limits::max())); min_distance = (std::numeric_limits::max)(); o1 = NULL; o2 = NULL; b1 = NONE; b2 = NONE; nearest_points[0] = nearest_points[1] = normal = inf; timings.clear(); } /// @brief Whether two DistanceResult are the same or not. /// This compares the two results terms by terms. /// Note: two results may be equal even if they point to different collision /// geometries o1/o2, as long as these geometries are equal. inline bool operator==(const DistanceResult& other) const { if (this == &other) return true; COAL_EQUAL_OPERATOR_CHECK((QueryResult::operator==(other))); COAL_EQUAL_OPERATOR_CHECK(min_distance == other.min_distance); COAL_EQUAL_OPERATOR_CHECK(nearest_points[0] == other.nearest_points[0]); COAL_EQUAL_OPERATOR_CHECK(nearest_points[1] == other.nearest_points[1]); COAL_EQUAL_OPERATOR_CHECK(normal == other.normal); COAL_EQUAL_OPERATOR_CHECK(((o1 == nullptr && other.o1 == nullptr) || (o1 != nullptr && other.o1 != nullptr))); COAL_EQUAL_OPERATOR_CHECK(((o2 == nullptr && other.o2 == nullptr) || (o2 != nullptr && other.o2 != nullptr))); if ((o1 && other.o1) && (o1 != other.o1)) COAL_EQUAL_OPERATOR_CHECK(*o1 == *other.o1); if ((o2 && other.o2) && (o2 != other.o2)) COAL_EQUAL_OPERATOR_CHECK(*o2 == *other.o2); COAL_EQUAL_OPERATOR_CHECK(b1 == other.b1); COAL_EQUAL_OPERATOR_CHECK(b2 == other.b2); return true; } /// @brief whether two DistanceResult are different or not inline bool operator!=(const DistanceResult& other) const { return !(*this == other); } /// @brief Prints the distance result to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; QueryResult::disp(os, prefix); os << prefix << " o1: " << (o1 ? get_node_type_name(o1->getNodeType()) : "null") << ",\n"; os << prefix << " o2: " << (o2 ? get_node_type_name(o2->getNodeType()) : "null") << ",\n"; os << prefix << " b1: " << b1 << ",\n"; os << prefix << " b2: " << b2 << ",\n"; os << prefix << " min_distance: " << min_distance << ",\n"; os << prefix << " normal: " << normal.transpose() << ",\n"; os << prefix << " nearest_points[0]: " << nearest_points[0].transpose() << ",\n"; os << prefix << " nearest_points[1]: " << nearest_points[1].transpose() << ",\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const DistanceResult& result) { os << "DistanceResult: {" << "\n"; result.disp(os); os << "}" << "\n"; return os; } }; namespace internal { inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/, CollisionResult& res, const Scalar sqrDistLowerBound) { // BV cannot find negative distance. if (res.distance_lower_bound <= 0) return; Scalar new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin; if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; } inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, CollisionResult& res, const Scalar& distance, const Vec3s& p0, const Vec3s& p1, const Vec3s& normal) { if (distance < res.distance_lower_bound) { res.distance_lower_bound = distance; res.nearest_points[0] = p0; res.nearest_points[1] = p1; res.normal = normal; } } } // namespace internal inline CollisionRequestFlag operator~(CollisionRequestFlag a) { return static_cast(~static_cast(a)); } inline CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b) { return static_cast(static_cast(a) | static_cast(b)); } inline CollisionRequestFlag operator&(CollisionRequestFlag a, CollisionRequestFlag b) { return static_cast(static_cast(a) & static_cast(b)); } inline CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b) { return static_cast(static_cast(a) ^ static_cast(b)); } inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a, CollisionRequestFlag b) { return (CollisionRequestFlag&)((int&)(a) |= static_cast(b)); } inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a, CollisionRequestFlag b) { return (CollisionRequestFlag&)((int&)(a) &= static_cast(b)); } inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, CollisionRequestFlag b) { return (CollisionRequestFlag&)((int&)(a) ^= static_cast(b)); } } // namespace coal #endif ================================================ FILE: include/coal/collision_func_matrix.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_COLLISION_FUNC_MATRIX_H #define COAL_COLLISION_FUNC_MATRIX_H #include "coal/collision_object.h" #include "coal/collision_data.h" #include "coal/narrowphase/narrowphase.h" namespace coal { /// @brief collision matrix stores the functions for collision between different /// types of objects and provides a uniform call interface struct COAL_DLLAPI CollisionFunctionMatrix { /// @brief the uniform call interface for collision: for collision, we need /// know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 /// and tf2; /// 2. the solver for narrow phase collision, this is for the collision /// between geometric shapes; /// 3. the request setting for collision (e.g., whether need to return normal /// information, whether need to compute cost); /// 4. the structure to return collision result typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result); /// @brief each item in the collision matrix is a function to handle collision /// between objects of type1 and type2 CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT]; CollisionFunctionMatrix(); }; } // namespace coal #endif ================================================ FILE: include/coal/collision_object.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_COLLISION_OBJECT_BASE_H #define COAL_COLLISION_OBJECT_BASE_H #include #include #include "coal/deprecated.hh" #include "coal/fwd.hh" #include "coal/BV/AABB.h" #include "coal/math/transform.h" #include "coal/shared_ptr_comparison.h" namespace coal { /// @brief object type: BVH (mesh, points), basic geometry, octree enum OBJECT_TYPE { OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_HFIELD, OT_COUNT }; /// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, /// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, /// cylinder, convex, plane, triangle), and octree enum NODE_TYPE { BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24, GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX16, GEOM_CONVEX32, GEOM_CONVEX = GEOM_CONVEX32, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, GEOM_ELLIPSOID, HF_AABB, HF_OBBRSS, NODE_COUNT }; /// @addtogroup Construction_Of_BVH /// @{ /// @brief The geometry for the object for collision or distance computation class COAL_DLLAPI CollisionGeometry { public: CollisionGeometry() : aabb_center(Vec3s::Constant((std::numeric_limits::max)())), aabb_radius(-1), cost_density(1), threshold_occupied(1), threshold_free(0) {} /// \brief Copy constructor CollisionGeometry(const CollisionGeometry& other) = default; virtual ~CollisionGeometry() {} /// @brief Clone *this into a new CollisionGeometry virtual CollisionGeometry* clone() const = 0; /// \brief Equality operator bool operator==(const CollisionGeometry& other) const { COAL_EQUAL_OPERATOR_CHECK(cost_density == other.cost_density); COAL_EQUAL_OPERATOR_CHECK(threshold_occupied == other.threshold_occupied); COAL_EQUAL_OPERATOR_CHECK(threshold_free == other.threshold_free); COAL_EQUAL_OPERATOR_CHECK(aabb_center == other.aabb_center); COAL_EQUAL_OPERATOR_CHECK(aabb_radius == other.aabb_radius); COAL_EQUAL_OPERATOR_CHECK(aabb_local == other.aabb_local); COAL_EQUAL_OPERATOR_CHECK(isEqual(other)); return true; } /// \brief Difference operator bool operator!=(const CollisionGeometry& other) const { return isNotEqual(other); } /// @brief get the type of the object virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; } /// @brief get the node type virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; } /// @brief compute the AABB for object in local coordinate virtual void computeLocalAABB() = 0; /// @brief get user data in geometry void* getUserData() const { return user_data; } /// @brief set user data in geometry void setUserData(void* data) { user_data = data; } /// @brief whether the object is completely occupied inline bool isOccupied() const { return cost_density >= threshold_occupied; } /// @brief whether the object is completely free inline bool isFree() const { return cost_density <= threshold_free; } /// @brief whether the object has some uncertainty bool isUncertain() const; /// @brief AABB center in local coordinate Vec3s aabb_center; /// @brief AABB radius Scalar aabb_radius; /// @brief AABB in local coordinate, used for tight AABB when only translation /// transform AABB aabb_local; /// @brief pointer to user defined data specific to this object void* user_data; /// @brief collision cost for unit volume Scalar cost_density; /// @brief threshold for occupied ( >= is occupied) Scalar threshold_occupied; /// @brief threshold for free (<= is free) Scalar threshold_free; /// @brief compute center of mass virtual Vec3s computeCOM() const { return Vec3s::Zero(); } /// @brief compute the inertia matrix, related to the origin virtual Matrix3s computeMomentofInertia() const { return Matrix3s::Constant(NAN); } /// @brief compute the volume virtual Scalar computeVolume() const { return 0; } /// @brief compute the inertia matrix, related to the com virtual Matrix3s computeMomentofInertiaRelatedToCOM() const { Matrix3s C = computeMomentofInertia(); Vec3s com = computeCOM(); Scalar V = computeVolume(); return (Matrix3s() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], C(1, 0) + V * com[1] * com[0], C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0], C(2, 1) + V * com[2] * com[1], C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])) .finished(); } private: /// @brief equal operator with another object of derived type. virtual bool isEqual(const CollisionGeometry& other) const = 0; /// @brief not equal operator with another object of derived type. virtual bool isNotEqual(const CollisionGeometry& other) const { return !(*this == other); } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief the object for collision or distance computation, contains the /// geometry and the transform information class COAL_DLLAPI CollisionObject { public: CollisionObject(const shared_ptr& cgeom_, bool compute_local_aabb = true) : cgeom(cgeom_), user_data(nullptr) { init(compute_local_aabb); } CollisionObject(const shared_ptr& cgeom_, const Transform3s& tf, bool compute_local_aabb = true) : cgeom(cgeom_), t(tf), user_data(nullptr) { init(compute_local_aabb); } CollisionObject(const shared_ptr& cgeom_, const Matrix3s& R, const Vec3s& T, bool compute_local_aabb = true) : cgeom(cgeom_), t(R, T), user_data(nullptr) { init(compute_local_aabb); } bool operator==(const CollisionObject& other) const { COAL_EQUAL_OPERATOR_CHECK(shared_ptrs_are_equal(cgeom, other.cgeom)); COAL_EQUAL_OPERATOR_CHECK(t == other.t); COAL_EQUAL_OPERATOR_CHECK(user_data == other.user_data); return true; } bool operator!=(const CollisionObject& other) const { return !(*this == other); } ~CollisionObject() {} /// @brief get the type of the object OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); } /// @brief get the node type NODE_TYPE getNodeType() const { return cgeom->getNodeType(); } /// @brief get the AABB in world space const AABB& getAABB() const { return aabb; } /// @brief get the AABB in world space AABB& getAABB() { return aabb; } /// @brief compute the AABB in world space void computeAABB() { if (t.getRotation().isIdentity()) { aabb = translate(cgeom->aabb_local, t.getTranslation()); } else { aabb.min_ = aabb.max_ = t.getTranslation(); Vec3s min_world, max_world; for (int k = 0; k < 3; ++k) { min_world.array() = t.getRotation().row(k).array() * cgeom->aabb_local.min_.transpose().array(); max_world.array() = t.getRotation().row(k).array() * cgeom->aabb_local.max_.transpose().array(); aabb.min_[k] += (min_world.array().min)(max_world.array()).sum(); aabb.max_[k] += (min_world.array().max)(max_world.array()).sum(); } } } /// @brief get user data in object void* getUserData() const { return user_data; } /// @brief set user data in object void setUserData(void* data) { user_data = data; } /// @brief get translation of the object inline const Vec3s& getTranslation() const { return t.getTranslation(); } /// @brief get matrix rotation of the object inline const Matrix3s& getRotation() const { return t.getRotation(); } /// @brief get object's transform inline const Transform3s& getTransform() const { return t; } /// @brief set object's rotation matrix void setRotation(const Matrix3s& R) { t.setRotation(R); } /// @brief set object's translation void setTranslation(const Vec3s& T) { t.setTranslation(T); } /// @brief set object's transform void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); } /// @brief set object's transform void setTransform(const Transform3s& tf) { t = tf; } /// @brief whether the object is in local coordinate bool isIdentityTransform() const { return t.isIdentity(); } /// @brief set the object in local coordinate void setIdentityTransform() { t.setIdentity(); } /// @brief get shared pointer to collision geometry of the object instance const shared_ptr collisionGeometry() const { return cgeom; } /// @brief get shared pointer to collision geometry of the object instance const shared_ptr& collisionGeometry() { return cgeom; } /// @brief get raw pointer to collision geometry of the object instance const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); } /// @brief get raw pointer to collision geometry of the object instance CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); } /// @brief Associate a new CollisionGeometry /// /// @param[in] collision_geometry The new CollisionGeometry /// @param[in] compute_local_aabb Whether the local aabb of the input new has /// to be computed. /// void setCollisionGeometry( const shared_ptr& collision_geometry, bool compute_local_aabb = true) { if (collision_geometry.get() != cgeom.get()) { cgeom = collision_geometry; init(compute_local_aabb); } } protected: void init(bool compute_local_aabb = true) { if (cgeom) { if (compute_local_aabb) cgeom->computeLocalAABB(); computeAABB(); } } shared_ptr cgeom; Transform3s t; /// @brief AABB in global coordinate mutable AABB aabb; /// @brief pointer to user defined data specific to this object void* user_data; }; } // namespace coal #endif ================================================ FILE: include/coal/collision_utility.h ================================================ // Copyright (c) 2017 CNRS // Copyright (c) 2022 INRIA // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // // This file is part of Coal. // Coal 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 // 3 of the License, or (at your option) any later version. // // Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // Coal. If not, see . #ifndef COAL_COLLISION_UTILITY_H #define COAL_COLLISION_UTILITY_H #include "coal/collision_object.h" namespace coal { COAL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, const Transform3s& pose, const AABB& aabb); /** * \brief Returns the name associated to a NODE_TYPE */ inline const char* get_node_type_name(NODE_TYPE node_type) { static const char* node_type_name_all[] = { "BV_UNKNOWN", "BV_AABB", "BV_OBB", "BV_RSS", "BV_kIOS", "BV_OBBRSS", "BV_KDOP16", "BV_KDOP18", "BV_KDOP24", "GEOM_BOX", "GEOM_SPHERE", "GEOM_CAPSULE", "GEOM_CONE", "GEOM_CYLINDER", "GEOM_CONVEX", "GEOM_PLANE", "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE", "GEOM_ELLIPSOID", "HF_AABB", "HF_OBBRSS", "NODE_COUNT"}; return node_type_name_all[node_type]; } /** * \brief Returns the name associated to a OBJECT_TYPE */ inline const char* get_object_type_name(OBJECT_TYPE object_type) { static const char* object_type_name_all[] = { "OT_UNKNOWN", "OT_BVH", "OT_GEOM", "OT_OCTREE", "OT_HFIELD", "OT_COUNT"}; return object_type_name_all[object_type]; } } // namespace coal #endif // COAL_COLLISION_UTILITY_H ================================================ FILE: include/coal/contact_patch/contact_patch_simplifier.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2025, INRIA * All rights reserved. * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #ifndef COAL_CONTACT_PATCH_SIMPLIFIER_H #define COAL_CONTACT_PATCH_SIMPLIFIER_H #include #include #include #include "coal/collision_data.h" namespace coal { /// @brief Naive patch simplifier. /// Computes the area of all possible combinations. /// Returns the combination with the largest area. /// This is very expensive and only meant to be used in tests to check /// if other simplifiers return the correct answer. class COAL_DLLAPI ContactPatchSimplifierNaive { public: ContactPatchSimplifierNaive() = default; /// @brief Compute the maximum-area subset of `patch_in` with /// `target_vertices` points and store it in `patch_out`. void compute(const ContactPatch& patch_in, std::size_t target_vertices, ContactPatch& patch_out); /// @brief In-place simplification helper that replaces the polygon with the /// max-area subset. void simplify(ContactPatch& patch, std::size_t target_vertices); private: std::vector simplified_buffer_; }; /// @brief Dynamic-programming simplifier that preserves the largest possible /// polygon area with `target_vertices` points. class COAL_DLLAPI ContactPatchSimplifierMaxArea { public: ContactPatchSimplifierMaxArea() = default; /// @brief Compute the maximum-area subset of `patch_in` with /// `target_vertices` points and store it in `patch_out`. void compute(const ContactPatch& patch_in, std::size_t target_vertices, ContactPatch& patch_out); /// @brief In-place simplification helper that replaces the polygon with the /// max-area subset. void simplify(ContactPatch& patch, std::size_t target_vertices); private: std::vector simplified_buffer_; }; /// @brief Greedy contact patch simplifier based on the Visvalingam–Whyatt rule. class COAL_DLLAPI ContactPatchSimplifierGreedy { public: using Index = std::size_t; ContactPatchSimplifierGreedy() = default; /// @brief Compute a simplified approximation of `patch_in` and write it into /// `patch_out` while preserving the supporting frame metadata. void compute(const ContactPatch& patch_in, std::size_t target_vertices, ContactPatch& patch_out); /// @brief In-place simplification helper that replaces the polygon with a /// greedy approximation. void simplify(ContactPatch& patch, std::size_t target_vertices); private: struct HeapEntry { Index idx; Scalar area; Index version; }; std::vector simplified_buffer_; std::vector prev_; std::vector next_; std::vector removed_; std::vector versions_; std::vector heap_storage_; }; } // namespace coal #endif // COAL_CONTACT_PATCH_SIMPLIFIER_H ================================================ FILE: include/coal/contact_patch/contact_patch_solver.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #ifndef COAL_CONTACT_PATCH_SOLVER_H #define COAL_CONTACT_PATCH_SOLVER_H #include "coal/collision_data.h" #include "coal/logging.h" #include "coal/narrowphase/gjk.h" namespace coal { /// @brief Solver to compute contact patches, i.e. the intersection between two /// contact surfaces projected onto the shapes' separating plane. /// Otherwise said, a contact patch is simply the intersection between two /// support sets: the support set of shape S1 in direction `n` and the support /// set of shape S2 in direction `-n`, where `n` is the contact normal /// (satisfying the optimality conditions of GJK/EPA). /// @note A contact patch is **not** the support set of the Minkowski Difference /// in the direction of the normal. /// A contact patch is actually the support set of the Minkowski difference in /// the direction of the normal, i.e. the instersection of the shapes support /// sets as mentioned above. /// /// TODO(louis): algo improvement: /// - The clipping algo is currently n1 * n2; it can be done in n1 + n2. struct COAL_DLLAPI ContactPatchSolver { // Note: `ContactPatch` is an alias for `SupportSet`. // The two can be used interchangeably. using ShapeSupportData = details::ShapeSupportData; using SupportSetDirection = SupportSet::PatchDirection; /// @brief Support set function for shape si. /// @param[in] shape the shape. /// @param[in/out] support_set a support set of the shape. A support set is /// attached to a frame. All the points of the set computed by this function /// will be expressed in the local frame of the support set. The support set /// is computed in the direction of the positive z-axis if its direction is /// DEFAULT, negative z-axis if its direction is INVERTED. /// @param[in/out] hint for the support computation of ConvexBase shapes. Gets /// updated after calling the function onto ConvexBase shapes. /// @param[in/out] support_data for the support computation of ConvexBase /// shapes. Gets updated with visited vertices after calling the function onto /// ConvexBase shapes. /// @param[in] num_sampled_supports for shapes like cone or cylinders which /// have smooth non-strictly convex sides (their bases are circles), we need /// to know how many supports we sample from these sides. For any other shape, /// this parameter is not used. /// @param[in] tol the "thickness" of the support plane. Any point v which /// satisfies `max_{x in shape}(x.dot(dir)) - v.dot(dir) <= tol` is tol /// distant from the support plane and is added to the support set. typedef void (*SupportSetFunction)(const ShapeBase* shape, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t num_sampled_supports, Scalar tol); /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors. static constexpr size_t default_num_preallocated_supports = 16; /// @brief Number of points sampled for Cone and Cylinder when the normal is /// orthogonal to the shapes' basis. /// See @ref ContactPatchRequest::m_num_samples_curved_shapes for more /// details. size_t num_samples_curved_shapes; /// @brief Tolerance below which points are added to the shapes support sets. /// See @ref ContactPatchRequest::m_patch_tolerance for more details. Scalar patch_tolerance; /// @brief Support set function for shape s1. mutable SupportSetFunction supportFuncShape1; /// @brief Support set function for shape s2. mutable SupportSetFunction supportFuncShape2; /// @brief Temporary data to compute the support sets on each shape. mutable std::array supports_data; /// @brief Guess for the support sets computation. mutable support_func_guess_t support_guess; /// @brief Holder for support set of shape 1, used for internal computation. /// After `computePatch` has been called, this support set is no longer valid. mutable SupportSet support_set_shape1; /// @brief Holder for support set of shape 2, used for internal computation. /// After `computePatch` has been called, this support set is no longer valid. mutable SupportSet support_set_shape2; /// @brief Temporary support set used for the Sutherland-Hodgman algorithm. mutable SupportSet support_set_buffer; /// @brief Tracks which point of the Sutherland-Hodgman result have been added /// to the contact patch. Only used if the post-processing step occurs, i.e. /// if the result of Sutherland-Hodgman has a size bigger than /// `max_patch_size`. mutable std::vector added_to_patch; /// @brief Default constructor. explicit ContactPatchSolver() { const size_t num_contact_patch = 1; const size_t preallocated_patch_size = ContactPatch::default_preallocated_size; const Scalar patch_tolerance = Scalar(1e-3); const ContactPatchRequest request(num_contact_patch, preallocated_patch_size, patch_tolerance); this->set(request); } /// @brief Construct the solver with a `ContactPatchRequest`. explicit ContactPatchSolver(const ContactPatchRequest& request) { this->set(request); } /// @brief Set up the solver using a `ContactPatchRequest`. void set(const ContactPatchRequest& request); /// @brief Sets the support guess used during support set computation of /// shapes s1 and s2. void setSupportGuess(const support_func_guess_t guess) const { this->support_guess = guess; } /// @brief Main API of the solver: compute a contact patch from a contact /// between shapes s1 and s2. /// The contact patch is the (triple) intersection between the separating /// plane passing (by `contact.pos` and supported by `contact.normal`) and the /// shapes s1 and s2. template void computePatch(const ShapeType1& s1, const Transform3s& tf1, const ShapeType2& s2, const Transform3s& tf2, const Contact& contact, ContactPatch& contact_patch) const; /// @brief Reset the internal quantities of the solver. template void reset(const ShapeType1& shape1, const Transform3s& tf1, const ShapeType2& shape2, const Transform3s& tf2, const ContactPatch& contact_patch) const; /// @brief Retrieve result, adds a post-processing step if result has bigger /// size than `this->max_patch_size`. void getResult(const Contact& contact, const std::vector* result, ContactPatch& contact_patch) const; /// @return the intersecting point between line defined by ray (a, b) and /// the segment [c, d]. /// @note we make the following hypothesis: /// 1) c != d (should be when creating initial polytopes) /// 2) (c, d) is not parallel to ray -> if so, we return d. static Vec2s computeLineSegmentIntersection(const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d); /// @brief Construct support set function for shape. static SupportSetFunction makeSupportSetFunction( const ShapeBase* shape, ShapeSupportData& support_data); bool operator==(const ContactPatchSolver& other) const { COAL_EQUAL_OPERATOR_CHECK(this->num_samples_curved_shapes == other.num_samples_curved_shapes); COAL_EQUAL_OPERATOR_CHECK(this->patch_tolerance == other.patch_tolerance); COAL_EQUAL_OPERATOR_CHECK(this->support_guess == other.support_guess); COAL_EQUAL_OPERATOR_CHECK(this->support_set_shape1 == other.support_set_shape1); COAL_EQUAL_OPERATOR_CHECK(this->support_set_shape2 == other.support_set_shape2); COAL_EQUAL_OPERATOR_CHECK(this->support_set_buffer == other.support_set_buffer); COAL_EQUAL_OPERATOR_CHECK(this->added_to_patch == other.added_to_patch); COAL_EQUAL_OPERATOR_CHECK(this->supportFuncShape1 == other.supportFuncShape1); COAL_EQUAL_OPERATOR_CHECK(this->supportFuncShape2 == other.supportFuncShape2); return true; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace coal #include "coal/contact_patch/contact_patch_solver.hxx" #endif // COAL_CONTACT_PATCH_SOLVER_H ================================================ FILE: include/coal/contact_patch/contact_patch_solver.hxx ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #ifndef COAL_CONTACT_PATCH_SOLVER_HXX #define COAL_CONTACT_PATCH_SOLVER_HXX #include #include "coal/data_types.h" #include "coal/shape/geometric_shapes_traits.h" #include "coal/tracy.hh" namespace coal { // ============================================================================ inline void ContactPatchSolver::set(const ContactPatchRequest& request) { // Note: it's important for the number of pre-allocated Vec3s in // `m_clipping_sets` to be larger than `request.max_size_patch` // because we don't know in advance how many supports will be discarded to // form the convex-hulls of the shapes supports which will serve as the // input of the Sutherland-Hodgman algorithm. size_t num_preallocated_supports = default_num_preallocated_supports; if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) { num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes(); } // Used for support set computation of shape1 and for the first iterate of the // Sutherland-Hodgman algo. this->support_set_shape1.points().reserve(num_preallocated_supports); this->support_set_shape1.direction = SupportSetDirection::DEFAULT; // Used for computing the next iterate of the Sutherland-Hodgman algo. this->support_set_buffer.points().reserve(num_preallocated_supports); // Used for support set computation of shape2 and acts as the "clipper" set in // the Sutherland-Hodgman algo. this->support_set_shape2.points().reserve(num_preallocated_supports); this->support_set_shape2.direction = SupportSetDirection::INVERTED; this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); this->patch_tolerance = request.getPatchTolerance(); } // ============================================================================ template void ContactPatchSolver::computePatch(const ShapeType1& s1, const Transform3s& tf1, const ShapeType2& s2, const Transform3s& tf2, const Contact& contact, ContactPatch& contact_patch) const { COAL_TRACY_ZONE_SCOPED_N("coal::ContactPatchSolver::computePatch"); // Note: `ContactPatch` is an alias for `SupportSet`. // Step 1 constructContactPatchFrameFromContact(contact, contact_patch); contact_patch.points().clear(); if ((bool)(shape_traits::IsStrictlyConvex) || (bool)(shape_traits::IsStrictlyConvex)) { // If a shape is strictly convex, the support set in any direction is // reduced to a single point. Thus, the contact point `contact.pos` is the // only point belonging to the contact patch, and it has already been // computed. // TODO(louis): even for strictly convex shapes, we can sample the support // function around the normal and return a pseudo support set. This would // allow spheres and ellipsoids to have a contact surface, which does make // sense in certain physics simulation cases. // Do the same for strictly convex regions of non-strictly convex shapes // like the ends of capsules. contact_patch.addPoint(contact.pos); return; } // Step 2 - Compute support set of each shape, in the direction of // the contact's normal. // The first shape's support set is called "current"; it will be the first // iterate of the Sutherland-Hodgman algorithm. The second shape's support set // is called "clipper"; it will be used to clip "current". The support set // computation step computes a convex polygon; its vertices are ordered // counter-clockwise. This is important as the Sutherland-Hodgman algorithm // expects points to be ranked counter-clockwise. this->reset(s1, tf1, s2, tf2, contact_patch); assert(this->num_samples_curved_shapes > 3); this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0], this->supports_data[0], this->num_samples_curved_shapes, this->patch_tolerance); this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1], this->supports_data[1], this->num_samples_curved_shapes, this->patch_tolerance); const auto enforce_ccw_orientation = [](SupportSet& support_set) { auto& polygon = support_set.points(); if (polygon.size() < 3) { return; } Scalar twice_area = Scalar(0); for (size_t i = 0; i < polygon.size(); ++i) { const Vec2s& p = polygon[i]; const Vec2s& q = polygon[(i + 1) % polygon.size()]; twice_area += p(0) * q(1) - q(0) * p(1); } if (twice_area < Scalar(0)) { std::reverse(polygon.begin(), polygon.end()); } }; enforce_ccw_orientation(this->support_set_shape1); enforce_ccw_orientation(this->support_set_shape2); // We can immediatly return if one of the support set has only // one point. if (this->support_set_shape1.size() <= 1 || this->support_set_shape2.size() <= 1) { contact_patch.addPoint(contact.pos); return; } // `eps` is be used to check strict positivity of determinants. const Scalar eps = Eigen::NumTraits::dummy_precision(); using Polygon = std::vector; if ((this->support_set_shape1.size() == 2) && (this->support_set_shape2.size() == 2)) { // Segment-Segment case // We compute the determinant; if it is non-zero, the intersection // has already been computed: it's `Contact::pos`. const Polygon& pts1 = this->support_set_shape1.points(); const Vec2s& a = pts1[0]; const Vec2s& b = pts1[1]; const Polygon& pts2 = this->support_set_shape2.points(); const Vec2s& c = pts2[0]; const Vec2s& d = pts2[1]; const Scalar det = (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0)); if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) || ((b - a).squaredNorm() < eps)) { contact_patch.addPoint(contact.pos); return; } const Vec2s cd = (d - c); const Scalar l = cd.squaredNorm(); Polygon& patch = contact_patch.points(); // Project a onto [c, d] Scalar t1 = (a - c).dot(cd); t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l)); const Vec2s p1 = c + t1 * cd; patch.emplace_back(p1); // Project b onto [c, d] Scalar t2 = (b - c).dot(cd); t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l)); const Vec2s p2 = c + t2 * cd; if ((p1 - p2).squaredNorm() >= eps) { patch.emplace_back(p2); } return; } // // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the // "current" polygon. The resulting intersection is the contact patch of the // contact between s1 and s2. "clipper" and "current" are the support sets of // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to // shape1 and current to shape2, depending on which case we are). Currently, // to clip one polygon with the other, we use the Sutherland-Hodgman // algorithm: // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using // another polygon of size >=3. However, it can be easily extended to handle // the segment-polygon case. // // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 + // n2 where n1 and n2 are the sizes of the first and second polygon. const size_t max_result_size = this->support_set_shape1.size() + this->support_set_shape2.size(); if (this->added_to_patch.size() < max_result_size) { this->added_to_patch.assign(max_result_size, false); } const Polygon* clipper_ptr = nullptr; Polygon* current_ptr = nullptr; Polygon* previous_ptr = &(this->support_set_buffer.points()); // Let the clipper set be the one with the most vertices, to make sure it is // at least a triangle. if (this->support_set_shape1.size() < this->support_set_shape2.size()) { current_ptr = &(this->support_set_shape1.points()); clipper_ptr = &(this->support_set_shape2.points()); } else { current_ptr = &(this->support_set_shape2.points()); clipper_ptr = &(this->support_set_shape1.points()); } const Polygon& clipper = *(clipper_ptr); const size_t clipper_size = clipper.size(); for (size_t i = 0; i < clipper_size; ++i) { // Swap `current` and `previous`. // `previous` tracks the last iteration of the algorithm; `current` is // filled by clipping `current` using `clipper`. Polygon* tmp_ptr = previous_ptr; previous_ptr = current_ptr; current_ptr = tmp_ptr; const Polygon& previous = *(previous_ptr); Polygon& current = *(current_ptr); current.clear(); const Vec2s& a = clipper[i]; const Vec2s& b = clipper[(i + 1) % clipper_size]; const Vec2s ab = b - a; if (previous.size() == 2) { // // Segment-Polygon case // const Vec2s& p1 = previous[0]; const Vec2s& p2 = previous[1]; const Vec2s ap1 = p1 - a; const Vec2s ap2 = p2 - a; const Scalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); const Scalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); if (det1 < 0 && det2 < 0) { // Both p1 and p2 are outside the clipping polygon, i.e. there is no // intersection. The algorithm can stop. break; } if (det1 >= 0 && det2 >= 0) { // Both p1 and p2 are inside the clipping polygon, there is nothing to // do; move to the next iteration. current = previous; continue; } // Compute the intersection between the line (a, b) and the segment // [p1, p2]. if (det1 >= 0) { if (det1 > eps) { const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); current.emplace_back(p1); current.emplace_back(p); continue; } else { // p1 is the only point of current which is also a point of the // clipper. We can exit. current.emplace_back(p1); break; } } else { if (det2 > eps) { const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); current.emplace_back(p2); current.emplace_back(p); continue; } else { // p2 is the only point of current which is also a point of the // clipper. We can exit. current.emplace_back(p2); break; } } } else { // // Polygon-Polygon case. // std::fill(this->added_to_patch.begin(), // this->added_to_patch.end(), // false); const size_t previous_size = previous.size(); for (size_t j = 0; j < previous_size; ++j) { const Vec2s& p1 = previous[j]; const Vec2s& p2 = previous[(j + 1) % previous_size]; const Vec2s ap1 = p1 - a; const Vec2s ap2 = p2 - a; const Scalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); const Scalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); if (det1 < 0 && det2 < 0) { // No intersection. Continue to next segment of previous. continue; } if (det1 >= 0 && det2 >= 0) { // Both p1 and p2 are inside the clipping polygon, add p1 to current // (only if it has not already been added). if (!this->added_to_patch[j]) { current.emplace_back(p1); this->added_to_patch[j] = true; } // Continue to next segment of previous. continue; } if (det1 >= 0) { if (det1 > eps) { if (!this->added_to_patch[j]) { current.emplace_back(p1); this->added_to_patch[j] = true; } const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); current.emplace_back(p); } else { // a, b and p1 are colinear; we add only p1. if (!this->added_to_patch[j]) { current.emplace_back(p1); this->added_to_patch[j] = true; } } } else { if (det2 > eps) { const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); current.emplace_back(p); } else { if (!this->added_to_patch[(j + 1) % previous.size()]) { current.emplace_back(p2); this->added_to_patch[(j + 1) % previous.size()] = true; } } } } } // // End of iteration i of Sutherland-Hodgman. if (current.size() <= 1) { // No intersection or one point found, the algo can early stop. break; } } // Transfer the result of the Sutherland-Hodgman algorithm to the contact // patch. this->getResult(contact, current_ptr, contact_patch); } // ============================================================================ inline void ContactPatchSolver::getResult(const Contact& contact, const std::vector* result_ptr, ContactPatch& contact_patch) const { if (result_ptr->size() <= 1) { contact_patch.addPoint(contact.pos); return; } const std::vector& result = *(result_ptr); std::vector& patch = contact_patch.points(); patch = result; } // ============================================================================ template inline void ContactPatchSolver::reset(const ShapeType1& shape1, const Transform3s& tf1, const ShapeType2& shape2, const Transform3s& tf2, const ContactPatch& contact_patch) const { // Reset internal quantities this->support_set_shape1.clear(); this->support_set_shape2.clear(); this->support_set_buffer.clear(); // Get the support function of each shape const Transform3s& tfc = contact_patch.tf; this->support_set_shape1.direction = SupportSetDirection::DEFAULT; // Set the reference frame of the support set of the first shape to be the // local frame of shape 1. Transform3s& tf1c = this->support_set_shape1.tf; tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation(); tf1c.translation().noalias() = tf1.rotation().transpose() * (tfc.translation() - tf1.translation()); this->supportFuncShape1 = this->makeSupportSetFunction(&shape1, this->supports_data[0]); this->support_set_shape2.direction = SupportSetDirection::INVERTED; // Set the reference frame of the support set of the second shape to be the // local frame of shape 2. Transform3s& tf2c = this->support_set_shape2.tf; tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation(); tf2c.translation().noalias() = tf2.rotation().transpose() * (tfc.translation() - tf2.translation()); this->supportFuncShape2 = this->makeSupportSetFunction(&shape2, this->supports_data[1]); } // ========================================================================== inline Vec2s ContactPatchSolver::computeLineSegmentIntersection( const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) { const Vec2s ab = b - a; const Vec2s n(-ab(1), ab(0)); const Scalar denominator = n.dot(c - d); if (std::abs(denominator) < std::numeric_limits::epsilon()) { return d; } const Scalar nominator = n.dot(a - d); Scalar alpha = nominator / denominator; alpha = std::min(1.0, std::max(0.0, alpha)); return alpha * c + (1 - alpha) * d; } } // namespace coal #endif // COAL_CONTACT_PATCH_SOLVER_HXX ================================================ FILE: include/coal/contact_patch/polygon_convex_hull.h ================================================ // // Copyright (c) 2026 INRIA // #ifndef COAL_CONTACT_PATCH_POLYGON_CONVEX_HULL_H #define COAL_CONTACT_PATCH_POLYGON_CONVEX_HULL_H #include #include "coal/config.hh" #include "coal/data_types.h" namespace coal { // ----------------------------------------------------------- // Collection of various utilities useful in the Coal library. // ----------------------------------------------------------- /// @brief Compute the convex hull of a polygon. /// @note This function internally uses alloca to avoid memory allocation on the /// heap. Hence, this function is meant to be used on point clouds with a small /// amount of points. If you encounter problems with this method, feel free to /// raise an issue. COAL_DLLAPI void computePolygonConvexHull(const std::vector& cloud, std::vector& cvx_hull); } // namespace coal #endif // COAL_CONTACT_PATCH_POLYGON_CONVEX_HULL_H ================================================ FILE: include/coal/contact_patch.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #ifndef COAL_CONTACT_PATCH_H #define COAL_CONTACT_PATCH_H #include "coal/data_types.h" #include "coal/collision_data.h" #include "coal/contact_patch/contact_patch_solver.h" #include "coal/contact_patch_func_matrix.h" #include "coal/contact_patch/contact_patch_simplifier.h" namespace coal { /// @brief Main contact patch computation interface. /// @note Please see @ref ContactPatchRequest and @ref ContactPatchResult for /// more info on the content of the input/output of this function. Also, please /// read @ref ContactPatch if you want to fully understand what is meant by /// "contact patch". COAL_DLLAPI void computeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result); /// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3s&, // const CollisionGeometry*, const Transform3s&, const CollisionResult&, const // ContactPatchRequest&, ContactPatchResult&); COAL_DLLAPI void computeContactPatch(const CollisionObject* o1, const CollisionObject* o2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result); /// @brief This class reduces the cost of identifying the geometry pair. /// This is usefull for repeated shape-shape queries. /// @note This needs to be called after `collide` or after `ComputeCollision`. /// /// \code /// ComputeContactPatch calc_patch (o1, o2); /// calc_patch(tf1, tf2, collision_result, patch_request, patch_result); /// \endcode class COAL_DLLAPI ComputeContactPatch { public: /// @brief Default constructor from two Collision Geometries. ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2); void operator()(const Transform3s& tf1, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const; bool operator==(const ComputeContactPatch& other) const { COAL_EQUAL_OPERATOR_CHECK(((o1 == nullptr && other.o1 == nullptr) || (o1 != nullptr && other.o1 != nullptr))); COAL_EQUAL_OPERATOR_CHECK(((o2 == nullptr && other.o2 == nullptr) || (o2 != nullptr && other.o2 != nullptr))); if ((o1 && other.o1) && (o1 != other.o1)) COAL_EQUAL_OPERATOR_CHECK(*o1 == *other.o1); if ((o2 && other.o2) && (o2 != other.o2)) COAL_EQUAL_OPERATOR_CHECK(*o2 == *other.o2); COAL_EQUAL_OPERATOR_CHECK(csolver == other.csolver); COAL_EQUAL_OPERATOR_CHECK(func == other.func); COAL_EQUAL_OPERATOR_CHECK(swap_geoms == other.swap_geoms); return true; } bool operator!=(const ComputeContactPatch& other) const { return !(*this == other); } virtual ~ComputeContactPatch() = default; protected: // These pointers are made mutable to let the derived classes to update // their values when updating the collision geometry (e.g. creating a new // one). This feature should be used carefully to avoid any mis usage (e.g, // changing the type of the collision geometry should be avoided). mutable const CollisionGeometry* o1; mutable const CollisionGeometry* o2; mutable ContactPatchSolver csolver; ContactPatchFunctionMatrix::ContactPatchFunc func; bool swap_geoms; virtual void run(const Transform3s& tf1, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace coal #endif ================================================ FILE: include/coal/contact_patch_func_matrix.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #ifndef COAL_CONTACT_PATCH_FUNC_MATRIX_H #define COAL_CONTACT_PATCH_FUNC_MATRIX_H #include "coal/collision_data.h" #include "coal/contact_patch/contact_patch_solver.h" #include "coal/narrowphase/narrowphase.h" namespace coal { /// @brief The contact patch matrix stores the functions for contact patches /// computation between different types of objects and provides a uniform call /// interface struct COAL_DLLAPI ContactPatchFunctionMatrix { /// @brief the uniform call interface for computing contact patches: we need /// know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 /// and tf2; /// 2. the collision result that generated contact patches candidates /// (`coal::Contact`), from which contact patches will be expanded; /// 3. the solver for computation of contact patches; /// 4. the request setting for contact patches (e.g. maximum amount of /// patches, patch tolerance etc.) /// 5. the structure to return contact patches /// (`coal::ContactPatchResult`). /// /// Note: we pass a GJKSolver, because it allows to reuse internal computation /// that was made during the narrow phase. It also allows to experiment with /// different ways to compute contact patches. We could, for example, perturb /// tf1 and tf2 and make multiple calls to the GJKSolver (although this is not /// the approach done by default). typedef void (*ContactPatchFunc)(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result); /// @brief Each item in the contact patch matrix is a function to handle /// contact patch computation between objects of type1 and type2. ContactPatchFunc contact_patch_matrix[NODE_COUNT][NODE_COUNT]; ContactPatchFunctionMatrix(); }; } // namespace coal #endif ================================================ FILE: include/coal/data_types.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2023, Inria * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_DATA_TYPES_H #define COAL_DATA_TYPES_H #include #include #include "coal/config.hh" #include "coal/deprecated.hh" #include "coal/fwd.hh" #ifdef COAL_HAS_OCTOMAP #define OCTOMAP_VERSION_AT_LEAST(x, y, z) \ (OCTOMAP_MAJOR_VERSION > x || \ (OCTOMAP_MAJOR_VERSION >= x && \ (OCTOMAP_MINOR_VERSION > y || \ (OCTOMAP_MINOR_VERSION >= y && OCTOMAP_PATCH_VERSION >= z)))) #define OCTOMAP_VERSION_AT_MOST(x, y, z) \ (OCTOMAP_MAJOR_VERSION < x || \ (OCTOMAP_MAJOR_VERSION <= x && \ (OCTOMAP_MINOR_VERSION < y || \ (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z)))) #endif // COAL_HAS_OCTOMAP namespace coal { #ifdef COAL_USE_FLOAT_PRECISION COAL_DEPRECATED typedef float CoalScalar; typedef float Scalar; #else COAL_DEPRECATED typedef double CoalScalar; typedef double Scalar; #endif typedef Eigen::Matrix Vec3s; typedef Eigen::Matrix Vec2s; typedef Eigen::Matrix Vec6s; typedef Eigen::Matrix VecXs; typedef Eigen::Matrix Matrix3s; typedef Eigen::Matrix MatrixX3s; typedef Eigen::Matrix MatrixX2s; typedef Eigen::Matrix Matrixx3i; typedef Eigen::Matrix MatrixXs; typedef Eigen::Vector2i support_func_guess_t; typedef double SolverScalar; typedef Eigen::Matrix Vec3ps; #ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL // We keep the FCL_REAL typedef and the Vec[..]f typedefs for backward // compatibility. typedef Scalar FCL_REAL; typedef Vec3s Vec3f; typedef Vec2s Vec2f; typedef Vec6s Vec6f; typedef VecXs VecXf; typedef Matrix3s Matrix3f; typedef MatrixX3s Matrixx3f; typedef MatrixX2s Matrixx2f; typedef MatrixXs MatrixXf; #endif /// @brief Initial guess to use for the GJK algorithm /// DefaultGuess: Vec3s(1, 0, 0) /// CachedGuess: previous vector found by GJK or guess cached by the user /// BoundingVolumeGuess: guess using the centers of the shapes' AABB /// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called /// on the two shapes. enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess }; /// @brief Variant to use for the GJK algorithm enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration }; /// @brief Which convergence criterion is used to stop the algorithm (when the /// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and /// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe /// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG. enum GJKConvergenceCriterion { Default, DualityGap, Hybrid }; /// @brief Wether the convergence criterion is scaled on the norm of the /// solution or not enum GJKConvergenceCriterionType { Relative, Absolute }; /// @brief Triangle with 3 indices for points template class TriangleTpl { public: // clang-format off COAL_DEPRECATED_MESSAGE(Use IndexType instead.) typedef _IndexType index_type; // clang-format on typedef _IndexType IndexType; typedef int size_type; template friend class TriangleTpl; /// @brief Default constructor TriangleTpl() {} /// @brief Copy constructor TriangleTpl(const TriangleTpl& other) { *this = other; } /// @brief Create a triangle with given vertex indices TriangleTpl(IndexType p1, IndexType p2, IndexType p3) { set(p1, p2, p3); } /// @brief Copy constructor from another vertex index type. template TriangleTpl(const TriangleTpl& other) { *this = other; } /// @brief Copy operator TriangleTpl& operator=(const TriangleTpl& other) { this->set(other.vids[0], other.vids[1], other.vids[2]); return *this; } /// @brief Copy operator from another index type. template TriangleTpl& operator=(const TriangleTpl& other) { *this = other.template cast(); return *this; } template TriangleTpl cast() const { TriangleTpl res; res.set(OtherIndexType(this->vids[0]), OtherIndexType(this->vids[1]), OtherIndexType(this->vids[2])); return res; } /// @brief Set the vertex indices of the triangle inline void set(IndexType p1, IndexType p2, IndexType p3) { vids[0] = p1; vids[1] = p2; vids[2] = p3; } /// @brief Access the triangle index inline IndexType operator[](IndexType i) const { return vids[i]; } inline IndexType& operator[](IndexType i) { return vids[i]; } static inline size_type size() { return 3; } bool operator==(const TriangleTpl& other) const { COAL_EQUAL_OPERATOR_CHECK(vids[0] == other.vids[0]); COAL_EQUAL_OPERATOR_CHECK(vids[1] == other.vids[1]); COAL_EQUAL_OPERATOR_CHECK(vids[2] == other.vids[2]); return true; } bool operator!=(const TriangleTpl& other) const { return !(*this == other); } bool isValid() const { return vids[0] != (std::numeric_limits::max)() && vids[1] != (std::numeric_limits::max)() && vids[2] != (std::numeric_limits::max)(); } protected: /// @brief indices for each vertex of triangle IndexType vids[3] = {(std::numeric_limits::max)(), (std::numeric_limits::max)(), (std::numeric_limits::max)()}; }; typedef TriangleTpl Triangle16; // typedef TriangleTpl Triangle32; // COAL_DEPRECATED_MESSAGE(Use Triangle32 instead.) typedef Triangle32 Triangle; /// @brief Quadrilateral with 4 indices for points template struct QuadrilateralTpl { // clang-format off COAL_DEPRECATED_MESSAGE(Use IndexType instead.) typedef _IndexType index_type; // clang-format on typedef _IndexType IndexType; typedef int size_type; /// @brief Default constructor QuadrilateralTpl() {} /// @brief Copy constructor QuadrilateralTpl(const QuadrilateralTpl& other) { *this = other; } /// @brief Copy constructor from another vertex index type. template QuadrilateralTpl(const QuadrilateralTpl& other) { *this = other; } /// @brief Copy operator QuadrilateralTpl& operator=(const QuadrilateralTpl& other) { this->set(other.vids[0], other.vids[1], other.vids[2], other.vids[3]); return *this; } /// @brief Copy operator from another index type. template QuadrilateralTpl& operator=(const QuadrilateralTpl& other) { *this = other.template cast(); return *this; } template QuadrilateralTpl cast() const { QuadrilateralTpl res; res.set(OtherIndexType(this->vids[0]), OtherIndexType(this->vids[1]), OtherIndexType(this->vids[2]), OtherIndexType(this->vids[3])); return res; } QuadrilateralTpl(IndexType p0, IndexType p1, IndexType p2, IndexType p3) { set(p0, p1, p2, p3); } /// @brief Set the vertex indices of the quadrilateral inline void set(IndexType p0, IndexType p1, IndexType p2, IndexType p3) { vids[0] = p0; vids[1] = p1; vids[2] = p2; vids[3] = p3; } /// @access the quadrilateral index inline IndexType operator[](IndexType i) const { return vids[i]; } inline IndexType& operator[](IndexType i) { return vids[i]; } static inline size_type size() { return 4; } bool operator==(const QuadrilateralTpl& other) const { COAL_EQUAL_OPERATOR_CHECK(vids[0] == other.vids[0]); COAL_EQUAL_OPERATOR_CHECK(vids[1] == other.vids[1]); COAL_EQUAL_OPERATOR_CHECK(vids[2] == other.vids[2]); COAL_EQUAL_OPERATOR_CHECK(vids[3] == other.vids[3]); return true; } bool operator!=(const QuadrilateralTpl& other) const { return !(*this == other); } protected: IndexType vids[4]; }; typedef QuadrilateralTpl Quadrilateral16; // typedef QuadrilateralTpl Quadrilateral32; // COAL_DEPRECATED_MESSAGE(Use Quadrilateral32 instead.) typedef Quadrilateral32 Quadrilateral; } // namespace coal #endif ================================================ FILE: include/coal/distance.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_DISTANCE_H #define COAL_DISTANCE_H #include "coal/collision_object.h" #include "coal/collision_data.h" #include "coal/distance_func_matrix.h" #include "coal/timings.h" namespace coal { /// @brief Main distance interface: given two collision objects, and the /// requirements for contacts, including whether return the nearest points, this /// function performs the distance between them. Return value is the minimum /// distance generated between the two objects. COAL_DLLAPI Scalar distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result); /// @copydoc distance(const CollisionObject*, const CollisionObject*, const /// DistanceRequest&, DistanceResult&) COAL_DLLAPI Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result); /// This class reduces the cost of identifying the geometry pair. /// This is mostly useful for repeated shape-shape queries. /// /// \code /// ComputeDistance calc_distance (o1, o2); /// Scalar distance = calc_distance(tf1, tf2, request, result); /// \endcode class COAL_DLLAPI ComputeDistance { public: ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2); Scalar operator()(const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) const; bool operator==(const ComputeDistance& other) const { COAL_EQUAL_OPERATOR_CHECK(((o1 == nullptr && other.o1 == nullptr) || (o1 != nullptr && other.o1 != nullptr))); COAL_EQUAL_OPERATOR_CHECK(((o2 == nullptr && other.o2 == nullptr) || (o2 != nullptr && other.o2 != nullptr))); if ((o1 && other.o1) && (o1 != other.o1)) COAL_EQUAL_OPERATOR_CHECK(*o1 == *other.o1); if ((o2 && other.o2) && (o2 != other.o2)) COAL_EQUAL_OPERATOR_CHECK(*o2 == *other.o2); COAL_EQUAL_OPERATOR_CHECK(swap_geoms == other.swap_geoms); COAL_EQUAL_OPERATOR_CHECK(solver == other.solver); COAL_EQUAL_OPERATOR_CHECK(func == other.func); return true; } bool operator!=(const ComputeDistance& other) const { return !(*this == other); } virtual ~ComputeDistance() {}; protected: // These pointers are made mutable to let the derived classes to update // their values when updating the collision geometry (e.g. creating a new // one). This feature should be used carefully to avoid any mis usage (e.g, // changing the type of the collision geometry should be avoided). mutable const CollisionGeometry* o1; mutable const CollisionGeometry* o2; mutable GJKSolver solver; DistanceFunctionMatrix::DistanceFunc func; bool swap_geoms; virtual Scalar run(const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) const; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace coal #endif ================================================ FILE: include/coal/distance_func_matrix.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_DISTANCE_FUNC_MATRIX_H #define COAL_DISTANCE_FUNC_MATRIX_H #include "coal/collision_object.h" #include "coal/collision_data.h" #include "coal/narrowphase/narrowphase.h" namespace coal { /// @brief distance matrix stores the functions for distance between different /// types of objects and provides a uniform call interface struct COAL_DLLAPI DistanceFunctionMatrix { /// @brief the uniform call interface for distance: for distance, we need know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 /// and tf2; /// 2. the solver for narrow phase collision, this is for distance computation /// between geometric shapes; /// 3. the request setting for distance (e.g., whether need to return nearest /// points); typedef Scalar (*DistanceFunc)(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result); /// @brief each item in the distance matrix is a function to handle distance /// between objects of type1 and type2 DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT]; DistanceFunctionMatrix(); }; } // namespace coal #endif ================================================ FILE: include/coal/doc.hh ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2014 CNRS-LAAS // Author: Florent Lamiraux // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS. nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. namespace coal { /// \mainpage /// \anchor coal_documentation /// /// \section coal_introduction Introduction /// /// Coal is a modified version the FCL libraries. /// /// It is a library for collision detection and distance computation between /// various types of geometric shapes reprensented either by /// \li basic shapes (coal::ShapeBase) like box, sphere, cylinders, ... /// \li or by bounding volume hierarchies of various types (coal::BVHModel) /// /// \par Using Coal /// /// The main entry points to the library are functions /// \li coal::collide(const CollisionObject*, const CollisionObject*, const /// CollisionRequest&, CollisionResult&) \li coal::distance(const /// CollisionObject*, const CollisionObject*, const DistanceRequest&, /// DistanceResult&) /// /// \section coal_collision_and_distance_lower_bound_computation Collision /// detection and distance lower bound /// /// Collision queries can return a distance lower bound between the two objects, /// which is computationally cheaper than computing the real distance. /// The following figure shows the returned result in /// CollisionResult::distance_lower_bound and DistanceResult::min_distance, /// with respect to the objects real distance. /// /// \image html distance_computation.png /// /// The two parameters refer to CollisionRequest::security_margin and /// CollisionRequest::break_distance. /// \note In the green hatched area, the distance lower bound is not known. It /// is only guaranted that it will be inferior to /// distance - security_margin and superior to \em break_distance. /// \note If CollisionRequest::security_margin is set to -inf, no collision test /// is performed by function coal::collide or class /// coal::ComputeCollision and the objects are considered as not /// colliding. } // namespace coal ================================================ FILE: include/coal/fwd.hh ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2014, CNRS-LAAS // Copyright (c) 2022-2024, Inria // Author: Florent Lamiraux // // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. // #ifndef COAL_FWD_HH #define COAL_FWD_HH #include #include #include #include #include "coal/config.hh" #include "coal/deprecated.hh" #include "coal/warning.hh" #if defined(_WIN32) #define COAL_PRETTY_FUNCTION __FUNCSIG__ #else #define COAL_PRETTY_FUNCTION __PRETTY_FUNCTION__ #endif #define COAL_UNUSED_VARIABLE(var) (void)(var) #ifdef NDEBUG #define COAL_ONLY_USED_FOR_DEBUG(var) COAL_UNUSED_VARIABLE(var) #else #define COAL_ONLY_USED_FOR_DEBUG(var) #endif #define COAL_THROW_PRETTY(message, exception) \ { \ std::stringstream ss; \ ss << "From file: " << __FILE__ << "\n"; \ ss << "in function: " << COAL_PRETTY_FUNCTION << "\n"; \ ss << "at line: " << __LINE__ << "\n"; \ ss << "message: " << message << "\n"; \ throw exception(ss.str()); \ } #define COAL_THROW_PRETTY_IF(condition, exception, message) \ if ((condition)) { \ COAL_THROW_PRETTY(message, exception); \ } /// @brief Macro to be used in an equality operator and return false if the /// boolean is false. This is useful for debugging as this macro can be /// overridden to throw an exception, print a message etc. #ifndef COAL_EQUAL_OPERATOR_CHECK #define COAL_EQUAL_OPERATOR_CHECK(boolean) \ if (!(boolean)) { \ return false; \ } #endif #ifdef COAL_TURN_ASSERT_INTO_EXCEPTION #define COAL_ASSERT(check, message, exception) \ do { \ if (!(check)) { \ COAL_THROW_PRETTY(message, exception); \ } \ } while (0) #else #define COAL_ASSERT(check, message, exception) \ { \ assert((check) && message); \ } #endif #if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) #define COAL_WITH_CXX11_SUPPORT #endif #if defined(__GNUC__) || defined(__clang__) #define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push") #define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop") #define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") // GCC version 4.6 and higher supports -Wmaybe-uninitialized #if (defined(__GNUC__) && \ ((__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))) #define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"") // Use __has_warning with clang. Clang introduced it in 2024 (3.5+) #elif (defined(__clang__) && defined(__has_warning) && \ __has_warning("-Wmaybe-uninitialized")) #define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"") #else #define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED #endif #elif defined(_WIN32) #define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)") #define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)") #define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ _Pragma("warning(disable : 4996)") #define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ _Pragma("warning(disable : 4700)") #else #define COAL_COMPILER_DIAGNOSTIC_PUSH #define COAL_COMPILER_DIAGNOSTIC_POP #define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED #endif // __GNUC__ namespace coal { using std::dynamic_pointer_cast; using std::make_shared; using std::shared_ptr; class CollisionObject; typedef shared_ptr CollisionObjectPtr_t; typedef shared_ptr CollisionObjectConstPtr_t; class CollisionGeometry; typedef shared_ptr CollisionGeometryPtr_t; typedef shared_ptr CollisionGeometryConstPtr_t; class Transform3s; class AABB; class BVHModelBase; typedef shared_ptr BVHModelPtr_t; class OcTree; typedef shared_ptr OcTreePtr_t; typedef shared_ptr OcTreeConstPtr_t; } // namespace coal #ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL namespace hpp { namespace fcl { using namespace coal; using Transform3f = Transform3s; // For backward compatibility } // namespace fcl } // namespace hpp #endif #endif // COAL_FWD_HH ================================================ FILE: include/coal/hfield.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Justin Carpentier */ #ifndef COAL_HEIGHT_FIELD_H #define COAL_HEIGHT_FIELD_H #include "coal/fwd.hh" #include "coal/data_types.h" #include "coal/collision_object.h" #include "coal/BV/BV_node.h" #include "coal/BVH/BVH_internal.h" #include namespace coal { /// @addtogroup Construction_Of_HeightField /// @{ struct COAL_DLLAPI HFNodeBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW enum class FaceOrientation { TOP = 1, BOTTOM = 1, NORTH = 2, EAST = 4, SOUTH = 8, WEST = 16 }; /// @brief An index for first child node or primitive /// If the value is positive, it is the index of the first child bv node /// If the value is negative, it is -(primitive index + 1) /// Zero is not used. size_t first_child; Eigen::DenseIndex x_id, x_size; Eigen::DenseIndex y_id, y_size; Scalar max_height; int contact_active_faces; /// @brief Default constructor HFNodeBase() : first_child(0), x_id(-1), x_size(0), y_id(-1), y_size(0), max_height(std::numeric_limits::lowest()), contact_active_faces(0) {} /// @brief Comparison operator bool operator==(const HFNodeBase& other) const { COAL_EQUAL_OPERATOR_CHECK(first_child == other.first_child); COAL_EQUAL_OPERATOR_CHECK(x_id == other.x_id); COAL_EQUAL_OPERATOR_CHECK(x_size == other.x_size); COAL_EQUAL_OPERATOR_CHECK(y_id == other.y_id); COAL_EQUAL_OPERATOR_CHECK(y_size == other.y_size); COAL_EQUAL_OPERATOR_CHECK(max_height == other.max_height); COAL_EQUAL_OPERATOR_CHECK(contact_active_faces == other.contact_active_faces); return true; } /// @brief Difference operator bool operator!=(const HFNodeBase& other) const { return !(*this == other); } /// @brief Whether current node is a leaf node (i.e. contains a primitive /// index) inline bool isLeaf() const { return x_size == 1 && y_size == 1; } /// @brief Return the index of the first child. The index is referred to the /// bounding volume array (i.e. bvs) in BVHModel inline size_t leftChild() const { return first_child; } /// @brief Return the index of the second child. The index is referred to the /// bounding volume array (i.e. bvs) in BVHModel inline size_t rightChild() const { return first_child + 1; } inline Eigen::Vector2i leftChildIndexes() const { return Eigen::Vector2i(x_id, y_id); } inline Eigen::Vector2i rightChildIndexes() const { return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2); } }; inline HFNodeBase::FaceOrientation operator&(HFNodeBase::FaceOrientation a, HFNodeBase::FaceOrientation b) { return HFNodeBase::FaceOrientation(int(a) & int(b)); } inline int operator&(int a, HFNodeBase::FaceOrientation b) { return a & int(b); } template struct COAL_DLLAPI HFNode : public HFNodeBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef HFNodeBase Base; /// @brief bounding volume storing the geometry BV bv; /// @brief Equality operator bool operator==(const HFNode& other) const { COAL_EQUAL_OPERATOR_CHECK(Base::operator==(other)); COAL_EQUAL_OPERATOR_CHECK(bv == other.bv); return true; } /// @brief Difference operator bool operator!=(const HFNode& other) const { return !(*this == other); } /// @brief Check whether two BVNode collide bool overlap(const HFNode& other) const { return bv.overlap(other.bv); } /// @brief Check whether two BVNode collide bool overlap(const HFNode& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const { return bv.overlap(other.bv, request, sqrDistLowerBound); } /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and /// the underlying BV supports distance, return the nearest points. Scalar distance(const HFNode& other, Vec3s* P1 = NULL, Vec3s* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } /// @brief Access to the center of the BV Vec3s getCenter() const { return bv.center(); } /// @brief Access to the orientation of the BV coal::Matrix3s::IdentityReturnType getOrientation() const { return Matrix3s::Identity(); } virtual ~HFNode() {} }; namespace details { template struct UpdateBoundingVolume { static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) { AABB bv_aabb(pointA, pointB); // AABB bv_aabb; // bv_aabb.update(pointA,pointB); convertBV(bv_aabb, bv); } }; template <> struct UpdateBoundingVolume { static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) { AABB bv_aabb(pointA, pointB); convertBV(bv_aabb, bv); // bv.update(pointA,pointB); } }; } // namespace details /// @brief Data structure depicting a height field given by the base grid /// dimensions and the elevation along the grid. \tparam BV one of the bounding /// volume class in \ref Bounding_Volume. /// /// An height field is defined by its base dimensions along the X and Y axes and /// a set ofpoints defined by their altitude, regularly dispatched on the grid. /// The height field is centered at the origin and the corners of the geometry /// correspond to the following coordinates [± x_dim/2; ± y_dim/2]. template class COAL_DLLAPI HeightField : public CollisionGeometry { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef CollisionGeometry Base; typedef HFNode Node; typedef std::vector> BVS; /// @brief Constructing an empty HeightField HeightField() : CollisionGeometry(), min_height((std::numeric_limits::min)()), max_height((std::numeric_limits::max)()) {} /// @brief Constructing an HeightField from its base dimensions and the set of /// heights points. /// The granularity of the height field along X and Y direction is /// extraded from the Z grid. /// /// \param[in] x_dim Dimension along the X axis /// \param[in] y_dim Dimension along the Y axis /// \param[in] heights Matrix containing the altitude of each point compositng /// the height field /// \param[in] min_height Minimal height of the height field /// HeightField(const Scalar x_dim, const Scalar y_dim, const MatrixXs& heights, const Scalar min_height = (Scalar)0) : CollisionGeometry() { init(x_dim, y_dim, heights, min_height); } /// @brief Copy contructor from another HeightField /// /// \param[in] other to copy. /// HeightField(const HeightField& other) : CollisionGeometry(other), x_dim(other.x_dim), y_dim(other.y_dim), heights(other.heights), min_height(other.min_height), max_height(other.max_height), x_grid(other.x_grid), y_grid(other.y_grid), bvs(other.bvs), num_bvs(other.num_bvs) {} /// @brief Returns a const reference of the grid along the X direction. const VecXs& getXGrid() const { return x_grid; } /// @brief Returns a const reference of the grid along the Y direction. const VecXs& getYGrid() const { return y_grid; } /// @brief Returns a const reference of the heights const MatrixXs& getHeights() const { return heights; } /// @brief Returns the dimension of the Height Field along the X direction. Scalar getXDim() const { return x_dim; } /// @brief Returns the dimension of the Height Field along the Y direction. Scalar getYDim() const { return y_dim; } /// @brief Returns the minimal height value of the Height Field. Scalar getMinHeight() const { return min_height; } /// @brief Returns the maximal height value of the Height Field. Scalar getMaxHeight() const { return max_height; } virtual HeightField* clone() const override { return new HeightField(*this); } const BVS& getNodes() const { return bvs; } /// @brief deconstruction, delete mesh data related. virtual ~HeightField() {} /// @brief Compute the AABB for the HeightField, used for broad-phase /// collision void computeLocalAABB() override { const Vec3s A(x_grid[0], y_grid[0], min_height); const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1], max_height); const AABB aabb_(A, B); aabb_radius = (A - B).norm() / Scalar(2); aabb_local = aabb_; aabb_center = aabb_.center(); } /// @brief Update Height Field height void updateHeights(const MatrixXs& new_heights) { if (new_heights.rows() != heights.rows() || new_heights.cols() != heights.cols()) COAL_THROW_PRETTY( "The matrix containing the new heights values does not have the same " "matrix size as the original one.\n" "\tinput values - rows: " << new_heights.rows() << " - cols: " << new_heights.cols() << "\n" << "\texpected values - rows: " << heights.rows() << " - cols: " << heights.cols() << "\n", std::invalid_argument); heights = new_heights.cwiseMax(min_height); this->max_height = recursiveUpdateHeight(0); assert(this->max_height == heights.maxCoeff()); } protected: void init(const Scalar x_dim, const Scalar y_dim, const MatrixXs& heights, const Scalar min_height) { this->x_dim = x_dim; this->y_dim = y_dim; this->heights = heights.cwiseMax(min_height); this->min_height = min_height; this->max_height = heights.maxCoeff(); const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows(); assert(NX >= 2 && "The number of columns is too small."); assert(NY >= 2 && "The number of rows is too small."); const Scalar half = Scalar(0.5); x_grid = VecXs::LinSpaced(NX, -half * x_dim, half * x_dim); y_grid = VecXs::LinSpaced(NY, half * y_dim, -half * y_dim); // Allocate BVS const size_t num_tot_bvs = (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1)); bvs.resize(num_tot_bvs); num_bvs = 0; // Build tree buildTree(); } /// @brief Get the object type: it is a HFIELD OBJECT_TYPE getObjectType() const override { return OT_HFIELD; } Vec3s computeCOM() const override { return Vec3s::Zero(); } Scalar computeVolume() const override { return 0; } Matrix3s computeMomentofInertia() const override { return Matrix3s::Zero(); } protected: /// @brief Dimensions in meters along X and Y directions Scalar x_dim, y_dim; /// @brief Elevation values in meters of the Height Field MatrixXs heights; /// @brief Minimal height of the Height Field: all values bellow min_height /// will be discarded. Scalar min_height, max_height; /// @brief Grids along the X and Y directions. Useful for plotting or other /// related things. VecXs x_grid, y_grid; /// @brief Bounding volume hierarchy BVS bvs; unsigned int num_bvs; /// @brief Build the bounding volume hierarchy int buildTree() { num_bvs = 1; const Scalar max_recursive_height = recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1); assert(max_recursive_height == max_height && "the maximal height is not correct"); COAL_UNUSED_VARIABLE(max_recursive_height); bvs.resize(num_bvs); return BVH_OK; } Scalar recursiveUpdateHeight(const size_t bv_id) { HFNode& bv_node = bvs[bv_id]; Scalar max_height; if (bv_node.isLeaf()) { max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff(); } else { Scalar max_left_height = recursiveUpdateHeight(bv_node.leftChild()), max_right_height = recursiveUpdateHeight(bv_node.rightChild()); max_height = (std::max)(max_left_height, max_right_height); } bv_node.max_height = max_height; const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size], y_grid[bv_node.y_id + bv_node.y_size], max_height); details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); return max_height; } Scalar recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id, const Eigen::DenseIndex x_size, const Eigen::DenseIndex y_id, const Eigen::DenseIndex y_size) { assert(x_id < heights.cols() && "x_id is out of bounds"); assert(y_id < heights.rows() && "y_id is out of bounds"); assert(x_size >= 0 && y_size >= 0 && "x_size or y_size are not of correct value"); assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension"); HFNode& bv_node = bvs[bv_id]; Scalar max_height; if (x_size == 1 && y_size == 1) // don't build any BV for the current child node { max_height = heights.block<2, 2>(y_id, x_id).maxCoeff(); } else { bv_node.first_child = num_bvs; num_bvs += 2; Scalar max_left_height = min_height, max_right_height = min_height; if (x_size >= y_size) // splitting along the X axis { Eigen::DenseIndex x_size_half = x_size / 2; if (x_size == 1) x_size_half = 1; max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size_half, y_id, y_size); max_right_height = recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half, x_size - x_size_half, y_id, y_size); } else // splitting along the Y axis { Eigen::DenseIndex y_size_half = y_size / 2; if (y_size == 1) y_size_half = 1; max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size, y_id, y_size_half); max_right_height = recursiveBuildTree(bv_node.rightChild(), x_id, x_size, y_id + y_size_half, y_size - y_size_half); } max_height = (std::max)(max_left_height, max_right_height); } bv_node.max_height = max_height; // max_height = std::max(max_height,min_height); const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height); assert(x_id + x_size < x_grid.size()); assert(y_id + y_size < y_grid.size()); const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], max_height); details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); bv_node.x_id = x_id; bv_node.y_id = y_id; bv_node.x_size = x_size; bv_node.y_size = y_size; if (bv_node.isLeaf()) { int& contact_active_faces = bv_node.contact_active_faces; contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP); contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM); if (bv_node.x_id == 0) // first col contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST); if (bv_node.y_id == 0) // first row (TOP) contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH); if (bv_node.x_id + 1 == heights.cols() - 1) // last col contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST); if (bv_node.y_id + 1 == heights.rows() - 1) // last row (BOTTOM) contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH); } return max_height; } public: /// @brief Access the bv giving the its index const HFNode& getBV(unsigned int i) const { if (i >= num_bvs) COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument); return bvs[i]; } /// @brief Access the bv giving the its index HFNode& getBV(unsigned int i) { if (i >= num_bvs) COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument); return bvs[i]; } /// @brief Get the BV type: default is unknown NODE_TYPE getNodeType() const override { return BV_UNKNOWN; } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const HeightField* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const HeightField& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(x_dim == other.x_dim); COAL_EQUAL_OPERATOR_CHECK(y_dim == other.y_dim); COAL_EQUAL_OPERATOR_CHECK(heights == other.heights); COAL_EQUAL_OPERATOR_CHECK(min_height == other.min_height); COAL_EQUAL_OPERATOR_CHECK(max_height == other.max_height); COAL_EQUAL_OPERATOR_CHECK(x_grid == other.x_grid); COAL_EQUAL_OPERATOR_CHECK(y_grid == other.y_grid); COAL_EQUAL_OPERATOR_CHECK(bvs == other.bvs); COAL_EQUAL_OPERATOR_CHECK(num_bvs == other.num_bvs); return true; } }; /// @brief Specialization of getNodeType() for HeightField with different BV /// types template <> NODE_TYPE HeightField::getNodeType() const; template <> NODE_TYPE HeightField::getNodeType() const; template <> NODE_TYPE HeightField::getNodeType() const; template <> NODE_TYPE HeightField::getNodeType() const; template <> NODE_TYPE HeightField::getNodeType() const; template <> NODE_TYPE HeightField>::getNodeType() const; template <> NODE_TYPE HeightField>::getNodeType() const; template <> NODE_TYPE HeightField>::getNodeType() const; /// @} } // namespace coal #endif ================================================ FILE: include/coal/internal/BV_fitter.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_BV_FITTER_H #define COAL_BV_FITTER_H #include "coal/BVH/BVH_internal.h" #include "coal/BV/kIOS.h" #include "coal/BV/OBBRSS.h" #include "coal/BV/AABB.h" #include namespace coal { /// @brief Compute a bounding volume that fits a set of n points. template void fit(Vec3s* ps, unsigned int n, BV& bv) { for (unsigned int i = 0; i < n; ++i) // TODO(jcarpent): vectorize { bv += ps[i]; } } template <> void fit(Vec3s* ps, unsigned int n, OBB& bv); template <> void fit(Vec3s* ps, unsigned int n, RSS& bv); template <> void fit(Vec3s* ps, unsigned int n, kIOS& bv); template <> void fit(Vec3s* ps, unsigned int n, OBBRSS& bv); template <> void fit(Vec3s* ps, unsigned int n, AABB& bv); /// @brief The class for the default algorithm fitting a bounding volume to a /// set of points template class COAL_DLLAPI BVFitterTpl { public: /// @brief default deconstructor virtual ~BVFitterTpl() {} /// @brief Prepare the geometry primitive data for fitting void set(Vec3s* vertices_, Triangle32* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = NULL; tri_indices = tri_indices_; type = type_; } /// @brief Prepare the geometry primitive data for fitting, for deformable /// mesh void set(Vec3s* vertices_, Vec3s* prev_vertices_, Triangle32* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = prev_vertices_; tri_indices = tri_indices_; type = type_; } /// @brief Compute the fitting BV virtual BV fit(unsigned int* primitive_indices, unsigned int num_primitives) = 0; /// @brief Clear the geometry primitive data void clear() { vertices = NULL; prev_vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } protected: Vec3s* vertices; Vec3s* prev_vertices; Triangle32* tri_indices; BVHModelType type; }; /// @brief The class for the default algorithm fitting a bounding volume to a /// set of points template class COAL_DLLAPI BVFitter : public BVFitterTpl { typedef BVFitterTpl Base; public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and /// primitive_indices is the primitive index relative to the data BV fit(unsigned int* primitive_indices, unsigned int num_primitives) { BV bv; if (type == BVH_MODEL_TRIANGLES) /// The primitive is triangle { for (unsigned int i = 0; i < num_primitives; ++i) { Triangle32 t = tri_indices[primitive_indices[i]]; bv += vertices[t[0]]; bv += vertices[t[1]]; bv += vertices[t[2]]; if (prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[t[0]]; bv += prev_vertices[t[1]]; bv += prev_vertices[t[2]]; } } } else if (type == BVH_MODEL_POINTCLOUD) /// The primitive is point { for (unsigned int i = 0; i < num_primitives; ++i) { bv += vertices[primitive_indices[i]]; if (prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[primitive_indices[i]]; } } } return bv; } protected: using Base::prev_vertices; using Base::tri_indices; using Base::type; using Base::vertices; }; /// @brief Specification of BVFitter for OBB bounding volume template <> class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and /// primitive_indices is the primitive index relative to the data. OBB fit(unsigned int* primitive_indices, unsigned int num_primitives); }; /// @brief Specification of BVFitter for RSS bounding volume template <> class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and /// primitive_indices is the primitive index relative to the data. RSS fit(unsigned int* primitive_indices, unsigned int num_primitives); }; /// @brief Specification of BVFitter for kIOS bounding volume template <> class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and /// primitive_indices is the primitive index relative to the data. kIOS fit(unsigned int* primitive_indices, unsigned int num_primitives); }; /// @brief Specification of BVFitter for OBBRSS bounding volume template <> class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and /// primitive_indices is the primitive index relative to the data. OBBRSS fit(unsigned int* primitive_indices, unsigned int num_primitives); }; /// @brief Specification of BVFitter for AABB bounding volume template <> class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and /// primitive_indices is the primitive index relative to the data. AABB fit(unsigned int* primitive_indices, unsigned int num_primitives); }; } // namespace coal #endif ================================================ FILE: include/coal/internal/BV_splitter.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_BV_SPLITTER_H #define COAL_BV_SPLITTER_H #include "coal/BVH/BVH_internal.h" #include "coal/BV/kIOS.h" #include "coal/BV/OBBRSS.h" #include #include namespace coal { /// @brief Three types of split algorithms are provided in FCL as default enum SplitMethodType { SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER }; /// @brief A class describing the split rule that splits each BV node template class BVSplitter { public: BVSplitter(SplitMethodType method) : split_vector(0, 0, 0), split_method(method) {} /// @brief Default deconstructor virtual ~BVSplitter() {} /// @brief Set the geometry data needed by the split rule void set(Vec3s* vertices_, Triangle32* tri_indices_, BVHModelType type_) { vertices = vertices_; tri_indices = tri_indices_; type = type_; } /// @brief Compute the split rule according to a subset of geometry and the /// corresponding BV node void computeRule(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives) { switch (split_method) { case SPLIT_METHOD_MEAN: computeRule_mean(bv, primitive_indices, num_primitives); break; case SPLIT_METHOD_MEDIAN: computeRule_median(bv, primitive_indices, num_primitives); break; case SPLIT_METHOD_BV_CENTER: computeRule_bvcenter(bv, primitive_indices, num_primitives); break; default: std::cerr << "Split method not supported" << std::endl; } } /// @brief Apply the split rule on a given point bool apply(const Vec3s& q) const { return q[split_axis] > split_value; } /// @brief Clear the geometry data set before void clear() { vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } protected: /// @brief The axis based on which the split decision is made. For most BV, /// the axis is aligned with one of the world coordinate, so only split_axis /// is needed. For oriented node, we can use a vector to make a better split /// decision. int split_axis; Vec3s split_vector; /// @brief The split threshold, different primitives are splitted according /// whether their projection on the split_axis is larger or smaller than the /// threshold Scalar split_value; /// @brief The mesh vertices or points handled by the splitter Vec3s* vertices; /// @brief The triangles handled by the splitter Triangle32* tri_indices; /// @brief Whether the geometry is mesh or point cloud BVHModelType type; /// @brief The split algorithm used SplitMethodType split_method; /// @brief Split algorithm 1: Split the node from center void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) { Vec3s center = bv.center(); int axis = 2; if (bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; split_value = center[axis]; } /// @brief Split algorithm 2: Split the node according to the mean of the data /// contained void computeRule_mean(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives) { int axis = 2; if (bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; Scalar sum = 0; if (type == BVH_MODEL_TRIANGLES) { for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle32& t = tri_indices[primitive_indices[i]]; sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]); } sum /= 3; } else if (type == BVH_MODEL_POINTCLOUD) { for (unsigned int i = 0; i < num_primitives; ++i) { sum += vertices[primitive_indices[i]][split_axis]; } } split_value = sum / Scalar(num_primitives); } /// @brief Split algorithm 3: Split the node according to the median of the /// data contained void computeRule_median(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives) { int axis = 2; if (bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; std::vector proj((size_t)num_primitives); if (type == BVH_MODEL_TRIANGLES) { for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle32& t = tri_indices[primitive_indices[i]]; proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3; } } else if (type == BVH_MODEL_POINTCLOUD) { for (unsigned int i = 0; i < num_primitives; ++i) proj[i] = vertices[primitive_indices[i]][split_axis]; } std::sort(proj.begin(), proj.end()); if (num_primitives % 2 == 1) { split_value = proj[(num_primitives - 1) / 2]; } else { split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; } } }; template <> bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; template <> bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; template <> bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; template <> bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; template <> void COAL_DLLAPI BVSplitter::computeRule_bvcenter( const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_mean( const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_median( const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_bvcenter( const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_mean( const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_median( const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_bvcenter( const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_mean( const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_median( const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_bvcenter( const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_mean( const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> void COAL_DLLAPI BVSplitter::computeRule_median( const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); } // namespace coal #endif ================================================ FILE: include/coal/internal/intersect.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_INTERSECT_H #define COAL_INTERSECT_H /// @cond INTERNAL #include "coal/math/transform.h" namespace coal { /// @brief CCD intersect kernel among primitives class COAL_DLLAPI Intersect { public: static bool buildTrianglePlane(const Vec3s& v1, const Vec3s& v2, const Vec3s& v3, Vec3s* n, Scalar* t); }; // class Intersect /// @brief Project functions template class Project { public: typedef _Scalar Scalar; typedef Eigen::Matrix Vec3; struct ProjectResult { /// @brief Parameterization of the projected point (based on the simplex to /// be projected, use 2 or 3 or 4 of the array) Scalar parameterization[4]; /// @brief square distance from the query point to the projected simplex Scalar sqr_distance; /// @brief the code of the projection type unsigned int encode; ProjectResult() : sqr_distance(-1), encode(0) {} }; /// @brief Project point p onto line a-b static ProjectResult projectLine(const Vec3& a, const Vec3& b, const Vec3& p); /// @brief Project point p onto triangle a-b-c static ProjectResult projectTriangle(const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& p); /// @brief Project point p onto tetrahedra a-b-c-d static ProjectResult projectTetrahedra(const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& d, const Vec3& p); /// @brief Project origin (0) onto line a-b static ProjectResult projectLineOrigin(const Vec3& a, const Vec3& b); /// @brief Project origin (0) onto triangle a-b-c static ProjectResult projectTriangleOrigin(const Vec3& a, const Vec3& b, const Vec3& c); /// @brief Project origin (0) onto tetrahedran a-b-c-d static ProjectResult projectTetrahedraOrigin(const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& d); }; /// @brief Triangle distance functions class COAL_DLLAPI TriangleDistance { public: /// @brief Returns closest points between an segment pair. /// The first segment is P + t * A /// The second segment is Q + t * B /// X, Y are the closest points on the two segments /// VEC is the vector between X and Y static void segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q, const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y); /// Compute squared distance between triangles /// @param S and T are two triangles /// @retval P, Q closest points if triangles do not intersect. /// @return squared distance if triangles do not intersect, 0 otherwise. /// If the triangles are disjoint, P and Q give the closet points of /// S and T respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. static Scalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P, Vec3s& Q); static Scalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, const Vec3s& S3, const Vec3s& T1, const Vec3s& T2, const Vec3s& T3, Vec3s& P, Vec3s& Q); /// Compute squared distance between triangles /// @param S and T are two triangles /// @param R, Tl, rotation and translation applied to T, /// @retval P, Q closest points if triangles do not intersect. /// @return squared distance if triangles do not intersect, 0 otherwise. /// If the triangles are disjoint, P and Q give the closet points of /// S and T respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. static Scalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], const Matrix3s& R, const Vec3s& Tl, Vec3s& P, Vec3s& Q); /// Compute squared distance between triangles /// @param S and T are two triangles /// @param tf, rotation and translation applied to T, /// @retval P, Q closest points if triangles do not intersect. /// @return squared distance if triangles do not intersect, 0 otherwise. /// If the triangles are disjoint, P and Q give the closet points of /// S and T respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. static Scalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], const Transform3s& tf, Vec3s& P, Vec3s& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices /// @param R, Tl, rotation and translation applied to T1, T2, T3, /// @retval P, Q closest points if triangles do not intersect. /// @return squared distance if triangles do not intersect, 0 otherwise. /// If the triangles are disjoint, P and Q give the closet points of /// S and T respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. static Scalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, const Vec3s& S3, const Vec3s& T1, const Vec3s& T2, const Vec3s& T3, const Matrix3s& R, const Vec3s& Tl, Vec3s& P, Vec3s& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices /// @param tf, rotation and translation applied to T1, T2, T3, /// @retval P, Q closest points if triangles do not intersect. /// @return squared distance if triangles do not intersect, 0 otherwise. /// If the triangles are disjoint, P and Q give the closet points of /// S and T respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. static Scalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, const Vec3s& S3, const Vec3s& T1, const Vec3s& T2, const Vec3s& T3, const Transform3s& tf, Vec3s& P, Vec3s& Q); }; } // namespace coal /// @endcond #include "coal/internal/intersect.hxx" #endif ================================================ FILE: include/coal/internal/intersect.hxx ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_INTERSECT_HXX #define COAL_INTERSECT_HXX #include "coal/internal/intersect.h" #include "coal/internal/tools.h" #include #include #include #include namespace coal { template inline typename Project::ProjectResult Project::projectLine( const Vec3& a, const Vec3& b, const Vec3& p) { ProjectResult res; const Vec3 d = b - a; const Scalar l = d.squaredNorm(); if (l > 0) { const Scalar t = (p - a).dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; if (t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } else if (t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } } return res; } template inline typename Project::ProjectResult Project::projectTriangle( const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& p) { ProjectResult res; static const size_t nexti[3] = {1, 2, 0}; const Vec3* vt[] = {&a, &b, &c}; const Vec3 dl[] = {a - b, b - c, c - a}; const Vec3& n = dl[0].cross(dl[1]); const Scalar l = n.squaredNorm(); if (l > 0) { Scalar mindist = -1; for (size_t i = 0; i < 3; ++i) { if ((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the // optimal can only be on the edge { size_t j = nexti[i]; ProjectResult res_line = projectLine(*vt[i], *vt[j], p); if (mindist < 0 || res_line.sqr_distance < mindist) { mindist = res_line.sqr_distance; res.encode = static_cast(((res_line.encode & 1) ? 1 << i : 0) + ((res_line.encode & 2) ? 1 << j : 0)); res.parameterization[i] = res_line.parameterization[0]; res.parameterization[j] = res_line.parameterization[1]; res.parameterization[nexti[j]] = 0; } } } if (mindist < 0) // the origin project is within the triangle { Scalar d = (a - p).dot(n); Scalar s = std::sqrt(l); Vec3 p_to_project = n * (d / l); mindist = p_to_project.squaredNorm(); res.encode = 7; // m = 0x111 res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s; res.parameterization[1] = dl[2].cross(c - p - p_to_project).norm() / s; res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; } res.sqr_distance = mindist; } return res; } template inline typename Project::ProjectResult Project::projectTetrahedra(const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& d, const Vec3& p) { ProjectResult res; static const size_t nexti[] = {1, 2, 0}; const Vec3* vt[] = {&a, &b, &c, &d}; const Vec3 dl[3] = {a - d, b - d, c - d}; Scalar vl = triple(dl[0], dl[1], dl[2]); bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0; if (ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng // is false, then the last vertex in the tetrahedron // does not grow toward the origin (in fact origin is // on the other side of the abc face) { Scalar mindist = -1; for (size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; Scalar s = vl * (d - p).dot(dl[i].cross(dl[j])); if (s > 0) // the origin is to the outside part of a triangle face, then // the optimal can only be on the triangle face { ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); if (mindist < 0 || res_triangle.sqr_distance < mindist) { mindist = res_triangle.sqr_distance; res.encode = static_cast((res_triangle.encode & 1 ? 1 << i : 0) + (res_triangle.encode & 2 ? 1 << j : 0) + (res_triangle.encode & 4 ? 8 : 0)); res.parameterization[i] = res_triangle.parameterization[0]; res.parameterization[j] = res_triangle.parameterization[1]; res.parameterization[nexti[j]] = 0; res.parameterization[3] = res_triangle.parameterization[2]; } } } if (mindist < 0) { mindist = 0; res.encode = 15; res.parameterization[0] = triple(c - p, b - p, d - p) / vl; res.parameterization[1] = triple(a - p, c - p, d - p) / vl; res.parameterization[2] = triple(b - p, a - p, d - p) / vl; res.parameterization[3] = 1 - (res.parameterization[0] + res.parameterization[1] + res.parameterization[2]); } res.sqr_distance = mindist; } else if (!ng) { res = projectTriangle(a, b, c, p); res.parameterization[3] = 0; } return res; } template inline typename Project::ProjectResult Project::projectLineOrigin(const Vec3& a, const Vec3& b) { ProjectResult res; const Vec3 d = b - a; const Scalar l = d.squaredNorm(); if (l > 0) { const Scalar t = -a.dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; if (t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } else if (t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } } return res; } template inline typename Project::ProjectResult Project::projectTriangleOrigin(const Vec3& a, const Vec3& b, const Vec3& c) { ProjectResult res; static const size_t nexti[3] = {1, 2, 0}; const Vec3* vt[] = {&a, &b, &c}; const Vec3 dl[] = {a - b, b - c, c - a}; const Vec3& n = dl[0].cross(dl[1]); const Scalar l = n.squaredNorm(); if (l > 0) { Scalar mindist = -1; for (size_t i = 0; i < 3; ++i) { if (vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the // optimal can only be on the edge { size_t j = nexti[i]; ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); if (mindist < 0 || res_line.sqr_distance < mindist) { mindist = res_line.sqr_distance; res.encode = static_cast(((res_line.encode & 1) ? 1 << i : 0) + ((res_line.encode & 2) ? 1 << j : 0)); res.parameterization[i] = res_line.parameterization[0]; res.parameterization[j] = res_line.parameterization[1]; res.parameterization[nexti[j]] = 0; } } } if (mindist < 0) // the origin project is within the triangle { Scalar d = a.dot(n); Scalar s = std::sqrt(l); Vec3 o_to_project = n * (d / l); mindist = o_to_project.squaredNorm(); res.encode = 7; // m = 0x111 res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; } res.sqr_distance = mindist; } return res; } template inline typename Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& d) { ProjectResult res; static const size_t nexti[] = {1, 2, 0}; const Vec3* vt[] = {&a, &b, &c, &d}; const Vec3 dl[3] = {a - d, b - d, c - d}; Scalar vl = triple(dl[0], dl[1], dl[2]); bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0; if (ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng // is false, then the last vertex in the tetrahedron // does not grow toward the origin (in fact origin is // on the other side of the abc face) { Scalar mindist = -1; for (size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; Scalar s = vl * d.dot(dl[i].cross(dl[j])); if (s > 0) // the origin is to the outside part of a triangle face, then // the optimal can only be on the triangle face { ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); if (mindist < 0 || res_triangle.sqr_distance < mindist) { mindist = res_triangle.sqr_distance; res.encode = static_cast((res_triangle.encode & 1 ? 1 << i : 0) + (res_triangle.encode & 2 ? 1 << j : 0) + (res_triangle.encode & 4 ? 8 : 0)); res.parameterization[i] = res_triangle.parameterization[0]; res.parameterization[j] = res_triangle.parameterization[1]; res.parameterization[nexti[j]] = 0; res.parameterization[3] = res_triangle.parameterization[2]; } } } if (mindist < 0) { mindist = 0; res.encode = 15; res.parameterization[0] = triple(c, b, d) / vl; res.parameterization[1] = triple(a, c, d) / vl; res.parameterization[2] = triple(b, a, d) / vl; res.parameterization[3] = 1 - (res.parameterization[0] + res.parameterization[1] + res.parameterization[2]); } res.sqr_distance = mindist; } else if (!ng) { res = projectTriangleOrigin(a, b, c); res.parameterization[3] = 0; } return res; } } // namespace coal #endif // COAL_INTERSECT_HXX ================================================ FILE: include/coal/internal/shape_shape_contact_patch_func.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #ifndef COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H #define COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H #include "coal/collision_data.h" #include "coal/collision_utility.h" #include "coal/narrowphase/narrowphase.h" #include "coal/contact_patch/contact_patch_solver.h" #include "coal/shape/geometric_shapes_traits.h" #include "coal/tracy.hh" namespace coal { /// @brief Shape-shape contact patch computation. /// Assumes that `csolver` and the `ContactPatchResult` have already been set up /// by the `ContactPatchRequest`. template struct ComputeShapeShapeContactPatch { static void run(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { // Note: see specializations for Plane and Halfspace below. if (!collision_result.isCollision()) { return; } COAL_ASSERT( result.check(request), "The contact patch result and request are incompatible (issue of " "contact patch size or maximum number of contact patches). Make sure " "result is initialized with request.", std::logic_error); const ShapeType1& s1 = static_cast(*o1); const ShapeType2& s2 = static_cast(*o2); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; } csolver->setSupportGuess(collision_result.cached_support_func_guess); const Contact& contact = collision_result.getContact(i); ContactPatch& contact_patch = result.getUnusedContactPatch(); csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch); } } }; /// @brief Computes the contact patch between a Plane/Halfspace and another /// shape. /// @tparam InvertShapes set to true if the first shape of the collision pair /// is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling /// this function). template void computePatchPlaneOrHalfspace(const OtherShapeType& s1, const Transform3s& tf1, const PlaneOrHalfspace& s2, const Transform3s& tf2, const ContactPatchSolver* csolver, const Contact& contact, ContactPatch& contact_patch) { COAL_UNUSED_VARIABLE(s2); COAL_UNUSED_VARIABLE(tf2); constructContactPatchFrameFromContact(contact, contact_patch); if ((bool)(shape_traits::IsStrictlyConvex)) { // Only one point of contact; it has already been computed. contact_patch.addPoint(contact.pos); return; } // We only need to compute the support set in the direction of the normal. // We need to temporarily express the patch in the local frame of shape1. SupportSet& support_set = csolver->support_set_shape1; support_set.tf.rotation().noalias() = tf1.rotation().transpose() * contact_patch.tf.rotation(); support_set.tf.translation().noalias() = tf1.rotation().transpose() * (contact_patch.tf.translation() - tf1.translation()); // Note: for now, taking into account swept-sphere radius does not change // anything to the support set computations. However it will be used in the // future if we want to store the offsets to the support plane for each point // in a support set. using SupportOptions = details::SupportOptions; if (InvertShapes) { support_set.direction = ContactPatch::PatchDirection::INVERTED; details::getShapeSupportSet( &s1, support_set, csolver->support_guess[1], csolver->supports_data[1], csolver->num_samples_curved_shapes, csolver->patch_tolerance); } else { support_set.direction = ContactPatch::PatchDirection::DEFAULT; details::getShapeSupportSet( &s1, support_set, csolver->support_guess[0], csolver->supports_data[0], csolver->num_samples_curved_shapes, csolver->patch_tolerance); } csolver->getResult(contact, &(support_set.points()), contact_patch); } #define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace) \ template \ struct ComputeShapeShapeContactPatch { \ static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const CollisionResult& collision_result, \ const ContactPatchSolver* csolver, \ const ContactPatchRequest& request, \ ContactPatchResult& result) { \ COAL_TRACY_ZONE_SCOPED_N( \ "coal::ComputeShapeShapeContactPatch::run"); \ if (!collision_result.isCollision()) { \ return; \ } \ COAL_ASSERT( \ result.check(request), \ "The contact patch result and request are incompatible (issue of " \ "contact patch size or maximum number of contact patches). Make " \ "sure " \ "result is initialized with request.", \ std::logic_error); \ \ const OtherShapeType& s1 = static_cast(*o1); \ const PlaneOrHspace& s2 = static_cast(*o2); \ for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ if (i >= request.max_num_patch) { \ break; \ } \ csolver->setSupportGuess(collision_result.cached_support_func_guess); \ const Contact& contact = collision_result.getContact(i); \ ContactPatch& contact_patch = result.getUnusedContactPatch(); \ computePatchPlaneOrHalfspace( \ s1, tf1, s2, tf2, csolver, contact, contact_patch); \ } \ } \ }; \ \ template \ struct ComputeShapeShapeContactPatch { \ static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const CollisionResult& collision_result, \ const ContactPatchSolver* csolver, \ const ContactPatchRequest& request, \ ContactPatchResult& result) { \ COAL_TRACY_ZONE_SCOPED_N( \ "coal::ComputeShapeShapeContactPatch::run"); \ if (!collision_result.isCollision()) { \ return; \ } \ COAL_ASSERT( \ result.check(request), \ "The contact patch result and request are incompatible (issue of " \ "contact patch size or maximum number of contact patches). Make " \ "sure " \ "result is initialized with request.", \ std::logic_error); \ \ const PlaneOrHspace& s1 = static_cast(*o1); \ const OtherShapeType& s2 = static_cast(*o2); \ for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ if (i >= request.max_num_patch) { \ break; \ } \ csolver->setSupportGuess(collision_result.cached_support_func_guess); \ const Contact& contact = collision_result.getContact(i); \ ContactPatch& contact_patch = result.getUnusedContactPatch(); \ computePatchPlaneOrHalfspace( \ s2, tf2, s1, tf1, csolver, contact, contact_patch); \ } \ } \ }; PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane) PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace) #define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2) \ template <> \ struct ComputeShapeShapeContactPatch { \ static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const CollisionResult& collision_result, \ const ContactPatchSolver* csolver, \ const ContactPatchRequest& request, \ ContactPatchResult& result) { \ COAL_UNUSED_VARIABLE(o1); \ COAL_UNUSED_VARIABLE(tf1); \ COAL_UNUSED_VARIABLE(o2); \ COAL_UNUSED_VARIABLE(tf2); \ COAL_UNUSED_VARIABLE(csolver); \ if (!collision_result.isCollision()) { \ return; \ } \ COAL_ASSERT( \ result.check(request), \ "The contact patch result and request are incompatible (issue of " \ "contact patch size or maximum number of contact patches). Make " \ "sure " \ "result is initialized with request.", \ std::logic_error); \ \ for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ if (i >= request.max_num_patch) { \ break; \ } \ const Contact& contact = collision_result.getContact(i); \ ContactPatch& contact_patch = result.getUnusedContactPatch(); \ constructContactPatchFrameFromContact(contact, contact_patch); \ contact_patch.addPoint(contact.pos); \ } \ } \ }; PLANE_HSPACE_CONTACT_PATCH(Plane, Plane) PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace) PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane) PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace) #undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH #undef PLANE_HSPACE_CONTACT_PATCH template void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { COAL_TRACY_ZONE_SCOPED_N("coal::ShapeShapeContactPatch"); return ComputeShapeShapeContactPatch::run( o1, tf1, o2, tf2, collision_result, csolver, request, result); } } // namespace coal #endif ================================================ FILE: include/coal/internal/shape_shape_func.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2014, CNRS-LAAS * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #ifndef COAL_INTERNAL_SHAPE_SHAPE_FUNC_H #define COAL_INTERNAL_SHAPE_SHAPE_FUNC_H /// @cond INTERNAL #include "coal/collision_data.h" #include "coal/collision_utility.h" #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes_traits.h" #include "coal/tracy.hh" namespace coal { template struct ShapeShapeDistancer { static Scalar run(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; // Witness points on shape1 and shape2, normal pointing from shape1 to // shape2. Vec3s p1, p2, normal; const Scalar distance = ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2, normal); result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE, p1, p2, normal); return distance; } static Scalar run(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const ShapeType1* obj1 = static_cast(o1); const ShapeType2* obj2 = static_cast(o2); return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2, compute_signed_distance, p1, p2, normal); } }; /// @brief Shape-shape distance computation. /// Assumes that `nsolver` has already been set up by a `DistanceRequest` or /// a `CollisionRequest`. /// /// @note This function is typically used for collision pairs containing two /// primitive shapes. /// @note This function might be specialized for some pairs of shapes. template Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { COAL_TRACY_ZONE_SCOPED_N("coal::ShapeShapeDistance"); return ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, request, result); } namespace internal { /// @brief Shape-shape distance computation. /// Assumes that `nsolver` has already been set up by a `DistanceRequest` or /// a `CollisionRequest`. /// /// @note This function is typically used for collision pairs complex structures /// like BVHs, HeightFields or Octrees. These structures contain sets of /// primitive shapes. /// This function is meant to be called on the pairs of primitive shapes of /// these structures. /// @note This function might be specialized for some pairs of shapes. template Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) { return ::coal::ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal); } } // namespace internal /// @brief Shape-shape collision detection. /// Assumes that `nsolver` has already been set up by a `DistanceRequest` or /// a `CollisionRequest`. /// /// @note This function is typically used for collision pairs containing two /// primitive shapes. /// Complex structures like BVHs, HeightFields or Octrees contain sets of /// primitive shapes should use the `ShapeShapeDistance` function to do their /// internal collision detection checks. template struct ShapeShapeCollider { static std::size_t run(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); const bool compute_penetration = request.enable_contact || (request.security_margin < 0); Vec3s p1, p2, normal; Scalar distance = internal::ShapeShapeDistance( o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal); const Scalar distToCollision = distance - request.security_margin; internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, p1, p2, normal); if (distToCollision <= request.collision_distance_threshold && result.numContacts() < request.num_max_contacts) { if (result.numContacts() < request.num_max_contacts) { Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal, distance); result.addContact(contact); } } return result.numContacts(); } }; template std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { COAL_TRACY_ZONE_SCOPED_N("coal::ShapeShapeCollide"); return ShapeShapeCollider::run( o1, tf1, o2, tf2, nsolver, request, result); } // clang-format off // ============================================================================================================== // ============================================================================================================== // ============================================================================================================== // Shape distance algorithms based on: // - built-in function: 0 // - GJK: 1 // // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | box | 1 | 0 | 1 | 1 | 1 | 0 | 0 | 1 | 1 | 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | sphere |/////| 0 | 0 | 1 | 0 | 0 | 0 | 0 | 1 | 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | capsule |/////|////////| 0 | 1 | 1 | 0 | 0 | 1 | 1 | 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1 | 1 | 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1 | 1 | 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | plane |/////|////////|/////////|//////|//////////| ? | ? | 0 | 0 | 0 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | half-space |/////|////////|/////////|//////|//////////|///////| ? | 0 | 0 | 0 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | triangle |/////|////////|/////////|//////|//////////|///////|////////////| 0 | 1 | 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | ellipsoid |/////|////////|/////////|//////|//////////|///////|////////////|//////////| 1 | 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // | convex |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////| 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+ // // Number of pairs: 55 // - Specialized: 26 // - GJK: 29 // clang-format on #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ template <> \ COAL_DLLAPI Scalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ Vec3s& p2, Vec3s& normal); \ template <> \ COAL_DLLAPI Scalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ Vec3s& p2, Vec3s& normal); \ template <> \ inline COAL_DLLAPI Scalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ DistanceResult& result) { \ result.o1 = o1; \ result.o2 = o2; \ result.b1 = DistanceResult::NONE; \ result.b2 = DistanceResult::NONE; \ result.min_distance = internal::ShapeShapeDistance( \ o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ result.nearest_points[0], result.nearest_points[1], result.normal); \ return result.min_distance; \ } \ template <> \ inline COAL_DLLAPI Scalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ DistanceResult& result) { \ result.o1 = o1; \ result.o2 = o2; \ result.b1 = DistanceResult::NONE; \ result.b2 = DistanceResult::NONE; \ result.min_distance = internal::ShapeShapeDistance( \ o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ result.nearest_points[0], result.nearest_points[1], result.normal); \ return result.min_distance; \ } #define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \ template <> \ COAL_DLLAPI Scalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ Vec3s& p2, Vec3s& normal); \ template <> \ inline COAL_DLLAPI Scalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ DistanceResult& result) { \ result.o1 = o1; \ result.o2 = o2; \ result.b1 = DistanceResult::NONE; \ result.b2 = DistanceResult::NONE; \ result.min_distance = internal::ShapeShapeDistance( \ o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \ result.nearest_points[0], result.nearest_points[1], result.normal); \ return result.min_distance; \ } SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere) SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane) SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase16, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase32, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase16, Plane) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase32, Plane) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane) SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere) SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace) SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane) SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace) #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION #undef SHAPE_SELF_DISTANCE_SPECIALIZATION } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/tools.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Joseph Mirabel */ #ifndef COAL_INTERNAL_TOOLS_H #define COAL_INTERNAL_TOOLS_H #include "coal/fwd.hh" #include #include #include #include "coal/data_types.h" namespace coal { template static inline typename Derived::Scalar triple( const Eigen::MatrixBase& x, const Eigen::MatrixBase& y, const Eigen::MatrixBase& z) { return x.derived().dot(y.derived().cross(z.derived())); } template void generateCoordinateSystem(const Eigen::MatrixBase& _w, const Eigen::MatrixBase& _u, const Eigen::MatrixBase& _v) { typedef typename Derived1::Scalar T; Eigen::MatrixBase& w = const_cast&>(_w); Eigen::MatrixBase& u = const_cast&>(_u); Eigen::MatrixBase& v = const_cast&>(_v); T inv_length; if (std::abs(w[0]) >= std::abs(w[1])) { inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]); u[0] = -w[2] * inv_length; u[1] = (T)0; u[2] = w[0] * inv_length; v[0] = w[1] * u[2]; v[1] = w[2] * u[0] - w[0] * u[2]; v[2] = -w[1] * u[0]; } else { inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]); u[0] = (T)0; u[1] = w[2] * inv_length; u[2] = -w[1] * inv_length; v[0] = w[1] * u[2] - w[2] * u[1]; v[1] = -w[0] * u[2]; v[2] = w[0] * u[1]; } } /* ----- Start Matrices ------ */ template void relativeTransform(const Eigen::MatrixBase& R1, const Eigen::MatrixBase& t1, const Eigen::MatrixBase& R2, const Eigen::MatrixBase& t2, const Eigen::MatrixBase& R, const Eigen::MatrixBase& t) { const_cast&>(R) = R1.transpose() * R2; const_cast&>(t) = R1.transpose() * (t2 - t1); } /// @brief compute the eigen vector and eigen vector of a matrix. dout is the /// eigen values, vout is the eigen vectors template void eigen(const Eigen::MatrixBase& m, typename Derived::Scalar dout[3], Vector* vout) { typedef typename Derived::Scalar S; Derived R(m.derived()); int n = 3; int j, iq, ip, i; S tresh, theta, tau, t, sm, s, h, g, c; S b[3]; S z[3]; S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; S d[3]; for (ip = 0; ip < n; ++ip) { b[ip] = d[ip] = R(ip, ip); z[ip] = 0; } for (i = 0; i < 50; ++i) { sm = 0; for (ip = 0; ip < n; ++ip) for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq)); if (sm == 0.0) { vout[0] << v[0][0], v[0][1], v[0][2]; vout[1] << v[1][0], v[1][1], v[1][2]; vout[2] << v[2][0], v[2][1], v[2][2]; dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2]; return; } if (i < 3) tresh = Scalar(0.2 * sm / (n * n)); else tresh = 0.0; for (ip = 0; ip < n; ++ip) { for (iq = ip + 1; iq < n; ++iq) { g = Scalar(100) * std::abs(R(ip, iq)); if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) && std::abs(d[iq]) + g == std::abs(d[iq])) R(ip, iq) = 0.0; else if (std::abs(R(ip, iq)) > tresh) { h = d[iq] - d[ip]; if (std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h; else { theta = Scalar(0.5) * h / (R(ip, iq)); t = 1 / (std::abs(theta) + std::sqrt(1 + theta * theta)); if (theta < 0.0) t = -t; } c = 1 / std::sqrt(1 + t * t); s = t * c; tau = s / (1 + c); h = t * R(ip, iq); z[ip] -= h; z[iq] += h; d[ip] -= h; d[iq] += h; R(ip, iq) = 0.0; for (j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); } for (j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); } for (j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); } for (j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); } } } } for (ip = 0; ip < n; ++ip) { b[ip] += z[ip]; d[ip] = b[ip]; z[ip] = 0.0; } } std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl; return; } template bool isEqual(const Eigen::MatrixBase& lhs, const Eigen::MatrixBase& rhs, const Scalar tol = std::numeric_limits::epsilon() * 100) { return ((lhs - rhs).array().abs() < tol).all(); } } // namespace coal #endif ================================================ FILE: include/coal/internal/traversal.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2019, LAAS CNRS * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Joseph Mirabel */ #ifndef COAL_TRAVERSAL_DETAILS_TRAVERSAL_H #define COAL_TRAVERSAL_DETAILS_TRAVERSAL_H /// @cond INTERNAL namespace coal { enum { RelativeTransformationIsIdentity = 1 }; namespace details { template struct COAL_DLLAPI RelativeTransformation { RelativeTransformation() : R(Matrix3s::Identity()) {} const Matrix3s& _R() const { return R; } const Vec3s& _T() const { return T; } Matrix3s R; Vec3s T; }; template <> struct COAL_DLLAPI RelativeTransformation { static const Matrix3s& _R() { COAL_THROW_PRETTY("should never reach this point", std::logic_error); } static const Vec3s& _T() { COAL_THROW_PRETTY("should never reach this point", std::logic_error); } }; } // namespace details } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_node_base.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRAVERSAL_NODE_BASE_H #define COAL_TRAVERSAL_NODE_BASE_H /// @cond INTERNAL #include "coal/data_types.h" #include "coal/math/transform.h" #include "coal/collision_data.h" namespace coal { /// @brief Node structure encoding the information required for traversal. class TraversalNodeBase { public: TraversalNodeBase() : enable_statistics(false) {} virtual ~TraversalNodeBase() {} virtual void preprocess() {} virtual void postprocess() {} /// @brief Whether b is a leaf node in the first BVH tree virtual bool isFirstNodeLeaf(unsigned int /*b*/) const { return true; } /// @brief Whether b is a leaf node in the second BVH tree virtual bool isSecondNodeLeaf(unsigned int /*b*/) const { return true; } /// @brief Traverse the subtree of the node in the first tree first virtual bool firstOverSecond(unsigned int /*b1*/, unsigned int /*b2*/) const { return true; } /// @brief Get the left child of the node b in the first tree virtual int getFirstLeftChild(unsigned int b) const { return (int)b; } /// @brief Get the right child of the node b in the first tree virtual int getFirstRightChild(unsigned int b) const { return (int)b; } /// @brief Get the left child of the node b in the second tree virtual int getSecondLeftChild(unsigned int b) const { return (int)b; } /// @brief Get the right child of the node b in the second tree virtual int getSecondRightChild(unsigned int b) const { return (int)b; } /// @brief Whether store some statistics information during traversal void enableStatistics(bool enable) { enable_statistics = enable; } /// @brief configuation of first object Transform3s tf1; /// @brief configuration of second object Transform3s tf2; /// @brief Whether stores statistics bool enable_statistics; }; /// @defgroup Traversal_For_Collision /// regroup class about traversal for distance. /// @{ /// @brief Node structure encoding the information required for collision /// traversal. class CollisionTraversalNodeBase : public TraversalNodeBase { public: CollisionTraversalNodeBase(const CollisionRequest& request_) : request(request_), result(NULL) {} virtual ~CollisionTraversalNodeBase() {} /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. virtual bool BVDisjoints(unsigned int b1, unsigned int b2, Scalar& sqrDistLowerBound) const = 0; /// @brief Leaf test between node b1 and b2, if they are both leafs virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/, Scalar& /*sqrDistLowerBound*/) const = 0; /// @brief Check whether the traversal can stop bool canStop() const { return this->request.isSatisfied(*(this->result)); } /// @brief request setting for collision const CollisionRequest& request; /// @brief collision result kept during the traversal iteration CollisionResult* result; }; /// @} /// @defgroup Traversal_For_Distance /// regroup class about traversal for distance. /// @{ /// @brief Node structure encoding the information required for distance /// traversal. class DistanceTraversalNodeBase : public TraversalNodeBase { public: DistanceTraversalNodeBase() : result(NULL) {} virtual ~DistanceTraversalNodeBase() {} /// @brief BV test between b1 and b2 /// @return a lower bound of the distance between the two BV. /// @note except for OBB, this method returns the distance. virtual Scalar BVDistanceLowerBound(unsigned int /*b1*/, unsigned int /*b2*/) const { return (std::numeric_limits::max)(); } /// @brief Leaf test between node b1 and b2, if they are both leafs virtual void leafComputeDistance(unsigned int b1, unsigned int b2) const = 0; /// @brief Check whether the traversal can stop virtual bool canStop(Scalar /*c*/) const { return false; } /// @brief request setting for distance DistanceRequest request; /// @brief distance result kept during the traversal iteration DistanceResult* result; }; ///@} } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_node_bvh_hfield.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Justin Carpentier */ #ifndef COAL_TRAVERSAL_NODE_BVH_HFIELD_H #define COAL_TRAVERSAL_NODE_BVH_HFIELD_H /// @cond INTERNAL #include "coal/collision_data.h" #include "coal/internal/traversal_node_base.h" #include "coal/internal/traversal_node_hfield_shape.h" #include "coal/BV/BV_node.h" #include "coal/BV/BV.h" #include "coal/BVH/BVH_model.h" #include "coal/hfield.h" #include "coal/internal/intersect.h" #include "coal/shape/geometric_shapes.h" #include "coal/narrowphase/narrowphase.h" #include "coal/internal/traversal.h" #include #include #include namespace coal { /// @addtogroup Traversal_For_Collision /// @{ /// @brief Traversal node for collision between one BVH model and one /// HeightField template class MeshHeightFieldCollisionTraversalNode : public CollisionTraversalNodeBase { public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; MeshHeightFieldCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; vertices1 = NULL; tri_indices1 = NULL; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(unsigned int b) const { assert(model1 != NULL && "model1 is NULL"); return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(unsigned int b) const { assert(model2 != NULL && "model2 is NULL"); return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { Scalar sz1 = model1->getBV(b1).bv.size(); Scalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); if (l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node bool BVDisjoints(unsigned int b1, unsigned int b2) const { if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).overlap(this->model2->getBV(b2)); else return !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); } /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. bool BVDisjoints(unsigned int b1, unsigned int b2, Scalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).overlap(this->model2->getBV(b2), this->request, sqrDistLowerBound); else { bool res = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, this->model2->getBV(b2).bv, this->request, sqrDistLowerBound); assert(!res || sqrDistLowerBound > 0); return res; } } /// Intersection testing between leaves (two triangles) /// /// @param b1, b2 id of primitive in bounding volume hierarchy /// @retval sqrDistLowerBound squared lower bound of distance between /// primitives if they are not in collision. /// /// This method supports a security margin. If the distance between /// the primitives is less than the security margin, the objects are /// considered as in collision. in this case a contact point is /// returned in the CollisionResult. /// /// @note If the distance between objects is less than the security margin, /// and the object are not colliding, the penetration depth is /// negative. void leafCollides(unsigned int b1, unsigned int b2, Scalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const HeightFieldNode& node2 = this->model2->getBV(b2); int primitive_id1 = node1.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Vec3s& P1 = vertices1[tri_id1[0]]; const Vec3s& P2 = vertices1[tri_id1[1]]; const Vec3s& P3 = vertices1[tri_id1[2]]; TriangleP tri1(P1, P2, P3); typedef Convex ConvexTriangle; ConvexTriangle convex1, convex2; details::buildConvexTriangles(node2, *this->model2, convex2, convex2); GJKSolver solver; Vec3s p1, p2; // closest points if no collision contact points if collision. Vec3s normal; Scalar distance; solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2, normal); Scalar distToCollision = distance - this->request.security_margin; sqrDistLowerBound = distance * distance; if (distToCollision <= 0) { // collision Vec3s p(p1); // contact point Scalar penetrationDepth(0); if (this->result->numContacts() < this->request.num_max_contacts) { // How much (Q1, Q2, Q3) should be moved so that all vertices are // above (P1, P2, P3). penetrationDepth = -distance; if (distance > 0) { normal = (p2 - p1).normalized(); p = .5 * (p1 + p2); } this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, p, normal, penetrationDepth)); } } } /// @brief The first BVH model const BVHModel* model1; /// @brief The second HeightField model const HeightField* model2; /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; Vec3s* vertices1 Triangle* tri_indices1; details::RelativeTransformation RT; }; /// @brief Traversal node for collision between two meshes if their underlying /// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) typedef MeshHeightFieldCollisionTraversalNode MeshHeightFieldCollisionTraversalNodeOBB; typedef MeshHeightFieldCollisionTraversalNode MeshHeightFieldCollisionTraversalNodeRSS; typedef MeshHeightFieldCollisionTraversalNode MeshHeightFieldCollisionTraversalNodekIOS; typedef MeshHeightFieldCollisionTraversalNode MeshHeightFieldCollisionTraversalNodeOBBRSS; /// @} namespace details { template struct DistanceTraversalBVDistanceLowerBound_impl { static Scalar run(const BVNode& b1, const BVNode& b2) { return b1.distance(b2); } static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { return distance(R, T, b1.bv, b2.bv); } }; template <> struct DistanceTraversalBVDistanceLowerBound_impl { static Scalar run(const BVNode& b1, const BVNode& b2) { Scalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { // TODO A penetration upper bound should be computed. return -1; } return sqrt(sqrDistLowerBound); } static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { Scalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { // TODO A penetration upper bound should be computed. return -1; } return sqrt(sqrDistLowerBound); } }; template <> struct DistanceTraversalBVDistanceLowerBound_impl { static Scalar run(const BVNode& b1, const BVNode& b2) { Scalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { // TODO A penetration upper bound should be computed. return -1; } return sqrt(sqrDistLowerBound); } static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { Scalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { // TODO A penetration upper bound should be computed. return -1; } return sqrt(sqrDistLowerBound); } }; } // namespace details /// @addtogroup Traversal_For_Distance /// @{ /// @brief Traversal node for distance computation between BVH models template class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { public: BVHDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { Scalar sz1 = model1->getBV(b1).bv.size(); Scalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); if (l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// @brief The first BVH model const BVHModel* model1; /// @brief The second BVH model const BVHModel* model2; /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; }; /// @brief Traversal node for distance computation between two meshes template class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; using BVHDistanceTraversalNode::enable_statistics; using BVHDistanceTraversalNode::request; using BVHDistanceTraversalNode::result; using BVHDistanceTraversalNode::tf1; using BVHDistanceTraversalNode::model1; using BVHDistanceTraversalNode::model2; using BVHDistanceTraversalNode::num_bv_tests; using BVHDistanceTraversalNode::num_leaf_tests; MeshDistanceTraversalNode() : BVHDistanceTraversalNode() { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; tri_indices2 = NULL; rel_err = this->request.rel_err; abs_err = this->request.abs_err; } void preprocess() { if (!RTIsIdentity) preprocessOrientedNode(); } void postprocess() { if (!RTIsIdentity) postprocessOrientedNode(); } /// @brief BV culling test in one BVTT node Scalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { if (enable_statistics) num_bv_tests++; if (RTIsIdentity) return details::DistanceTraversalBVDistanceLowerBound_impl::run( model1->getBV(b1), model2->getBV(b2)); else return details::DistanceTraversalBVDistanceLowerBound_impl::run( RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); } /// @brief Distance testing between leaves (two triangles) void leafComputeDistance(unsigned int b1, unsigned int b2) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; const Vec3s& t11 = vertices1[tri_id1[0]]; const Vec3s& t12 = vertices1[tri_id1[1]]; const Vec3s& t13 = vertices1[tri_id1[2]]; const Vec3s& t21 = vertices2[tri_id2[0]]; const Vec3s& t22 = vertices2[tri_id2[1]]; const Vec3s& t23 = vertices2[tri_id2[2]]; // nearest point pair Vec3s P1, P2, normal; Scalar d2; if (RTIsIdentity) d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, P2); else d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, RT._R(), RT._T(), P1, P2); Scalar d = sqrt(d2); this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2, normal); } /// @brief Whether the traversal process can stop early bool canStop(Scalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } Vec3s* vertices1; Vec3s* vertices2; Triangle* tri_indices1; Triangle* tri_indices2; /// @brief relative and absolute error, default value is 0.01 for both terms Scalar rel_err; Scalar abs_err; details::RelativeTransformation RT; private: void preprocessOrientedNode() { const int init_tri_id1 = 0, init_tri_id2 = 0; const Triangle& init_tri1 = tri_indices1[init_tri_id1]; const Triangle& init_tri2 = tri_indices2[init_tri_id2]; Vec3s init_tri1_points[3]; Vec3s init_tri2_points[3]; init_tri1_points[0] = vertices1[init_tri1[0]]; init_tri1_points[1] = vertices1[init_tri1[1]]; init_tri1_points[2] = vertices1[init_tri1[2]]; init_tri2_points[0] = vertices2[init_tri2[0]]; init_tri2_points[1] = vertices2[init_tri2[1]]; init_tri2_points[2] = vertices2[init_tri2[2]]; Vec3s p1, p2, normal; Scalar distance = sqrt(TriangleDistance::sqrTriDistance( init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), RT._T(), p1, p2)); result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2, normal); } void postprocessOrientedNode() { /// the points obtained by triDistance are not in world space: both are in /// object1's local coordinate system, so we need to convert them into the /// world space. if (request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2)) { result->nearest_points[0] = tf1.transform(result->nearest_points[0]); result->nearest_points[1] = tf1.transform(result->nearest_points[1]); } } }; /// @brief Traversal node for distance computation between two meshes if their /// underlying BVH node is oriented node (RSS, OBBRSS, kIOS) typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeRSS; typedef MeshDistanceTraversalNode MeshDistanceTraversalNodekIOS; typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; /// @} /// @brief for OBB and RSS, there is local coordinate of BV, so normal need to /// be transformed namespace details { template inline const Matrix3s& getBVAxes(const BV& bv) { return bv.axes; } template <> inline const Matrix3s& getBVAxes(const OBBRSS& bv) { return bv.obb.axes; } } // namespace details } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_node_bvh_shape.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRAVERSAL_NODE_MESH_SHAPE_H #define COAL_TRAVERSAL_NODE_MESH_SHAPE_H /// @cond INTERNAL #include "coal/collision_data.h" #include "coal/shape/geometric_shapes.h" #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes_utility.h" #include "coal/internal/traversal_node_base.h" #include "coal/internal/traversal.h" #include "coal/BVH/BVH_model.h" #include "coal/internal/shape_shape_func.h" namespace coal { /// @addtogroup Traversal_For_Collision /// @{ /// @brief Traversal node for collision between BVH and shape template class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: BVHShapeCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } const BVHModel* model1; const S* model2; BV model2_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; }; /// @brief Traversal node for collision between mesh and shape template class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode { public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; MeshShapeCollisionTraversalNode(const CollisionRequest& request) : BVHShapeCollisionTraversalNode(request) { vertices = NULL; tri_indices = NULL; nsolver = NULL; } /// test between BV b1 and shape /// @param b1 BV to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. /// @brief BV culling test in one BVTT node bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, Scalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; bool disjoint; if (RTIsIdentity) disjoint = !this->model1->getBV(b1).bv.overlap( this->model2_bv, this->request, sqrDistLowerBound); else disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, this->request, sqrDistLowerBound); if (disjoint) internal::updateDistanceLowerBoundFromBV(this->request, *this->result, sqrDistLowerBound); assert(!disjoint || sqrDistLowerBound > 0); return disjoint; } /// @brief Intersection testing between leaves (one triangle and one shape) void leafCollides(unsigned int b1, unsigned int /*b2*/, Scalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle32& tri_id = this->tri_indices[primitive_id]; const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], this->vertices[tri_id[2]]); // When reaching this point, `this->solver` has already been set up // by the CollisionRequest `this->request`. // The only thing we need to (and can) pass to `ShapeShapeDistance` is // whether or not penetration information is should be computed in case of // collision. const bool compute_penetration = this->request.enable_contact || (this->request.security_margin < 0); Vec3s c1, c2, normal; Scalar distance; if (RTIsIdentity) { static const Transform3s Id; distance = internal::ShapeShapeDistance( &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration, c1, c2, normal); } else { distance = internal::ShapeShapeDistance( &tri, this->tf1, this->model2, this->tf2, this->nsolver, compute_penetration, c1, c2, normal); } const Scalar distToCollision = distance - this->request.security_margin; internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), distToCollision, c1, c2, normal); if (distToCollision <= this->request.collision_distance_threshold) { sqrDistLowerBound = 0; if (this->result->numContacts() < this->request.num_max_contacts) { this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, c1, c2, normal, distance)); assert(this->result->isCollision()); } } else { sqrDistLowerBound = distToCollision * distToCollision; } assert(this->result->isCollision() || sqrDistLowerBound > 0); } // leafCollides Vec3s* vertices; Triangle32* tri_indices; const GJKSolver* nsolver; }; /// @} /// @addtogroup Traversal_For_Distance /// @{ /// @brief Traversal node for distance computation between BVH and shape template class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { return model1->getBV(b1).bv.distance(model2_bv); } const BVHModel* model1; const S* model2; BV model2_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; }; /// @brief Traversal node for distance computation between shape and BVH template class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { public: ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node Scalar BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { return model1_bv.distance(model2->getBV(b2).bv); } const S* model1; const BVHModel* model2; BV model1_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; }; /// @brief Traversal node for distance between mesh and shape template class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode { public: MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode() { vertices = NULL; tri_indices = NULL; rel_err = 0; abs_err = 0; nsolver = NULL; } /// @brief Distance testing between leaves (one triangle and one shape) void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle32& tri_id = tri_indices[primitive_id]; const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], this->vertices[tri_id[2]]); Vec3s p1, p2, normal; const Scalar distance = internal::ShapeShapeDistance( &tri, this->tf1, this->model2, this->tf2, this->nsolver, this->request.enable_signed_distance, p1, p2, normal); this->result->update(distance, this->model1, this->model2, primitive_id, DistanceResult::NONE, p1, p2, normal); } /// @brief Whether the traversal process can stop early bool canStop(Scalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } Vec3s* vertices; Triangle32* tri_indices; Scalar rel_err; Scalar abs_err; const GJKSolver* nsolver; }; /// @cond IGNORE namespace details { template void meshShapeDistanceOrientedNodeleafComputeDistance( unsigned int b1, unsigned int /* b2 */, const BVHModel* model1, const S& model2, Vec3s* vertices, Triangle32* tri_indices, const Transform3s& tf1, const Transform3s& tf2, const GJKSolver* nsolver, bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request, DistanceResult& result) { if (enable_statistics) num_leaf_tests++; const BVNode& node = model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle32& tri_id = tri_indices[primitive_id]; const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], vertices[tri_id[2]]); Vec3s p1, p2, normal; const Scalar distance = internal::ShapeShapeDistance( &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, normal); result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, p1, p2, normal); } template static inline void distancePreprocessOrientedNode( const BVHModel* model1, Vec3s* vertices, Triangle32* tri_indices, int init_tri_id, const S& model2, const Transform3s& tf1, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { const Triangle32& tri_id = tri_indices[init_tri_id]; const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], vertices[tri_id[2]]); Vec3s p1, p2, normal; const Scalar distance = internal::ShapeShapeDistance( &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, normal); result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, p1, p2, normal); } } // namespace details /// @endcond /// @brief Traversal node for distance between mesh and shape, when mesh BVH is /// one of the oriented node (RSS, kIOS, OBBRSS) template class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode { public: MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode() {} void preprocess() { details::distancePreprocessOrientedNode( this->model1, this->vertices, this->tri_indices, 0, *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } void postprocess() {} Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { if (this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafComputeDistance(unsigned int b1, unsigned int b2) const { details::meshShapeDistanceOrientedNodeleafComputeDistance( b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; template class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode { public: MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode() {} void preprocess() { details::distancePreprocessOrientedNode( this->model1, this->vertices, this->tri_indices, 0, *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } void postprocess() {} Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { if (this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafComputeDistance(unsigned int b1, unsigned int b2) const { details::meshShapeDistanceOrientedNodeleafComputeDistance( b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; template class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode { public: MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode() {} void preprocess() { details::distancePreprocessOrientedNode( this->model1, this->vertices, this->tri_indices, 0, *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } void postprocess() {} Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { if (this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafComputeDistance(unsigned int b1, unsigned int b2) const { details::meshShapeDistanceOrientedNodeleafComputeDistance( b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; /// @} } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_node_bvhs.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRAVERSAL_NODE_MESHES_H #define COAL_TRAVERSAL_NODE_MESHES_H /// @cond INTERNAL #include "coal/collision_data.h" #include "coal/internal/traversal_node_base.h" #include "coal/BV/BV_node.h" #include "coal/BV/BV.h" #include "coal/BVH/BVH_model.h" #include "coal/internal/intersect.h" #include "coal/shape/geometric_shapes.h" #include "coal/narrowphase/narrowphase.h" #include "coal/internal/traversal.h" #include "coal/internal/shape_shape_func.h" #include namespace coal { /// @addtogroup Traversal_For_Collision /// @{ /// @brief Traversal node for collision between BVH models template class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { public: BVHCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(unsigned int b) const { assert(model1 != NULL && "model1 is NULL"); return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(unsigned int b) const { assert(model2 != NULL && "model2 is NULL"); return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { Scalar sz1 = model1->getBV(b1).bv.size(); Scalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); if (l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// @brief The first BVH model const BVHModel* model1; /// @brief The second BVH model const BVHModel* model2; /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; }; /// @brief Traversal node for collision between two meshes template class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; MeshCollisionTraversalNode(const CollisionRequest& request) : BVHCollisionTraversalNode(request) { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; tri_indices2 = NULL; } /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. bool BVDisjoints(unsigned int b1, unsigned int b2, Scalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; bool disjoint; if (RTIsIdentity) disjoint = !this->model1->getBV(b1).overlap( this->model2->getBV(b2), this->request, sqrDistLowerBound); else { disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv, this->model1->getBV(b1).bv, this->request, sqrDistLowerBound); } if (disjoint) internal::updateDistanceLowerBoundFromBV(this->request, *this->result, sqrDistLowerBound); return disjoint; } /// Intersection testing between leaves (two triangles) /// /// @param b1, b2 id of primitive in bounding volume hierarchy /// @retval sqrDistLowerBound squared lower bound of distance between /// primitives if they are not in collision. /// /// This method supports a security margin. If the distance between /// the primitives is less than the security margin, the objects are /// considered as in collision. in this case a contact point is /// returned in the CollisionResult. /// /// @note If the distance between objects is less than the security margin, /// and the object are not colliding, the penetration depth is /// negative. void leafCollides(unsigned int b1, unsigned int b2, Scalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle32& tri_id1 = tri_indices1[primitive_id1]; const Triangle32& tri_id2 = tri_indices2[primitive_id2]; const Vec3s& P1 = vertices1[tri_id1[0]]; const Vec3s& P2 = vertices1[tri_id1[1]]; const Vec3s& P3 = vertices1[tri_id1[2]]; const Vec3s& Q1 = vertices2[tri_id2[0]]; const Vec3s& Q2 = vertices2[tri_id2[1]]; const Vec3s& Q3 = vertices2[tri_id2[2]]; TriangleP tri1(P1, P2, P3); TriangleP tri2(Q1, Q2, Q3); // TODO(louis): MeshCollisionTraversalNode should have its own GJKSolver. GJKSolver solver(this->request); const bool compute_penetration = this->request.enable_contact || (this->request.security_margin < 0); Vec3s p1, p2, normal; DistanceResult distanceResult; Scalar distance = internal::ShapeShapeDistance( &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1, p2, normal); const Scalar distToCollision = distance - this->request.security_margin; internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), distToCollision, p1, p2, normal); if (distToCollision <= this->request.collision_distance_threshold) { // collision sqrDistLowerBound = 0; if (this->result->numContacts() < this->request.num_max_contacts) { this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, p1, p2, normal, distance)); } } else sqrDistLowerBound = distToCollision * distToCollision; } Vec3s* vertices1; Vec3s* vertices2; Triangle32* tri_indices1; Triangle32* tri_indices2; details::RelativeTransformation RT; }; /// @brief Traversal node for collision between two meshes if their underlying /// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBB; typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeRSS; typedef MeshCollisionTraversalNode MeshCollisionTraversalNodekIOS; typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBBRSS; /// @} namespace details { template struct DistanceTraversalBVDistanceLowerBound_impl { static Scalar run(const BVNode& b1, const BVNode& b2) { return b1.distance(b2); } static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { return distance(R, T, b1.bv, b2.bv); } }; template <> struct DistanceTraversalBVDistanceLowerBound_impl { static Scalar run(const BVNode& b1, const BVNode& b2) { Scalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { // TODO A penetration upper bound should be computed. return -1; } return std::sqrt(sqrDistLowerBound); } static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { Scalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { // TODO A penetration upper bound should be computed. return -1; } return std::sqrt(sqrDistLowerBound); } }; template <> struct DistanceTraversalBVDistanceLowerBound_impl { static Scalar run(const BVNode& b1, const BVNode& b2) { Scalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { // TODO A penetration upper bound should be computed. return -1; } return std::sqrt(sqrDistLowerBound); } static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { Scalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { // TODO A penetration upper bound should be computed. return -1; } return std::sqrt(sqrDistLowerBound); } }; } // namespace details /// @addtogroup Traversal_For_Distance /// @{ /// @brief Traversal node for distance computation between BVH models template class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { public: BVHDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { Scalar sz1 = model1->getBV(b1).bv.size(); Scalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); if (l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// @brief The first BVH model const BVHModel* model1; /// @brief The second BVH model const BVHModel* model2; /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; }; /// @brief Traversal node for distance computation between two meshes template class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; using BVHDistanceTraversalNode::enable_statistics; using BVHDistanceTraversalNode::request; using BVHDistanceTraversalNode::result; using BVHDistanceTraversalNode::tf1; using BVHDistanceTraversalNode::model1; using BVHDistanceTraversalNode::model2; using BVHDistanceTraversalNode::num_bv_tests; using BVHDistanceTraversalNode::num_leaf_tests; MeshDistanceTraversalNode() : BVHDistanceTraversalNode() { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; tri_indices2 = NULL; rel_err = this->request.rel_err; abs_err = this->request.abs_err; } void preprocess() { if (!RTIsIdentity) preprocessOrientedNode(); } void postprocess() { if (!RTIsIdentity) postprocessOrientedNode(); } /// @brief BV culling test in one BVTT node Scalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { if (enable_statistics) num_bv_tests++; if (RTIsIdentity) return details::DistanceTraversalBVDistanceLowerBound_impl::run( model1->getBV(b1), model2->getBV(b2)); else return details::DistanceTraversalBVDistanceLowerBound_impl::run( RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); } /// @brief Distance testing between leaves (two triangles) void leafComputeDistance(unsigned int b1, unsigned int b2) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle32& tri_id1 = tri_indices1[primitive_id1]; const Triangle32& tri_id2 = tri_indices2[primitive_id2]; const Vec3s& t11 = vertices1[tri_id1[0]]; const Vec3s& t12 = vertices1[tri_id1[1]]; const Vec3s& t13 = vertices1[tri_id1[2]]; const Vec3s& t21 = vertices2[tri_id2[0]]; const Vec3s& t22 = vertices2[tri_id2[1]]; const Vec3s& t23 = vertices2[tri_id2[2]]; // nearest point pair Vec3s P1, P2, normal; Scalar d2; if (RTIsIdentity) d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, P2); else d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, RT._R(), RT._T(), P1, P2); Scalar d = sqrt(d2); this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2, normal); } /// @brief Whether the traversal process can stop early bool canStop(Scalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } Vec3s* vertices1; Vec3s* vertices2; Triangle32* tri_indices1; Triangle32* tri_indices2; /// @brief relative and absolute error, default value is 0.01 for both terms Scalar rel_err; Scalar abs_err; details::RelativeTransformation RT; private: void preprocessOrientedNode() { const int init_tri_id1 = 0, init_tri_id2 = 0; const Triangle32& init_tri1 = tri_indices1[init_tri_id1]; const Triangle32& init_tri2 = tri_indices2[init_tri_id2]; Vec3s init_tri1_points[3]; Vec3s init_tri2_points[3]; init_tri1_points[0] = vertices1[init_tri1[0]]; init_tri1_points[1] = vertices1[init_tri1[1]]; init_tri1_points[2] = vertices1[init_tri1[2]]; init_tri2_points[0] = vertices2[init_tri2[0]]; init_tri2_points[1] = vertices2[init_tri2[1]]; init_tri2_points[2] = vertices2[init_tri2[2]]; Vec3s p1, p2, normal; Scalar distance = sqrt(TriangleDistance::sqrTriDistance( init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), RT._T(), p1, p2)); result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2, normal); } void postprocessOrientedNode() { /// the points obtained by triDistance are not in world space: both are in /// object1's local coordinate system, so we need to convert them into the /// world space. if (request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2)) { result->nearest_points[0] = tf1.transform(result->nearest_points[0]); result->nearest_points[1] = tf1.transform(result->nearest_points[1]); } } }; /// @brief Traversal node for distance computation between two meshes if their /// underlying BVH node is oriented node (RSS, OBBRSS, kIOS) typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeRSS; typedef MeshDistanceTraversalNode MeshDistanceTraversalNodekIOS; typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; /// @} /// @brief for OBB and RSS, there is local coordinate of BV, so normal need to /// be transformed namespace details { template inline const Matrix3s& getBVAxes(const BV& bv) { return bv.axes; } template <> inline const Matrix3s& getBVAxes(const OBBRSS& bv) { return bv.obb.axes; } } // namespace details } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_node_hfield_shape.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021-2024, INRIA. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H #define COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H /// @cond INTERNAL #include "coal/collision_data.h" #include "coal/shape/geometric_shapes.h" #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes_utility.h" #include "coal/internal/shape_shape_func.h" #include "coal/internal/traversal_node_base.h" #include "coal/internal/traversal.h" #include "coal/internal/intersect.h" #include "coal/hfield.h" #include "coal/shape/convex.h" namespace coal { /// @addtogroup Traversal_For_Collision /// @{ namespace details { template ConvexTpl buildConvexQuadrilateral( const HFNode& node, const HeightField& model) { const MatrixXs& heights = model.getHeights(); const VecXs& x_grid = model.getXGrid(); const VecXs& y_grid = model.getYGrid(); const Scalar min_height = model.getMinHeight(); const Scalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); assert(cell.maxCoeff() > min_height && "max_height is lower than min_height"); // Check whether the geometry // is degenerated std::shared_ptr> pts(new std::vector({ Vec3s(x0, y0, min_height), Vec3s(x0, y1, min_height), Vec3s(x1, y1, min_height), Vec3s(x1, y0, min_height), Vec3s(x0, y0, cell(0, 0)), Vec3s(x0, y1, cell(1, 0)), Vec3s(x1, y1, cell(1, 1)), Vec3s(x1, y0, cell(0, 1)), })); std::shared_ptr> polygons( new std::vector(6)); (*polygons)[0].set(0, 3, 2, 1); // x+ side (*polygons)[1].set(0, 1, 5, 4); // y- side (*polygons)[2].set(1, 2, 6, 5); // x- side (*polygons)[3].set(2, 3, 7, 6); // y+ side (*polygons)[4].set(3, 0, 4, 7); // z- side (*polygons)[5].set(4, 5, 6, 7); // z+ side return ConvexTpl(pts, // points 8, // num points polygons, 6 // number of polygons ); } enum class FaceOrientationConvexPart1 { BOTTOM = 0, TOP = 1, WEST = 2, SOUTH_EAST = 4, NORTH = 8, }; enum class FaceOrientationConvexPart2 { BOTTOM = 0, TOP = 1, SOUTH = 2, NORTH_WEST = 4, EAST = 8, }; template void buildConvexTriangles(const HFNode& node, const HeightField& model, ConvexTpl& convex1, int& convex1_active_faces, ConvexTpl& convex2, int& convex2_active_faces) { const MatrixXs& heights = model.getHeights(); const VecXs& x_grid = model.getXGrid(); const VecXs& y_grid = model.getYGrid(); const Scalar min_height = model.getMinHeight(); const Scalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; const Scalar max_height = node.max_height; const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); const int contact_active_faces = node.contact_active_faces; convex1_active_faces = 0; convex2_active_faces = 0; typedef HFNodeBase::FaceOrientation FaceOrientation; if (contact_active_faces & FaceOrientation::TOP) { convex1_active_faces |= int(details::FaceOrientationConvexPart1::TOP); convex2_active_faces |= int(details::FaceOrientationConvexPart2::TOP); } if (contact_active_faces & FaceOrientation::BOTTOM) { convex1_active_faces |= int(details::FaceOrientationConvexPart1::BOTTOM); convex2_active_faces |= int(details::FaceOrientationConvexPart2::BOTTOM); } // Specific orientation for Convex1 if (contact_active_faces & FaceOrientation::WEST) { convex1_active_faces |= int(details::FaceOrientationConvexPart1::WEST); } if (contact_active_faces & FaceOrientation::NORTH) { convex1_active_faces |= int(details::FaceOrientationConvexPart1::NORTH); } // Specific orientation for Convex2 if (contact_active_faces & FaceOrientation::EAST) { convex2_active_faces |= int(details::FaceOrientationConvexPart2::EAST); } if (contact_active_faces & FaceOrientation::SOUTH) { convex2_active_faces |= int(details::FaceOrientationConvexPart2::SOUTH); } assert(max_height > min_height && "max_height is lower than min_height"); // Check whether the geometry // is degenerated COAL_UNUSED_VARIABLE(max_height); { std::shared_ptr> pts(new std::vector({ Vec3s(x0, y0, min_height), // A Vec3s(x0, y1, min_height), // B Vec3s(x1, y0, min_height), // C Vec3s(x0, y0, cell(0, 0)), // D Vec3s(x0, y1, cell(1, 0)), // E Vec3s(x1, y0, cell(0, 1)), // F })); std::shared_ptr> triangles( new std::vector(8)); (*triangles)[0].set(0, 2, 1); // bottom (*triangles)[1].set(3, 4, 5); // top (*triangles)[2].set(0, 1, 3); // West 1 (*triangles)[3].set(3, 1, 4); // West 2 (*triangles)[4].set(1, 2, 5); // South-East 1 (*triangles)[5].set(1, 5, 4); // South-East 1 (*triangles)[6].set(0, 5, 2); // North 1 (*triangles)[7].set(5, 0, 3); // North 2 convex1.set(pts, // points 6, // num points triangles, 8 // number of polygons ); } { std::shared_ptr> pts(new std::vector({ Vec3s(x0, y1, min_height), // A Vec3s(x1, y1, min_height), // B Vec3s(x1, y0, min_height), // C Vec3s(x0, y1, cell(1, 0)), // D Vec3s(x1, y1, cell(1, 1)), // E Vec3s(x1, y0, cell(0, 1)), // F })); std::shared_ptr> triangles( new std::vector(8)); (*triangles)[0].set(2, 1, 0); // bottom (*triangles)[1].set(3, 4, 5); // top (*triangles)[2].set(0, 1, 3); // South 1 (*triangles)[3].set(3, 1, 4); // South 2 (*triangles)[4].set(0, 5, 2); // North West 1 (*triangles)[5].set(0, 3, 5); // North West 2 (*triangles)[6].set(1, 2, 5); // East 1 (*triangles)[7].set(4, 1, 2); // East 2 convex2.set(pts, // points 6, // num points triangles, 8 // number of polygons ); } } inline Vec3s projectTriangle(const Vec3s& pointA, const Vec3s& pointB, const Vec3s& pointC, const Vec3s& point) { const Project::ProjectResult result = Project::projectTriangle(pointA, pointB, pointC, point); Vec3s res = result.parameterization[0] * pointA + result.parameterization[1] * pointB + result.parameterization[2] * pointC; return res; } inline Vec3s projectTetrahedra(const Vec3s& pointA, const Vec3s& pointB, const Vec3s& pointC, const Vec3s& pointD, const Vec3s& point) { const Project::ProjectResult result = Project::projectTetrahedra(pointA, pointB, pointC, pointD, point); Vec3s res = result.parameterization[0] * pointA + result.parameterization[1] * pointB + result.parameterization[2] * pointC + result.parameterization[3] * pointD; return res; } inline Vec3s computeTriangleNormal(const Triangle32& triangle, const std::vector& points) { const Vec3s pointA = points[triangle[0]]; const Vec3s pointB = points[triangle[1]]; const Vec3s pointC = points[triangle[2]]; const Vec3s normal = (pointB - pointA).cross(pointC - pointA).normalized(); assert(!normal.array().isNaN().any() && "normal is ill-defined"); return normal; } inline Vec3s projectPointOnTriangle(const Vec3s& contact_point, const Triangle32& triangle, const std::vector& points) { const Vec3s pointA = points[triangle[0]]; const Vec3s pointB = points[triangle[1]]; const Vec3s pointC = points[triangle[2]]; const Vec3s contact_point_projected = projectTriangle(pointA, pointB, pointC, contact_point); return contact_point_projected; } inline Scalar distanceContactPointToTriangle(const Vec3s& contact_point, const Triangle32& triangle, const std::vector& points) { const Vec3s contact_point_projected = projectPointOnTriangle(contact_point, triangle, points); return (contact_point_projected - contact_point).norm(); } inline Scalar distanceContactPointToFace(const size_t face_id, const Vec3s& contact_point, const ConvexTpl& convex, size_t& closest_face_id) { assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]"); const std::vector& points = *(convex.points); if (face_id <= 1) { const Triangle32& triangle = (*(convex.polygons))[face_id]; closest_face_id = face_id; return distanceContactPointToTriangle(contact_point, triangle, points); } else { const Triangle32& triangle1 = (*(convex.polygons))[face_id]; const Scalar distance_to_triangle1 = distanceContactPointToTriangle(contact_point, triangle1, points); const Triangle32& triangle2 = (*(convex.polygons))[face_id + 1]; const Scalar distance_to_triangle2 = distanceContactPointToTriangle(contact_point, triangle2, points); if (distance_to_triangle1 > distance_to_triangle2) { closest_face_id = face_id + 1; return distance_to_triangle2; } else { closest_face_id = face_id; return distance_to_triangle1; } } } template bool binCorrection(const ConvexTpl& convex, const int convex_active_faces, const Shape& shape, const Transform3s& shape_pose, Scalar& distance, Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal, Vec3s& face_normal, const bool is_collision) { const Scalar prec = Scalar(1e-12); const std::vector& points = *(convex.points); bool hfield_witness_is_on_bin_side = true; // int closest_face_id_bottom_face = -1; // int closest_face_id_top_face = -1; std::vector active_faces; active_faces.reserve(5); active_faces.push_back(0); active_faces.push_back(1); if (convex_active_faces & 2) active_faces.push_back(2); if (convex_active_faces & 4) active_faces.push_back(4); if (convex_active_faces & 8) active_faces.push_back(6); Triangle32 face_triangle; Scalar shortest_distance_to_face = (std::numeric_limits::max)(); face_normal = normal; for (const size_t active_face : active_faces) { size_t closest_face_id; const Scalar distance_to_face = distanceContactPointToFace( active_face, contact_1, convex, closest_face_id); const bool contact_point_is_on_face = distance_to_face <= prec; if (contact_point_is_on_face) { hfield_witness_is_on_bin_side = false; face_triangle = (*(convex.polygons))[closest_face_id]; shortest_distance_to_face = distance_to_face; break; } else if (distance_to_face < shortest_distance_to_face) { face_triangle = (*(convex.polygons))[closest_face_id]; shortest_distance_to_face = distance_to_face; } } // We correct only if there is a collision with the bin if (is_collision) { if (!face_triangle.isValid()) COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error); const Vec3s face_pointA = points[face_triangle[0]]; face_normal = computeTriangleNormal(face_triangle, points); int hint = 0; // Since we compute the support manually, we need to take into account the // sphere swept radius of the shape. // TODO: take into account the swept-sphere radius of the bin. const Vec3s _support = getSupport( &shape, -shape_pose.rotation().transpose() * face_normal, hint); const Vec3s support = shape_pose.rotation() * _support + shape_pose.translation(); // Project support into the inclined bin having triangle const Scalar offset_plane = face_normal.dot(face_pointA); const Plane projection_plane(face_normal, offset_plane); const Scalar distance_support_projection_plane = projection_plane.signedDistance(support); const Vec3s projected_support = support - distance_support_projection_plane * face_normal; // We need now to project the projected in the triangle shape contact_1 = projectPointOnTriangle(projected_support, face_triangle, points); contact_2 = contact_1 + distance_support_projection_plane * face_normal; normal = face_normal; distance = -std::fabs(distance_support_projection_plane); } return hfield_witness_is_on_bin_side; } template bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request, const ConvexTpl& convex1, const int convex1_active_faces, const ConvexTpl& convex2, const int convex2_active_faces, const Transform3s& tf1, const Shape& shape, const Transform3s& tf2, Scalar& distance, Vec3s& c1, Vec3s& c2, Vec3s& normal, Vec3s& normal_top, bool& hfield_witness_is_on_bin_side) { enum { RTIsIdentity = Options & RelativeTransformationIsIdentity }; const Transform3s Id; // The solver `nsolver` has already been set up by the collision request // `request`. If GJK early stopping is enabled through `request`, it will be // used. // The only thing we need to make sure is that in case of collision, the // penetration information is computed (as we do bins comparison). const bool compute_penetration = true; Vec3s contact1_1, contact1_2, contact2_1, contact2_2; Vec3s normal1, normal1_top, normal2, normal2_top; Scalar distance1, distance2; if (RTIsIdentity) { distance1 = internal::ShapeShapeDistance, Shape>( &convex1, Id, &shape, tf2, nsolver, compute_penetration, contact1_1, contact1_2, normal1); } else { distance1 = internal::ShapeShapeDistance, Shape>( &convex1, tf1, &shape, tf2, nsolver, compute_penetration, contact1_1, contact1_2, normal1); } bool collision1 = (distance1 - request.security_margin <= request.collision_distance_threshold); bool hfield_witness_is_on_bin_side1 = binCorrection(convex1, convex1_active_faces, shape, tf2, distance1, contact1_1, contact1_2, normal1, normal1_top, collision1); if (RTIsIdentity) { distance2 = internal::ShapeShapeDistance, Shape>( &convex2, Id, &shape, tf2, nsolver, compute_penetration, contact2_1, contact2_2, normal2); } else { distance2 = internal::ShapeShapeDistance, Shape>( &convex2, tf1, &shape, tf2, nsolver, compute_penetration, contact2_1, contact2_2, normal2); } bool collision2 = (distance2 - request.security_margin <= request.collision_distance_threshold); bool hfield_witness_is_on_bin_side2 = binCorrection(convex2, convex2_active_faces, shape, tf2, distance2, contact2_1, contact2_2, normal2, normal2_top, collision2); if (collision1 && collision2) { if (distance1 > distance2) // switch values { distance = distance2; c1 = contact2_1; c2 = contact2_2; normal = normal2; normal_top = normal2_top; hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; } else { distance = distance1; c1 = contact1_1; c2 = contact1_2; normal = normal1; normal_top = normal1_top; hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; } return true; } else if (collision1) { distance = distance1; c1 = contact1_1; c2 = contact1_2; normal = normal1; normal_top = normal1_top; hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; return true; } else if (collision2) { distance = distance2; c1 = contact2_1; c2 = contact2_2; normal = normal2; normal_top = normal2_top; hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; return true; } if (distance1 > distance2) // switch values { distance = distance2; c1 = contact2_1; c2 = contact2_2; normal = normal2; normal_top = normal2_top; hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2; } else { distance = distance1; c1 = contact1_1; c2 = contact1_2; normal = normal1; normal_top = normal1_top; hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1; } return false; } } // namespace details /// @brief Traversal node for collision between height field and shape template class HeightFieldShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: typedef CollisionTraversalNodeBase Base; enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; nsolver = NULL; count = 0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(unsigned int b) const { return static_cast(model1->getBV(b).leftChild()); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(unsigned int b) const { return static_cast(model1->getBV(b).rightChild()); } /// test between BV b1 and shape /// @param b1 BV to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. /// @brief BV culling test in one BVTT node bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, Scalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; bool disjoint; if (RTIsIdentity) { assert(false && "must never happened"); disjoint = !this->model1->getBV(b1).bv.overlap( this->model2_bv, this->request, sqrDistLowerBound); } else { disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, this->request, sqrDistLowerBound); } if (disjoint) internal::updateDistanceLowerBoundFromBV(this->request, *this->result, sqrDistLowerBound); assert(!disjoint || sqrDistLowerBound > 0); return disjoint; } /// @brief Intersection testing between leaves (one ConvexTpl and one shape) void leafCollides(unsigned int b1, unsigned int /*b2*/, Scalar& sqrDistLowerBound) const { count++; if (this->enable_statistics) this->num_leaf_tests++; const HFNode& node = this->model1->getBV(b1); // Split quadrilateral primitives into two convex shapes corresponding to // polyhedron with triangular bases. This is essential to keep the convexity // typedef ConvexTpl ConvexQuadrilateral32; // const ConvexQuadrilateral32 convex = // details::buildConvexQuadrilateral(node,*this->model1); typedef ConvexTpl ConvexTriangle32; ConvexTriangle32 convex1, convex2; int convex1_active_faces, convex2_active_faces; // TODO: inherit from hfield's inflation here details::buildConvexTriangles(node, *this->model1, convex1, convex1_active_faces, convex2, convex2_active_faces); // Compute aabb_local for BoundingVolumeGuess case in the GJK solver if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { convex1.computeLocalAABB(); convex2.computeLocalAABB(); } Scalar distance; // Vec3s contact_point, normal; Vec3s c1, c2, normal, normal_face; bool hfield_witness_is_on_bin_side; bool collision = details::shapeDistance( nsolver, this->request, convex1, convex1_active_faces, convex2, convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance, c1, c2, normal, normal_face, hfield_witness_is_on_bin_side); Scalar distToCollision = distance - this->request.security_margin; if (distToCollision <= this->request.collision_distance_threshold) { sqrDistLowerBound = 0; if (this->result->numContacts() < this->request.num_max_contacts) { if (normal_face.isApprox(normal) && (collision || !hfield_witness_is_on_bin_side)) { this->result->addContact(Contact(this->model1, this->model2, (int)b1, (int)Contact::NONE, c1, c2, normal, distance)); assert(this->result->isCollision()); } } } else sqrDistLowerBound = distToCollision * distToCollision; // const Vec3s c1 = contact_point - distance * 0.5 * normal; // const Vec3s c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, distToCollision, c1, c2, normal); assert(this->result->isCollision() || sqrDistLowerBound > 0); } const GJKSolver* nsolver; const HeightField* model1; const S* model2; BV model2_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; mutable int count; }; /// @} /// @addtogroup Traversal_For_Distance /// @{ /// @brief Traversal node for distance between height field and shape template class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: typedef DistanceTraversalNodeBase Base; enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_leaf_tests = 0; query_time_seconds = 0.0; rel_err = 0; abs_err = 0; nsolver = NULL; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { return model1->getBV(b1).bv.distance( model2_bv); // TODO(jcarpent): tf1 is not taken into account here. } /// @brief Distance testing between leaves (one bin of the height field and /// one shape) /// TODO(louis): deal with Hfield-Shape distance just like in Hfield-Shape /// collision (bin correction etc). void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model1->getBV(b1); typedef ConvexTpl ConvexQuadrilateral32; const ConvexQuadrilateral32 convex = details::buildConvexQuadrilateral(node, *this->model1); Vec3s p1, p2, normal; const Scalar distance = internal::ShapeShapeDistance( &convex, this->tf1, this->model2, this->tf2, this->nsolver, this->request.enable_signed_distance, p1, p2, normal); this->result->update(distance, this->model1, this->model2, b1, DistanceResult::NONE, p1, p2, normal); } /// @brief Whether the traversal process can stop early bool canStop(Scalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } Scalar rel_err; Scalar abs_err; const GJKSolver* nsolver; const HeightField* model1; const S* model2; BV model2_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable Scalar query_time_seconds; }; /// @} } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_node_octree.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2022-2023, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRAVERSAL_NODE_OCTREE_H #define COAL_TRAVERSAL_NODE_OCTREE_H /// @cond INTERNAL #include "coal/collision_data.h" #include "coal/internal/traversal_node_base.h" #include "coal/internal/traversal_node_hfield_shape.h" #include "coal/narrowphase/narrowphase.h" #include "coal/hfield.h" #include "coal/octree.h" #include "coal/BVH/BVH_model.h" #include "coal/shape/geometric_shapes_utility.h" #include "coal/internal/shape_shape_func.h" namespace coal { /// @brief Algorithms for collision related with octree class COAL_DLLAPI OcTreeSolver { private: const GJKSolver* solver; mutable const CollisionRequest* crequest; mutable const DistanceRequest* drequest; mutable CollisionResult* cresult; mutable DistanceResult* dresult; public: OcTreeSolver(const GJKSolver* solver_) : solver(solver_), crequest(NULL), drequest(NULL), cresult(NULL), dresult(NULL) {} /// @brief collision between two octrees void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, tree2->getRoot(), tree2->getRootBV(), tf1, tf2); } /// @brief distance between two octrees void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, tree2->getRoot(), tree2->getRootBV(), tf1, tf2); } /// @brief collision between octree and mesh template void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel* tree2, const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, 0, tf1, tf2); } /// @brief distance between octree and mesh template void OcTreeMeshDistance(const OcTree* tree1, const BVHModel* tree2, const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, 0, tf1, tf2); } /// @brief collision between mesh and octree template void MeshOcTreeIntersect(const BVHModel* tree1, const OcTree* tree2, const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), tree1, 0, tf2, tf1); } /// @brief distance between mesh and octree template void MeshOcTreeDistance(const BVHModel* tree1, const OcTree* tree2, const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(), tree2->getRootBV(), tf1, tf2); } template void OcTreeHeightFieldIntersect( const OcTree* tree1, const HeightField* tree2, const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_, Scalar& sqrDistLowerBound) const { crequest = &request_; cresult = &result_; OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, 0, tf1, tf2, sqrDistLowerBound); } template void HeightFieldOcTreeIntersect(const HeightField* tree1, const OcTree* tree2, const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_, Scalar& sqrDistLowerBound) const { crequest = &request_; cresult = &result_; HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(), tree2->getRootBV(), tf1, tf2, sqrDistLowerBound); } /// @brief collision between octree and shape template void OcTreeShapeIntersect(const OcTree* tree, const S& s, const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; AABB bv2; computeBV(s, Transform3s(), bv2); OBB obb2; convertBV(bv2, tf2, obb2); OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, obb2, tf1, tf2); } /// @brief collision between shape and octree template void ShapeOcTreeIntersect(const S& s, const OcTree* tree, const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; AABB bv1; computeBV(s, Transform3s(), bv1); OBB obb1; convertBV(bv1, tf1, obb1); OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, obb1, tf2, tf1); } /// @brief distance between octree and shape template void OcTreeShapeDistance(const OcTree* tree, const S& s, const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; AABB aabb2; computeBV(s, tf2, aabb2); OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, aabb2, tf1, tf2); } /// @brief distance between shape and octree template void ShapeOcTreeDistance(const S& s, const OcTree* tree, const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; AABB aabb1; computeBV(s, tf1, aabb1); OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, aabb1, tf2, tf1); } private: template bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const S& s, const AABB& aabb2, const Transform3s& tf1, const Transform3s& tf2) const { if (!tree1->nodeHasChildren(root1)) { if (tree1->isNodeOccupied(root1)) { Box box; Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); } Vec3s p1, p2, normal; const Scalar distance = internal::ShapeShapeDistance( &box, box_tf, &s, tf2, this->solver, this->drequest->enable_signed_distance, p1, p2, normal); this->dresult->update(distance, tree1, &s, (int)(root1 - tree1->getRoot()), DistanceResult::NONE, p1, p2, normal); return drequest->isSatisfied(*dresult); } else return false; } if (!tree1->isNodeOccupied(root1)) return false; for (unsigned int i = 0; i < 8; ++i) { if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); AABB aabb1; convertBV(child_bv, tf1, aabb1); Scalar d = aabb1.distance(aabb2); if (d < dresult->min_distance) { if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2)) return true; } } } return false; } template bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const S& s, const OBB& obb2, const Transform3s& tf1, const Transform3s& tf2) const { // Empty OcTree is considered free. if (!root1) return false; /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least of one the nodes is free; OR /// 2) (two uncertain nodes or one node occupied and one node /// uncertain) AND cost not required if (tree1->isNodeFree(root1)) return false; else if ((tree1->isNodeUncertain(root1) || s.isUncertain())) return false; else { OBB obb1; convertBV(bv1, tf1, obb1); Scalar sqrDistLowerBound; if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, sqrDistLowerBound); return false; } } if (!tree1->nodeHasChildren(root1)) { assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. Box box; Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); } bool contactNotAdded = (cresult->numContacts() >= crequest->num_max_contacts); std::size_t ncontact = cresult->numContacts(); // Get the number of new contacts added. ncontact = ShapeShapeCollide(&box, box_tf, &s, tf2, solver, *crequest, *cresult) - ncontact; assert(ncontact == 0 || ncontact == 1); if (!contactNotAdded && ncontact == 1) { // Update contact information. const Contact& c = cresult->getContact(cresult->numContacts() - 1); cresult->setContact( cresult->numContacts() - 1, Contact(tree1, c.o2, static_cast(root1 - tree1->getRoot()), c.b2, c.pos, c.normal, c.penetration_depth)); } // no need to call `internal::updateDistanceLowerBoundFromLeaf` here // as it is already done internally in `ShapeShapeCollide` above. return crequest->isSatisfied(*cresult); } for (unsigned int i = 0; i < 8; ++i) { if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2)) return true; } } return false; } template bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const BVHModel* tree2, unsigned int root2, const Transform3s& tf1, const Transform3s& tf2) const { if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { if (tree1->isNodeOccupied(root1)) { Box box; Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); size_t primitive_id = static_cast(tree2->getBV(root2).primitiveId()); const Triangle32& tri_id = (*(tree2->tri_indices))[primitive_id]; TriangleP tri((*(tree2->vertices))[tri_id[0]], (*(tree2->vertices))[tri_id[1]], (*(tree2->vertices))[tri_id[2]]); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); tri.computeLocalAABB(); } Vec3s p1, p2, normal; const Scalar distance = internal::ShapeShapeDistance( &box, box_tf, &tri, tf2, this->solver, this->drequest->enable_signed_distance, p1, p2, normal); this->dresult->update(distance, tree1, tree2, (int)(root1 - tree1->getRoot()), static_cast(primitive_id), p1, p2, normal); return this->drequest->isSatisfied(*dresult); } else return false; } if (!tree1->isNodeOccupied(root1)) return false; if (tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) { for (unsigned int i = 0; i < 8; ++i) { if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); Scalar d; AABB aabb1, aabb2; convertBV(child_bv, tf1, aabb1); convertBV(tree2->getBV(root2).bv, tf2, aabb2); d = aabb1.distance(aabb2); if (d < dresult->min_distance) { if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) return true; } } } } else { Scalar d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); unsigned int child = (unsigned int)tree2->getBV(root2).leftChild(); convertBV(tree2->getBV(child).bv, tf2, aabb2); d = aabb1.distance(aabb2); if (d < dresult->min_distance) { if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) return true; } child = (unsigned int)tree2->getBV(root2).rightChild(); convertBV(tree2->getBV(child).bv, tf2, aabb2); d = aabb1.distance(aabb2); if (d < dresult->min_distance) { if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) return true; } } return false; } /// \return True if the request is satisfied. template bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const BVHModel* tree2, unsigned int root2, const Transform3s& tf1, const Transform3s& tf2) const { // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The // code in this if(!root1) did not output anything so the empty OcTree is // considered free. Should an empty OcTree be considered free ? // Empty OcTree is considered free. if (!root1) return false; BVNode const& bvn2 = tree2->getBV(root2); /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR /// 2) (two uncertain nodes OR one node occupied and one node /// uncertain) AND cost not required if (tree1->isNodeFree(root1)) return false; else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) return false; else { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bvn2.bv, tf2, obb2); Scalar sqrDistLowerBound; if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, sqrDistLowerBound); return false; } } // Check if leaf collides. if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. Box box; Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); size_t primitive_id = static_cast(bvn2.primitiveId()); const Triangle32& tri_id = (*(tree2->tri_indices))[primitive_id]; TriangleP tri((*(tree2->vertices))[tri_id[0]], (*(tree2->vertices))[tri_id[1]], (*(tree2->vertices))[tri_id[2]]); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); tri.computeLocalAABB(); } // When reaching this point, `this->solver` has already been set up // by the CollisionRequest `this->crequest`. // The only thing we need to (and can) pass to `ShapeShapeDistance` is // whether or not penetration information is should be computed in case of // collision. const bool compute_penetration = this->crequest->enable_contact || (this->crequest->security_margin < 0); Vec3s c1, c2, normal; const Scalar distance = internal::ShapeShapeDistance( &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2, normal); const Scalar distToCollision = distance - this->crequest->security_margin; internal::updateDistanceLowerBoundFromLeaf( *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal); if (cresult->numContacts() < crequest->num_max_contacts) { if (distToCollision <= crequest->collision_distance_threshold) { cresult->addContact(Contact( tree1, tree2, (int)(root1 - tree1->getRoot()), static_cast(primitive_id), c1, c2, normal, distance)); } } return crequest->isSatisfied(*cresult); } // Determine which tree to traverse first. if (bvn2.isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { for (unsigned int i = 0; i < 8; ++i) { if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) return true; } } } else { if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, (unsigned int)bvn2.leftChild(), tf1, tf2)) return true; if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, (unsigned int)bvn2.rightChild(), tf1, tf2)) return true; } return false; } /// \return True if the request is satisfied. template bool OcTreeHeightFieldIntersectRecurse( const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const HeightField* tree2, unsigned int root2, const Transform3s& tf1, const Transform3s& tf2, Scalar& sqrDistLowerBound) const { // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The // code in this if(!root1) did not output anything so the empty OcTree is // considered free. Should an empty OcTree be considered free ? // Empty OcTree is considered free. if (!root1) return false; HFNode const& bvn2 = tree2->getBV(root2); /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR /// 2) (two uncertain nodes OR one node occupied and one node /// uncertain) AND cost not required if (tree1->isNodeFree(root1)) return false; else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) return false; else { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bvn2.bv, tf2, obb2); Scalar sqrDistLowerBound_; if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) { if (sqrDistLowerBound_ < sqrDistLowerBound) sqrDistLowerBound = sqrDistLowerBound_; internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, sqrDistLowerBound); return false; } } // Check if leaf collides. if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. Box box; Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); } typedef ConvexTpl ConvexTriangle; ConvexTriangle convex1, convex2; int convex1_active_faces, convex2_active_faces; details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces, convex2, convex2_active_faces); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { convex1.computeLocalAABB(); convex2.computeLocalAABB(); } Vec3s c1, c2, normal, normal_top; Scalar distance; bool hfield_witness_is_on_bin_side; bool collision = details::shapeDistance( solver, *crequest, convex1, convex1_active_faces, convex2, convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal, normal_top, hfield_witness_is_on_bin_side); Scalar distToCollision = distance - crequest->security_margin * (normal_top.dot(normal)); if (distToCollision <= crequest->collision_distance_threshold) { sqrDistLowerBound = 0; if (crequest->num_max_contacts > cresult->numContacts()) { if (normal_top.isApprox(normal) && (collision || !hfield_witness_is_on_bin_side)) { cresult->addContact( Contact(tree1, tree2, (int)(root1 - tree1->getRoot()), (int)Contact::NONE, c1, c2, -normal, distance)); } } } else { Scalar sqrDistLowerBound_ = distToCollision * distToCollision; if (sqrDistLowerBound_ < sqrDistLowerBound) sqrDistLowerBound = sqrDistLowerBound_; } // const Vec3s c1 = contact_point - distance * 0.5 * normal; // const Vec3s c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf( *crequest, *cresult, distToCollision, c1, c2, -normal); assert(cresult->isCollision() || sqrDistLowerBound > 0); return crequest->isSatisfied(*cresult); } // Determine which tree to traverse first. if (bvn2.isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { for (unsigned int i = 0; i < 8; ++i) { if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2, sqrDistLowerBound)) return true; } } } else { if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2, (unsigned int)bvn2.leftChild(), tf1, tf2, sqrDistLowerBound)) return true; if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2, (unsigned int)bvn2.rightChild(), tf1, tf2, sqrDistLowerBound)) return true; } return false; } /// \return True if the request is satisfied. template bool HeightFieldOcTreeIntersectRecurse( const HeightField* tree1, unsigned int root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1, const Transform3s& tf2, Scalar& sqrDistLowerBound) const { // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The // code in this if(!root1) did not output anything so the empty OcTree is // considered free. Should an empty OcTree be considered free ? // Empty OcTree is considered free. if (!root2) return false; HFNode const& bvn1 = tree1->getBV(root1); /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR /// 2) (two uncertain nodes OR one node occupied and one node /// uncertain) AND cost not required if (tree2->isNodeFree(root2)) return false; else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain())) return false; else { OBB obb1, obb2; convertBV(bvn1.bv, tf1, obb1); convertBV(bv2, tf2, obb2); Scalar sqrDistLowerBound_; if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) { if (sqrDistLowerBound_ < sqrDistLowerBound) sqrDistLowerBound = sqrDistLowerBound_; internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, sqrDistLowerBound); return false; } } // Check if leaf collides. if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) { assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain. Box box; Transform3s box_tf; constructBox(bv2, tf2, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); } typedef ConvexTpl ConvexTriangle; ConvexTriangle convex1, convex2; int convex1_active_faces, convex2_active_faces; details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces, convex2, convex2_active_faces); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { convex1.computeLocalAABB(); convex2.computeLocalAABB(); } Vec3s c1, c2, normal, normal_top; Scalar distance; bool hfield_witness_is_on_bin_side; bool collision = details::shapeDistance( solver, *crequest, convex1, convex1_active_faces, convex2, convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal, normal_top, hfield_witness_is_on_bin_side); Scalar distToCollision = distance - crequest->security_margin * (normal_top.dot(normal)); if (distToCollision <= crequest->collision_distance_threshold) { sqrDistLowerBound = 0; if (crequest->num_max_contacts > cresult->numContacts()) { if (normal_top.isApprox(normal) && (collision || !hfield_witness_is_on_bin_side)) { cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE, (int)(root2 - tree2->getRoot()), c1, c2, normal, distance)); } } } else { Scalar sqrDistLowerBound_ = distToCollision * distToCollision; if (sqrDistLowerBound_ < sqrDistLowerBound) sqrDistLowerBound = sqrDistLowerBound_; } // const Vec3s c1 = contact_point - distance * 0.5 * normal; // const Vec3s c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf( *crequest, *cresult, distToCollision, c1, c2, normal); assert(cresult->isCollision() || sqrDistLowerBound > 0); return crequest->isSatisfied(*cresult); } // Determine which tree to traverse first. if (bvn1.isLeaf() || (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child, child_bv, tf1, tf2, sqrDistLowerBound)) return true; } } } else { if (HeightFieldOcTreeIntersectRecurse( tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1, tf2, sqrDistLowerBound)) return true; if (HeightFieldOcTreeIntersectRecurse( tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1, tf2, sqrDistLowerBound)) return true; } return false; } bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1, const Transform3s& tf2) const { if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { Box box1, box2; Transform3s box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box1.computeLocalAABB(); box2.computeLocalAABB(); } Vec3s p1, p2, normal; const Scalar distance = internal::ShapeShapeDistance( &box1, box1_tf, &box2, box2_tf, this->solver, this->drequest->enable_signed_distance, p1, p2, normal); this->dresult->update(distance, tree1, tree2, (int)(root1 - tree1->getRoot()), (int)(root2 - tree2->getRoot()), p1, p2, normal); return drequest->isSatisfied(*dresult); } else return false; } if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false; if (!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { for (unsigned int i = 0; i < 8; ++i) { if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); Scalar d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); convertBV(bv2, tf2, aabb2); d = aabb1.distance(aabb2); if (d < dresult->min_distance) { if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2)) return true; } } } } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); Scalar d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); convertBV(bv2, tf2, aabb2); d = aabb1.distance(aabb2); if (d < dresult->min_distance) { if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2)) return true; } } } } return false; } bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1, const Transform3s& tf2) const { // Empty OcTree is considered free. if (!root1) return false; if (!root2) return false; /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR /// 2) (two uncertain nodes OR one node occupied and one node /// uncertain) AND cost not required if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false; else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2))) return false; bool bothAreLeaves = (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)); if (!bothAreLeaves || !crequest->enable_contact) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); Scalar sqrDistLowerBound; if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { if (cresult->distance_lower_bound > 0 && sqrDistLowerBound < cresult->distance_lower_bound * cresult->distance_lower_bound) cresult->distance_lower_bound = sqrt(sqrDistLowerBound) - crequest->security_margin; return false; } if (bothAreLeaves) { // Both are leaf nodes and overlap if (cresult->distance_lower_bound > 0 && sqrDistLowerBound < cresult->distance_lower_bound * cresult->distance_lower_bound) cresult->distance_lower_bound = sqrt(sqrDistLowerBound) - crequest->security_margin; if (cresult->numContacts() < crequest->num_max_contacts) cresult->addContact( Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), static_cast(root2 - tree2->getRoot()))); return crequest->isSatisfied(*cresult); } } // Both node are leaves if (bothAreLeaves) { assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)); Box box1, box2; Transform3s box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); if (this->solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box1.computeLocalAABB(); box2.computeLocalAABB(); } // When reaching this point, `this->solver` has already been set up // by the CollisionRequest `this->request`. // The only thing we need to (and can) pass to `ShapeShapeDistance` is // whether or not penetration information is should be computed in case of // collision. const bool compute_penetration = (this->crequest->enable_contact || (this->crequest->security_margin < 0)); Vec3s c1, c2, normal; Scalar distance = internal::ShapeShapeDistance( &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1, c2, normal); const Scalar distToCollision = distance - this->crequest->security_margin; internal::updateDistanceLowerBoundFromLeaf( *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal); if (this->cresult->numContacts() < this->crequest->num_max_contacts) { if (distToCollision <= this->crequest->collision_distance_threshold) this->cresult->addContact( Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), static_cast(root2 - tree2->getRoot()), c1, c2, normal, distance)); } return crequest->isSatisfied(*cresult); } // Determine which tree to traverse first. if (!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { for (unsigned int i = 0; i < 8; ++i) { if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2)) return true; } } } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2)) return true; } } } return false; } }; /// @addtogroup Traversal_For_Collision /// @{ /// @brief Traversal node for octree collision class COAL_DLLAPI OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVDisjoints(unsigned, unsigned, Scalar&) const { return false; } void leafCollides(unsigned, unsigned, Scalar& sqrDistLowerBound) const { otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } const OcTree* model1; const OcTree* model2; Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for shape-octree collision template class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } void leafCollides(unsigned int, unsigned int, Scalar& sqrDistLowerBound) const { otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } const S* model1; const OcTree* model2; Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-shape collision template class COAL_DLLAPI OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVDisjoints(unsigned int, unsigned int, coal::Scalar&) const { return false; } void leafCollides(unsigned int, unsigned int, Scalar& sqrDistLowerBound) const { otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } const OcTree* model1; const S* model2; Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for mesh-octree collision template class COAL_DLLAPI MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } void leafCollides(unsigned int, unsigned int, Scalar& sqrDistLowerBound) const { otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } const BVHModel* model1; const OcTree* model2; Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-mesh collision template class COAL_DLLAPI OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } void leafCollides(unsigned int, unsigned int, Scalar& sqrDistLowerBound) const { otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } const OcTree* model1; const BVHModel* model2; Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-height-field collision template class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } void leafCollides(unsigned int, unsigned int, Scalar& sqrDistLowerBound) const { otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request, *result, sqrDistLowerBound); } const OcTree* model1; const HeightField* model2; Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-height-field collision template class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } void leafCollides(unsigned int, unsigned int, Scalar& sqrDistLowerBound) const { otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request, *result, sqrDistLowerBound); } const HeightField* model1; const OcTree* model2; Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; /// @} /// @addtogroup Traversal_For_Distance /// @{ /// @brief Traversal node for octree distance class COAL_DLLAPI OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } Scalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; } bool BVDistanceLowerBound(unsigned, unsigned, Scalar&) const { return false; } void leafComputeDistance(unsigned, unsigned int) const { otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result); } const OcTree* model1; const OcTree* model2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for shape-octree distance template class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: ShapeOcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); } const Shape* model1; const OcTree* model2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-shape distance template class COAL_DLLAPI OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeShapeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); } const OcTree* model1; const Shape* model2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for mesh-octree distance template class COAL_DLLAPI MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: MeshOcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); } const BVHModel* model1; const OcTree* model2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-mesh distance template class COAL_DLLAPI OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeMeshDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); } const OcTree* model1; const BVHModel* model2; const OcTreeSolver* otsolver; }; /// @} } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_node_setup.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRAVERSAL_NODE_SETUP_H #define COAL_TRAVERSAL_NODE_SETUP_H /// @cond INTERNAL #include "coal/internal/tools.h" #include "coal/internal/traversal_node_shapes.h" #include "coal/internal/traversal_node_bvhs.h" #include "coal/internal/traversal_node_bvh_shape.h" // #include #include "coal/internal/traversal_node_hfield_shape.h" #ifdef COAL_HAS_OCTOMAP #include "coal/internal/traversal_node_octree.h" #endif #include "coal/BVH/BVH_utility.h" namespace coal { #ifdef COAL_HAS_OCTOMAP /// @brief Initialize traversal node for collision between two octrees, given /// current object transform inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1, const Transform3s& tf1, const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for distance between two octrees, given /// current object transform inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1, const Transform3s& tf1, const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one shape and one /// octree, given current object transform template bool initialize(ShapeOcTreeCollisionTraversalNode& node, const S& model1, const Transform3s& tf1, const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one octree and one /// shape, given current object transform template bool initialize(OcTreeShapeCollisionTraversalNode& node, const OcTree& model1, const Transform3s& tf1, const S& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for distance between one shape and one /// octree, given current object transform template bool initialize(ShapeOcTreeDistanceTraversalNode& node, const S& model1, const Transform3s& tf1, const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for distance between one octree and one /// shape, given current object transform template bool initialize(OcTreeShapeDistanceTraversalNode& node, const OcTree& model1, const Transform3s& tf1, const S& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one mesh and one /// octree, given current object transform template bool initialize(MeshOcTreeCollisionTraversalNode& node, const BVHModel& model1, const Transform3s& tf1, const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one octree and one /// mesh, given current object transform template bool initialize(OcTreeMeshCollisionTraversalNode& node, const OcTree& model1, const Transform3s& tf1, const BVHModel& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one octree and one /// height field, given current object transform template bool initialize(OcTreeHeightFieldCollisionTraversalNode& node, const OcTree& model1, const Transform3s& tf1, const HeightField& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one height field and /// one octree, given current object transform template bool initialize(HeightFieldOcTreeCollisionTraversalNode& node, const HeightField& model1, const Transform3s& tf1, const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for distance between one mesh and one /// octree, given current object transform template bool initialize(MeshOcTreeDistanceTraversalNode& node, const BVHModel& model1, const Transform3s& tf1, const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one octree and one /// mesh, given current object transform template bool initialize(OcTreeMeshDistanceTraversalNode& node, const OcTree& model1, const Transform3s& tf1, const BVHModel& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } #endif /// @brief Initialize traversal node for collision between two geometric shapes, /// given current object transform template bool initialize(ShapeCollisionTraversalNode& node, const S1& shape1, const Transform3s& tf1, const S2& shape2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result) { node.model1 = &shape1; node.tf1 = tf1; node.model2 = &shape2; node.tf2 = tf2; node.nsolver = nsolver; node.result = &result; return true; } /// @brief Initialize traversal node for collision between one mesh and one /// shape, given current object transform template bool initialize(MeshShapeCollisionTraversalNode& node, BVHModel& model1, Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (!tf1.isIdentity() && model1.vertices.get()) // TODO(jcarpent): vectorized version { std::vector vertices_transformed(model1.num_vertices); const std::vector& model1_vertices_ = *(model1.vertices); for (unsigned int i = 0; i < model1.num_vertices; ++i) { const Vec3s& p = model1_vertices_[i]; Vec3s new_v = tf1.transform(p); vertices_transformed[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed); model1.endReplaceModel(use_refit, refit_bottomup); tf1.setIdentity(); } node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model2, tf2, node.model2_bv); node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; node.tri_indices = model1.tri_indices.get() ? model1.tri_indices->data() : NULL; node.result = &result; return true; } /// @brief Initialize traversal node for collision between one mesh and one /// shape template bool initialize(MeshShapeCollisionTraversalNode& node, const BVHModel& model1, const Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model2, tf2, node.model2_bv); node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; node.tri_indices = model1.tri_indices.get() ? model1.tri_indices->data() : NULL; node.result = &result; return true; } /// @brief Initialize traversal node for collision between one mesh and one /// shape, given current object transform template bool initialize(HeightFieldShapeCollisionTraversalNode& node, HeightField& model1, Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false); /// @brief Initialize traversal node for collision between one mesh and one /// shape template bool initialize(HeightFieldShapeCollisionTraversalNode& node, const HeightField& model1, const Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result) { node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model2, tf2, node.model2_bv); node.result = &result; return true; } /// @cond IGNORE namespace details { template class OrientedNode> static inline bool setupShapeMeshCollisionOrientedNode( OrientedNode& node, const S& model1, const Transform3s& tf1, const BVHModel& model2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result) { if (model2.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model1, tf1, node.model1_bv); node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL; node.tri_indices = model2.tri_indices.get() ? model2.tri_indices->data() : NULL; node.result = &result; return true; } } // namespace details /// @endcond /// @brief Initialize traversal node for collision between two meshes, given the /// current transforms template bool initialize( MeshCollisionTraversalNode& node, BVHModel& model1, Transform3s& tf1, BVHModel& model2, Transform3s& tf2, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (model2.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (!tf1.isIdentity() && model1.vertices.get()) { std::vector vertices_transformed1(model1.num_vertices); const std::vector& model1_vertices_ = *(model1.vertices); for (unsigned int i = 0; i < model1.num_vertices; ++i) { const Vec3s& p = model1_vertices_[i]; Vec3s new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed1); model1.endReplaceModel(use_refit, refit_bottomup); tf1.setIdentity(); } if (!tf2.isIdentity() && model2.vertices.get()) { std::vector vertices_transformed2(model2.num_vertices); const std::vector& model2_vertices_ = *(model2.vertices); for (unsigned int i = 0; i < model2.num_vertices; ++i) { const Vec3s& p = model2_vertices_[i]; Vec3s new_v = tf2.transform(p); vertices_transformed2[i] = new_v; } model2.beginReplaceModel(); model2.replaceSubModel(vertices_transformed2); model2.endReplaceModel(use_refit, refit_bottomup); tf2.setIdentity(); } node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; node.tri_indices1 = model1.tri_indices.get() ? model1.tri_indices->data() : NULL; node.tri_indices2 = model2.tri_indices.get() ? model2.tri_indices->data() : NULL; node.result = &result; return true; } template bool initialize(MeshCollisionTraversalNode& node, const BVHModel& model1, const Transform3s& tf1, const BVHModel& model2, const Transform3s& tf2, CollisionResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (model2.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) node.vertices1 = model1.vertices ? model1.vertices->data() : NULL; node.vertices2 = model2.vertices ? model2.vertices->data() : NULL; node.tri_indices1 = model1.tri_indices.get() ? model1.tri_indices->data() : NULL; node.tri_indices2 = model2.tri_indices.get() ? model2.tri_indices->data() : NULL; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.result = &result; node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation(); node.RT.T.noalias() = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation()); return true; } /// @brief Initialize traversal node for distance between two geometric shapes template bool initialize(ShapeDistanceTraversalNode& node, const S1& shape1, const Transform3s& tf1, const S2& shape2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &shape1; node.tf1 = tf1; node.model2 = &shape2; node.tf2 = tf2; node.nsolver = nsolver; return true; } /// @brief Initialize traversal node for distance computation between two /// meshes, given the current transforms template bool initialize( MeshDistanceTraversalNode& node, BVHModel& model1, Transform3s& tf1, BVHModel& model2, Transform3s& tf2, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (model2.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (!tf1.isIdentity() && model1.vertices.get()) { std::vector vertices_transformed1(model1.num_vertices); const std::vector& model1_vertices_ = *(model1.vertices); for (unsigned int i = 0; i < model1.num_vertices; ++i) { const Vec3s& p = model1_vertices_[i]; Vec3s new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed1); model1.endReplaceModel(use_refit, refit_bottomup); tf1.setIdentity(); } if (!tf2.isIdentity() && model2.vertices.get()) { std::vector vertices_transformed2(model2.num_vertices); const std::vector& model2_vertices_ = *(model2.vertices); for (unsigned int i = 0; i < model2.num_vertices; ++i) { const Vec3s& p = model2_vertices_[i]; Vec3s new_v = tf2.transform(p); vertices_transformed2[i] = new_v; } model2.beginReplaceModel(); model2.replaceSubModel(vertices_transformed2); model2.endReplaceModel(use_refit, refit_bottomup); tf2.setIdentity(); } node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; node.tri_indices1 = model1.tri_indices.get() ? model1.tri_indices->data() : NULL; node.tri_indices2 = model2.tri_indices.get() ? model2.tri_indices->data() : NULL; return true; } /// @brief Initialize traversal node for distance computation between two meshes template bool initialize(MeshDistanceTraversalNode& node, const BVHModel& model1, const Transform3s& tf1, const BVHModel& model2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (model2.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL; node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL; node.tri_indices1 = model1.tri_indices.get() ? model1.tri_indices->data() : NULL; node.tri_indices2 = model2.tri_indices.get() ? model2.tri_indices->data() : NULL; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.RT.R, node.RT.T); COAL_COMPILER_DIAGNOSTIC_POP return true; } /// @brief Initialize traversal node for distance computation between one mesh /// and one shape, given the current transforms template bool initialize(MeshShapeDistanceTraversalNode& node, BVHModel& model1, Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (!tf1.isIdentity() && model1.vertices.get()) { const std::vector& model1_vertices_ = *(model1.vertices); std::vector vertices_transformed1(model1.num_vertices); for (unsigned int i = 0; i < model1.num_vertices; ++i) { const Vec3s& p = model1_vertices_[i]; Vec3s new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed1); model1.endReplaceModel(use_refit, refit_bottomup); tf1.setIdentity(); } node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; node.tri_indices = model1.tri_indices.get() ? model1.tri_indices->data() : NULL; computeBV(model2, tf2, node.model2_bv); return true; } /// @cond IGNORE namespace details { template class OrientedNode> static inline bool setupMeshShapeDistanceOrientedNode( OrientedNode& node, const BVHModel& model1, const Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model2, tf2, node.model2_bv); node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL; node.tri_indices = model1.tri_indices.get() ? model1.tri_indices->data() : NULL; return true; } } // namespace details /// @endcond /// @brief Initialize traversal node for distance computation between one mesh /// and one shape, specialized for RSS type template bool initialize(MeshShapeDistanceTraversalNodeRSS& node, const BVHModel& model1, const Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode( node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize traversal node for distance computation between one mesh /// and one shape, specialized for kIOS type template bool initialize(MeshShapeDistanceTraversalNodekIOS& node, const BVHModel& model1, const Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode( node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize traversal node for distance computation between one mesh /// and one shape, specialized for OBBRSS type template bool initialize(MeshShapeDistanceTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3s& tf1, const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode( node, model1, tf1, model2, tf2, nsolver, request, result); } } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_node_shapes.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRAVERSAL_NODE_SHAPES_H #define COAL_TRAVERSAL_NODE_SHAPES_H /// @cond INTERNAL #include "coal/collision_data.h" #include "coal/BV/BV.h" #include "coal/shape/geometric_shapes_utility.h" #include "coal/internal/traversal_node_base.h" #include "coal/internal/shape_shape_func.h" namespace coal { /// @addtogroup Traversal_For_Collision /// @{ /// @brief Traversal node for collision between two shapes template class COAL_DLLAPI ShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: ShapeCollisionTraversalNode(const CollisionRequest& request) : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; nsolver = NULL; } /// @brief BV culling test in one BVTT node bool BVDisjoints(int, int, Scalar&) const { COAL_THROW_PRETTY("Not implemented", std::runtime_error); } /// @brief Intersection testing between leaves (two shapes) void leafCollides(int, int, Scalar&) const { ShapeShapeCollide(this->model1, this->tf1, this->model2, this->tf2, this->nsolver, this->request, *(this->result)); } const S1* model1; const S2* model2; const GJKSolver* nsolver; }; /// @} /// @addtogroup Traversal_For_Distance /// @{ /// @brief Traversal node for distance between two shapes template class COAL_DLLAPI ShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; nsolver = NULL; } /// @brief BV culling test in one BVTT node Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; // should not be used } /// @brief Distance testing between leaves (two shapes) void leafComputeDistance(unsigned int, unsigned int) const { ShapeShapeDistance(this->model1, this->tf1, this->model2, this->tf2, this->nsolver, this->request, *(this->result)); } const S1* model1; const S2* model2; const GJKSolver* nsolver; }; /// @} } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/internal/traversal_recurse.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRAVERSAL_RECURSE_H #define COAL_TRAVERSAL_RECURSE_H /// @cond INTERNAL #include "coal/BVH/BVH_front.h" #include "coal/internal/traversal_node_base.h" #include "coal/internal/traversal_node_bvhs.h" #include namespace coal { /// Recurse function for collision /// @param node collision node, /// @param b1, b2 ids of bounding volume nodes for object 1 and object 2 /// @retval sqrDistLowerBound squared lower bound on distance between objects. void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, Scalar& sqrDistLowerBound); void collisionNonRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list, Scalar& sqrDistLowerBound); /// @brief Recurse function for distance void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list); /// @brief Recurse function for distance, using queue acceleration void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, unsigned int qsize); /// @brief Recurse function for front list propagation void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, const CollisionRequest& request, CollisionResult& result, BVHFrontList* front_list); } // namespace coal /// @endcond #endif ================================================ FILE: include/coal/logging.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /// This file defines basic logging macros for Coal, based on Boost.Log. /// To enable logging, define the preprocessor macro `COAL_ENABLE_LOGGING`. #ifndef COAL_LOGGING_H #define COAL_LOGGING_H #ifdef COAL_ENABLE_LOGGING #include #define COAL_LOG_INFO(message) BOOST_LOG_TRIVIAL(info) << message #define COAL_LOG_DEBUG(message) BOOST_LOG_TRIVIAL(debug) << message #define COAL_LOG_WARNING(message) BOOST_LOG_TRIVIAL(warning) << message #define COAL_LOG_ERROR(message) BOOST_LOG_TRIVIAL(error) << message #else #define COAL_LOG_INFO(message) #define COAL_LOG_DEBUG(message) #define COAL_LOG_WARNING(message) #define COAL_LOG_ERROR(message) #endif #endif // COAL_LOGGING_H ================================================ FILE: include/coal/math/matrix_3f.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_MATRIX_3F_H #define COAL_MATRIX_3F_H #warning "This file is deprecated. Include instead." // List of equivalent includes. #include "coal/data_types.h" #endif ================================================ FILE: include/coal/math/transform.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_TRANSFORM_H #define COAL_TRANSFORM_H #include "coal/fwd.hh" #include "coal/data_types.h" namespace coal { COAL_DEPRECATED typedef Eigen::Quaternion Quaternion3f; typedef Eigen::Quaternion Quats; static inline std::ostream& operator<<(std::ostream& o, const Quats& q) { o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")"; return o; } /// @brief Simple transform class used locally by InterpMotion class COAL_DLLAPI Transform3s { /// @brief Matrix cache Matrix3s R; /// @brief Translation vector Vec3s T; public: /// @brief Default transform is no movement Transform3s() { setIdentity(); // set matrix_set true } static Transform3s Identity() { return Transform3s(); } /// @brief Construct transform from rotation and translation template Transform3s(const Eigen::MatrixBase& R_, const Eigen::MatrixBase& T_) : R(R_), T(T_) {} /// @brief Construct transform from rotation and translation template Transform3s(const Quats& q_, const Eigen::MatrixBase& T_) : R(q_.toRotationMatrix()), T(T_) {} /// @brief Construct transform from rotation Transform3s(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {} /// @brief Construct transform from rotation Transform3s(const Quats& q_) : R(q_), T(Vec3s::Zero()) {} /// @brief Construct transform from translation Transform3s(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {} /// @brief Construct transform from other transform Transform3s(const Transform3s& tf) : R(tf.R), T(tf.T) {} /// @brief operator = Transform3s& operator=(const Transform3s& tf) { R = tf.R; T = tf.T; return *this; } /// @brief get translation inline const Vec3s& getTranslation() const { return T; } /// @brief get translation inline const Vec3s& translation() const { return T; } /// @brief get translation inline Vec3s& translation() { return T; } /// @brief get rotation inline const Matrix3s& getRotation() const { return R; } /// @brief get rotation inline const Matrix3s& rotation() const { return R; } /// @brief get rotation inline Matrix3s& rotation() { return R; } /// @brief get quaternion inline Quats getQuatRotation() const { return Quats(R); } /// @brief set transform from rotation and translation template inline void setTransform(const Eigen::MatrixBase& R_, const Eigen::MatrixBase& T_) { R.noalias() = R_; T.noalias() = T_; } /// @brief set transform from rotation and translation inline void setTransform(const Quats& q_, const Vec3s& T_) { R = q_.toRotationMatrix(); T = T_; } /// @brief set transform from rotation template inline void setRotation(const Eigen::MatrixBase& R_) { R.noalias() = R_; } /// @brief set transform from translation template inline void setTranslation(const Eigen::MatrixBase& T_) { T.noalias() = T_; } /// @brief set transform from rotation inline void setQuatRotation(const Quats& q_) { R = q_.toRotationMatrix(); } /// @brief transform a given vector by the transform template inline Vec3s transform(const Eigen::MatrixBase& v) const { return R * v + T; } /// @brief transform a given vector by the inverse of the transform template inline Vec3s inverseTransform(const Eigen::MatrixBase& v) const { return R.transpose() * (v - T); } /// @brief inverse transform inline Transform3s& inverseInPlace() { R.transposeInPlace(); T = -R * T; return *this; } /// @brief inverse transform inline Transform3s inverse() { return Transform3s(R.transpose(), -R.transpose() * T); } /// @brief inverse the transform and multiply with another inline Transform3s inverseTimes(const Transform3s& other) const { return Transform3s(R.transpose() * other.R, R.transpose() * (other.T - T)); } /// @brief multiply with another transform inline const Transform3s& operator*=(const Transform3s& other) { T += R * other.T; R *= other.R; return *this; } /// @brief multiply with another transform inline Transform3s operator*(const Transform3s& other) const { return Transform3s(R * other.R, R * other.T + T); } /// @brief check whether the transform is identity inline bool isIdentity( const Scalar& prec = Eigen::NumTraits::dummy_precision()) const { return R.isIdentity(prec) && T.isZero(prec); } /// @brief set the transform to be identity transform inline void setIdentity() { R.setIdentity(); T.setZero(); } /// @brief return a random transform static Transform3s Random() { Transform3s tf = Transform3s(); tf.setRandom(); return tf; } /// @brief set the transform to a random transform inline void setRandom(); /// @brief Comparison operator bool operator==(const Transform3s& other) const { COAL_EQUAL_OPERATOR_CHECK(R == other.getRotation()); COAL_EQUAL_OPERATOR_CHECK(T == other.getTranslation()); return true; } /// @brief Comparison operator bool operator!=(const Transform3s& other) const { return !(*this == other); } /// @brief Prints the transform to the provided output stream. void disp(std::ostream& os, const std::string& prefix = "") const { using namespace std; // formatting for matrix const Eigen::IOFormat mfmt = Eigen::IOFormat(Eigen::StreamPrecision, !Eigen::DontAlignCols, ", ", // coeff separator ",\n" + prefix + " ", // row separator "[", "]", // row prefix and suffix prefix + " [", "]" // matrix prefix and suffix ); // formatting for vector const Eigen::IOFormat vfmt = Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); os << prefix << " Rotation:\n" << R.format(mfmt) << ",\n"; os << prefix << " Translation: " << T.format(vfmt) << ",\n"; } /// \copydoc disp friend std::ostream& operator<<(std::ostream& os, const Transform3s& tf) { os << "Transform3s: {" << "\n"; tf.disp(os); os << "}" << "\n"; return os; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; template inline Quats fromAxisAngle(const Eigen::MatrixBase& axis, Scalar angle) { return Quats(Eigen::AngleAxis(angle, axis)); } /// @brief Uniformly random quaternion sphere. /// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio). inline Quats uniformRandomQuaternion() { // Rotational part const Scalar u1 = (Scalar)rand() / Scalar(RAND_MAX); const Scalar u2 = (Scalar)rand() / Scalar(RAND_MAX); const Scalar u3 = (Scalar)rand() / Scalar(RAND_MAX); const Scalar mult1 = std::sqrt(Scalar(1.0) - u1); const Scalar mult2 = std::sqrt(u1); static const Scalar PI_value = static_cast(EIGEN_PI); Scalar s2 = std::sin(2 * PI_value * u2); Scalar c2 = std::cos(2 * PI_value * u2); Scalar s3 = std::sin(2 * PI_value * u3); Scalar c3 = std::cos(2 * PI_value * u3); Quats q; q.w() = mult1 * s2; q.x() = mult1 * c2; q.y() = mult2 * s3; q.z() = mult2 * c3; return q; } inline void Transform3s::setRandom() { const Quats q = uniformRandomQuaternion(); this->rotation() = q.matrix(); this->translation().setRandom(); } /// @brief Construct othonormal basis from vector. /// The z-axis is the normalized input vector. inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) { Matrix3s basis = Matrix3s::Zero(); basis.col(2) = vec.normalized(); basis.col(1) = -vec.unitOrthogonal(); basis.col(0) = basis.col(1).cross(vec); return basis; } } // namespace coal #endif ================================================ FILE: include/coal/math/types.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Joseph Mirabel */ #ifndef COAL_MATH_TYPES_H #define COAL_MATH_TYPES_H #warning "This file is deprecated. Include instead." // List of equivalent includes. #include "coal/data_types.h" #endif ================================================ FILE: include/coal/math/vec_3f.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_VEC_3F_H #define COAL_VEC_3F_H #warning "This file is deprecated. Include instead." // List of equivalent includes. #include "coal/data_types.h" #endif ================================================ FILE: include/coal/mesh_loader/assimp.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2016-2019, CNRS - LAAS * Copyright (c) 2019, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef COAL_MESH_LOADER_ASSIMP_H #define COAL_MESH_LOADER_ASSIMP_H #include "coal/fwd.hh" #include "coal/config.hh" #include "coal/BV/OBBRSS.h" #include "coal/BVH/BVH_model.h" struct aiScene; namespace Assimp { class Importer; } namespace coal { namespace internal { struct COAL_DLLAPI TriangleAndVertices { std::vector vertices_; std::vector triangles_; }; struct COAL_DLLAPI Loader { Loader(); ~Loader(); void load(const std::string& resource_path); Assimp::Importer* importer; aiScene const* scene; }; /** * @brief Recursive procedure for building a mesh * * @param[in] scale Scale to apply when reading the ressource * @param[in] scene Pointer to the assimp scene * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ COAL_DLLAPI void buildMesh(const coal::Vec3s& scale, const aiScene* scene, unsigned vertices_offset, TriangleAndVertices& tv); /** * @brief Convert an assimp scene to a mesh * * @param[in] scale Scale to apply when reading the ressource * @param[in] scene Pointer to the assimp scene * @param[out] mesh The mesh that must be built */ template inline void meshFromAssimpScene( const coal::Vec3s& scale, const aiScene* scene, const shared_ptr >& mesh) { TriangleAndVertices tv; int res = mesh->beginModel(); if (res != coal::BVH_OK) { COAL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error); } buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv); mesh->addSubModel(tv.vertices_, tv.triangles_); mesh->endModel(); } } // namespace internal /** * @brief Read a mesh file and convert it to a polyhedral mesh * * @param[in] resource_path Path to the ressource mesh file to be read * @param[in] scale Scale to apply when reading the ressource * @param[out] polyhedron The resulted polyhedron */ template inline void loadPolyhedronFromResource( const std::string& resource_path, const coal::Vec3s& scale, const shared_ptr >& polyhedron) { internal::Loader scene; scene.load(resource_path); internal::meshFromAssimpScene(scale, scene.scene, polyhedron); } } // namespace coal #endif // COAL_MESH_LOADER_ASSIMP_H ================================================ FILE: include/coal/mesh_loader/loader.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2016, CNRS - LAAS * Copyright (c) 2020, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef COAL_MESH_LOADER_LOADER_H #define COAL_MESH_LOADER_LOADER_H #include "coal/fwd.hh" #include "coal/config.hh" #include "coal/data_types.h" #include "coal/collision_object.h" #include #include namespace coal { /// Base class for building polyhedron from files. /// This class builds a new object for each file. class COAL_DLLAPI MeshLoader { public: virtual ~MeshLoader() {} virtual BVHModelPtr_t load(const std::string& filename, const Vec3s& scale = Vec3s::Ones()); /// Create an OcTree from a file in binary octomap format. /// \todo add OctreePtr_t virtual CollisionGeometryPtr_t loadOctree(const std::string& filename); MeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : bvType_(bvType) {} private: const NODE_TYPE bvType_; }; /// Class for building polyhedron from files with cache mechanism. /// This class builds a new object for each different file. /// If method CachedMeshLoader::load is called twice with the same arguments, /// the second call returns the result of the first call. class COAL_DLLAPI CachedMeshLoader : public MeshLoader { public: virtual ~CachedMeshLoader() {} CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {} virtual BVHModelPtr_t load(const std::string& filename, const Vec3s& scale); struct COAL_DLLAPI Key { std::string filename; Vec3s scale; Key(const std::string& f, const Vec3s& s) : filename(f), scale(s) {} bool operator<(const CachedMeshLoader::Key& b) const; }; struct COAL_DLLAPI Value { BVHModelPtr_t model; std::time_t mtime; }; typedef std::map Cache_t; const Cache_t& cache() const { return cache_; } private: Cache_t cache_; }; } // namespace coal #endif // COAL_MESH_LOADER_LOADER_H ================================================ FILE: include/coal/narrowphase/gjk.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_GJK_H #define COAL_GJK_H #include #include "coal/narrowphase/minkowski_difference.h" namespace coal { namespace details { /// @brief class for GJK algorithm /// /// @note The computations are performed in the frame of the first shape. struct COAL_DLLAPI GJK { struct COAL_DLLAPI SimplexV { /// @brief support vector for shape 0 and 1. Vec3ps w0, w1; /// @brief support vector (i.e., the furthest point on the shape along the /// support direction) Vec3ps w; }; typedef unsigned char vertex_id_t; /// @brief A simplex is a set of up to 4 vertices. /// Its rank is the number of vertices it contains. /// @note This data structure does **not** own the vertices it refers to. /// To be efficient, the constructor of `GJK` creates storage for 4 vertices. /// Since GJK does not need any more storage, it reuses these vertices /// throughout the algorithm by using multiple instance of this `Simplex` /// class. struct COAL_DLLAPI Simplex { /// @brief simplex vertex SimplexV* vertex[4]; /// @brief size of simplex (number of vertices) vertex_id_t rank; Simplex() {} void reset() { rank = 0; for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr; } }; /// @brief Status of the GJK algorithm: /// DidNotRun: GJK has not been run. /// Failed: GJK did not converge (it exceeded the maximum number of /// iterations). /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited /// before converting. The shapes are not in collision. /// NoCollision: GJK converged and the shapes are not in collision. /// Collision: GJK converged and the shapes are in collision. /// Failed: GJK did not converge. enum Status { DidNotRun, Failed, NoCollisionEarlyStopped, NoCollision, CollisionWithPenetrationInformation, Collision }; public: SolverScalar distance_upper_bound; Status status; GJKVariant gjk_variant; GJKConvergenceCriterion convergence_criterion; GJKConvergenceCriterionType convergence_criterion_type; MinkowskiDiff const* shape; Vec3ps ray; support_func_guess_t support_hint; /// @brief The distance between the two shapes, computed by GJK. /// If the distance is below GJK's threshold, the shapes are in collision in /// the eyes of GJK. If `distance_upper_bound` is set to a value lower than /// infinity, GJK will early stop as soon as it finds `distance` to be greater /// than `distance_upper_bound`. SolverScalar distance; Simplex* simplex; // Pointer to the result of the last run of GJK. private: // max_iteration and tolerance are made private // because they are meant to be set by the `reset` function. size_t max_iterations; SolverScalar tolerance; SimplexV store_v[4]; SimplexV* free_v[4]; vertex_id_t nfree; vertex_id_t current; Simplex simplices[2]; size_t iterations; size_t iterations_momentum_stop; public: /// \param max_iterations_ number of iteration before GJK returns failure. /// \param tolerance_ precision of the algorithm. /// /// The tolerance argument is useful for continuous shapes and for polyhedron /// with some vertices closer than this threshold. /// /// Suggested values are 100 iterations and a tolerance of 1e-6. GJK(size_t max_iterations_, SolverScalar tolerance_) : max_iterations(max_iterations_), tolerance(tolerance_) { COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", std::invalid_argument); initialize(); } /// @brief resets the GJK algorithm, preparing it for a new run. /// Other than the maximum number of iterations and the tolerance, /// this function does **not** modify the parameters of the GJK algorithm. void reset(size_t max_iterations_, SolverScalar tolerance_); /// @brief GJK algorithm, given the initial value guess Status evaluate( const MinkowskiDiff& shape, const Vec3ps& guess, const support_func_guess_t& supportHint = support_func_guess_t::Zero()); /// @brief apply the support function along a direction, the result is return /// in sv inline void getSupport(const Vec3ps& d, SimplexV& sv, support_func_guess_t& hint) const { Vec3s w0, w1; shape->support(d.cast(), w0, w1, hint); sv.w0 = w0.cast(); sv.w1 = w1.cast(); sv.w = sv.w0 - sv.w1; } /// @brief whether the simplex enclose the origin bool encloseOrigin(); /// @brief get the underlying simplex using in GJK, can be used for cache in /// next iteration inline Simplex* getSimplex() const { return simplex; } /// Tells whether the closest points are available. bool hasClosestPoints() const { return distance < distance_upper_bound; } /// Get the witness points on each object, and the corresponding normal. /// @param[in] shape is the Minkowski difference of the two shapes. /// @param[out] w0 is the witness point on shape0. /// @param[out] w1 is the witness point on shape1. /// @param[out] normal is the normal of the separating plane found by /// GJK. It points from shape0 to shape1. void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0, Vec3ps& w1, Vec3ps& normal) const; /// @brief get the guess from current simplex Vec3ps getGuessFromSimplex() const; /// @brief Distance threshold for early break. /// GJK stops when it proved the distance is more than this threshold. /// @note The closest points will be erroneous in this case. /// If you want the closest points, set this to infinity (the default). void setDistanceEarlyBreak(const SolverScalar& dup) { distance_upper_bound = dup; } /// @brief Convergence check used to stop GJK when shapes are not in /// collision. bool checkConvergence(const Vec3ps& w, const SolverScalar& rl, SolverScalar& alpha, const SolverScalar& omega) const; /// @brief Get the max number of iterations of GJK. size_t getNumMaxIterations() const { return max_iterations; } /// @brief Get the tolerance of GJK. SolverScalar getTolerance() const { return tolerance; } /// @brief Get the number of iterations of the last run of GJK. size_t getNumIterations() const { return iterations; } /// @brief Get GJK number of iterations before momentum stops. /// Only usefull if the Nesterov or Polyak acceleration activated. size_t getNumIterationsMomentumStopped() const { return iterations_momentum_stop; } private: /// @brief Initializes the GJK algorithm. /// This function should only be called by the constructor. /// Otherwise use \ref reset. void initialize(); /// @brief discard one vertex from the simplex inline void removeVertex(Simplex& simplex); /// @brief append one vertex to the simplex inline void appendVertex(Simplex& simplex, const Vec3ps& v, support_func_guess_t& hint); /// @brief Project origin (0) onto line a-b /// For a detailed explanation of how to efficiently project onto a simplex, /// check out Ericson's book, page 403: /// https://realtimecollisiondetection.net/ To sum up, a simplex has a voronoi /// region for each feature it has (vertex, edge, face). We find the voronoi /// region in which the origin lies and stop as soon as we find it; we then /// project onto it and return the result. We start by voronoi regions /// generated by vertices then move on to edges then faces. Checking voronoi /// regions is done using simple dot products. Moreover, edges voronoi checks /// reuse computations of vertices voronoi checks. The same goes for faces /// which reuse checks from edges. /// Finally, in addition to the voronoi procedure, checks relying on the order /// of construction /// of the simplex are added. To know more about these, visit /// https://caseymuratori.com/blog_0003. bool projectLineOrigin(const Simplex& current, Simplex& next); /// @brief Project origin (0) onto triangle a-b-c /// See \ref projectLineOrigin for an explanation on simplex projections. bool projectTriangleOrigin(const Simplex& current, Simplex& next); /// @brief Project origin (0) onto tetrahedron a-b-c-d /// See \ref projectLineOrigin for an explanation on simplex projections. bool projectTetrahedraOrigin(const Simplex& current, Simplex& next); }; /// @brief class for EPA algorithm struct COAL_DLLAPI EPA { typedef GJK::SimplexV SimplexVertex; struct COAL_DLLAPI SimplexFace { Vec3ps n; SolverScalar d; bool ignore; // If the origin does not project inside the face, we // ignore this face. size_t vertex_id[3]; // Index of vertex in sv_store. SimplexFace* adjacent_faces[3]; // A face has three adjacent faces. SimplexFace* prev_face; // The previous face in the list. SimplexFace* next_face; // The next face in the list. size_t adjacent_edge[3]; // Each face has 3 edges: `0`, `1` and `2`. // The i-th adjacent face is bound (to this face) // along its `adjacent_edge[i]`-th edge // (with 0 <= i <= 2). size_t pass; SimplexFace() : n(Vec3ps::Zero()), ignore(false) {}; }; /// @brief The simplex list of EPA is a linked list of faces. /// Note: EPA's linked list does **not** own any memory. /// The memory it refers to is contiguous and owned by a std::vector. struct COAL_DLLAPI SimplexFaceList { SimplexFace* root; size_t count; SimplexFaceList() : root(nullptr), count(0) {} void reset() { root = nullptr; count = 0; } void append(SimplexFace* face) { face->prev_face = nullptr; face->next_face = root; if (root != nullptr) root->prev_face = face; root = face; ++count; } void remove(SimplexFace* face) { if (face->next_face != nullptr) face->next_face->prev_face = face->prev_face; if (face->prev_face != nullptr) face->prev_face->next_face = face->next_face; if (face == root) root = face->next_face; --count; } }; /// @brief We bind the face `fa` along its edge `ea` to the face `fb` along /// its edge `fb`. static inline void bind(SimplexFace* fa, size_t ea, SimplexFace* fb, size_t eb) { assert(ea == 0 || ea == 1 || ea == 2); assert(eb == 0 || eb == 1 || eb == 2); fa->adjacent_edge[ea] = eb; fa->adjacent_faces[ea] = fb; fb->adjacent_edge[eb] = ea; fb->adjacent_faces[eb] = fa; } struct COAL_DLLAPI SimplexHorizon { SimplexFace* current_face; // current face in the horizon SimplexFace* first_face; // first face in the horizon size_t num_faces; // number of faces in the horizon SimplexHorizon() : current_face(nullptr), first_face(nullptr), num_faces(0) {} }; enum Status { DidNotRun = -1, Failed = 0, Valid = 1, AccuracyReached = 1 << 1 | Valid, Degenerated = 1 << 1 | Failed, NonConvex = 2 << 1 | Failed, InvalidHull = 3 << 1 | Failed, OutOfFaces = 4 << 1 | Failed, OutOfVertices = 5 << 1 | Failed, FallBack = 6 << 1 | Failed }; public: Status status; GJK::Simplex result; Vec3ps normal; support_func_guess_t support_hint; SolverScalar depth; SimplexFace* closest_face; private: // max_iteration and tolerance are made private // because they are meant to be set by the `reset` function. size_t max_iterations; SolverScalar tolerance; std::vector sv_store; std::vector fc_store; SimplexFaceList hull, stock; size_t num_vertices; // number of vertices in polytpoe constructed by EPA size_t iterations; public: EPA(size_t max_iterations_, SolverScalar tolerance_) : max_iterations(max_iterations_), tolerance(tolerance_) { initialize(); } /// @brief Copy constructor of EPA. /// Mostly needed for the copy constructor of `GJKSolver`. EPA(const EPA& other) : max_iterations(other.max_iterations), tolerance(other.tolerance), sv_store(other.sv_store), fc_store(other.fc_store) { initialize(); } /// @brief Get the max number of iterations of EPA. size_t getNumMaxIterations() const { return max_iterations; } /// @brief Get the max number of vertices of EPA. size_t getNumMaxVertices() const { return sv_store.size(); } /// @brief Get the max number of faces of EPA. size_t getNumMaxFaces() const { return fc_store.size(); } /// @brief Get the tolerance of EPA. SolverScalar getTolerance() const { return tolerance; } /// @brief Get the number of iterations of the last run of EPA. size_t getNumIterations() const { return iterations; } /// @brief Get the number of vertices in the polytope of the last run of EPA. size_t getNumVertices() const { return num_vertices; } /// @brief Get the number of faces in the polytope of the last run of EPA. size_t getNumFaces() const { return hull.count; } /// @brief resets the EPA algorithm, preparing it for a new run. /// It potentially reallocates memory for the vertices and faces /// if the passed parameters are bigger than the previous ones. /// This function does **not** modify the parameters of the EPA algorithm, /// i.e. the maximum number of iterations and the tolerance. /// @note calling this function destroys the previous state of EPA. /// In the future, we may want to copy it instead, i.e. when EPA will /// be (properly) warm-startable. void reset(size_t max_iterations, SolverScalar tolerance); /// \return a Status which can be demangled using (status & Valid) or /// (status & Failed). The other values provide a more detailled /// status Status evaluate(GJK& gjk, const Vec3ps& guess); /// Get the witness points on each object, and the corresponding normal. /// @param[in] shape is the Minkowski difference of the two shapes. /// @param[out] w0 is the witness point on shape0. /// @param[out] w1 is the witness point on shape1. /// @param[in] normal is the normal found by EPA. It points from shape0 to /// shape1. The normal is used to correct the witness points on the shapes if /// the shapes have a non-zero swept-sphere radius. void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0, Vec3ps& w1, Vec3ps& normal) const; private: /// @brief Allocates memory for the EPA algorithm. /// This function should only be called by the constructor. /// Otherwise use \ref reset. void initialize(); bool getEdgeDist(SimplexFace* face, const SimplexVertex& a, const SimplexVertex& b, SolverScalar& dist); /// @brief Add a new face to the polytope. /// This function sets the `ignore` flag to `true` if the origin does not /// project inside the face. SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex, bool force = false); /// @brief Find the best polytope face to split SimplexFace* findClosestFace(); /// @brief the goal is to add a face connecting vertex w and face edge f[e] bool expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, SimplexHorizon& horizon); // @brief Use this function to debug expand if needed. // void PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w); }; } // namespace details } // namespace coal #endif ================================================ FILE: include/coal/narrowphase/minkowski_difference.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ #ifndef COAL_MINKOWSKI_DIFFERENCE_H #define COAL_MINKOWSKI_DIFFERENCE_H #include "coal/shape/geometric_shapes.h" #include "coal/math/transform.h" #include "coal/narrowphase/support_functions.h" namespace coal { namespace details { /// @brief Minkowski difference class of two shapes /// /// @note The Minkowski difference is expressed in the frame of the first shape. struct COAL_DLLAPI MinkowskiDiff { typedef Eigen::Array Array2; /// @brief points to two shapes const ShapeBase* shapes[2]; /// @brief Store temporary data for the computation of the support point for /// each shape. ShapeSupportData data[2]; /// @brief rotation from shape1 to shape0 /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. Matrix3s oR1; /// @brief translation from shape1 to shape0 /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. Vec3s ot1; /// @brief The radii of the sphere swepted around each shape of the Minkowski /// difference. The 2 values correspond to the swept-sphere radius of shape 0 /// and shape 1. Array2 swept_sphere_radius; /// @brief Wether or not to use the normalize heuristic in the GJK Nesterov /// acceleration. This setting is only applied if the Nesterov acceleration in /// the GJK class is active. bool normalize_support_direction; typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff, const Vec3s& dir, Vec3s& support0, Vec3s& support1, support_func_guess_t& hint, ShapeSupportData data[2]); GetSupportFunction getSupportFunc; MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {} /// @brief Set the two shapes, assuming the relative transformation between /// them is identity. /// Consequently, all support points computed with the MinkowskiDiff /// will be expressed in the world frame. /// @param shape0 the first shape. /// @param shape1 the second shape. /// @tparam _SupportOptions is a value of the SupportOptions enum. If set to /// `WithSweptSphere`, the support computation will take into account the /// swept sphere radius of the shapes. If set to `NoSweptSphere`, where /// this information is simply stored in the Minkowski's difference /// `swept_sphere_radius` array. This array is then used to correct the /// solution found when GJK or EPA have converged. /// /// @note In practice, there is no need to take into account the swept-sphere /// radius in the iterations of GJK/EPA. It is in fact detrimental to the /// convergence of both algos. This is because it makes corners and edges of /// shapes look strictly convex to the algorithms, which leads to poor /// convergence. This swept sphere template parameter is only here for /// debugging purposes and for specific uses cases where the swept sphere /// radius is needed in the support function. The rule is simple. When /// interacting with GJK/EPA, the `SupportOptions` template /// parameter should **always** be set to `NoSweptSphere` (except for /// debugging or testing purposes). When working directly with the shapes /// involved in the collision, and not relying on GJK/EPA, the /// `SupportOptions` template parameter should be set to `WithSweptSphere`. /// This is for example the case for specialized collision/distance functions. template void set(const ShapeBase* shape0, const ShapeBase* shape1); /// @brief Set the two shapes, with a relative transformation from shape0 to /// shape1. Consequently, all support points computed with the MinkowskiDiff /// will be expressed in the frame of shape0. /// @param shape0 the first shape. /// @param shape1 the second shape. /// @param tf0 the transformation of the first shape. /// @param tf1 the transformation of the second shape. /// @tparam _SupportOptions see `set(const ShapeBase*, const /// ShapeBase*)` for more details. template void set(const ShapeBase* shape0, const ShapeBase* shape1, const Transform3s& tf0, const Transform3s& tf1); /// @brief support function for shape0. /// The output vector is expressed in the local frame of shape0. /// @return a point which belongs to the set {argmax_{v in shape0} /// v.dot(dir)}, i.e a support of shape0 in direction dir. /// @param dir support direction. /// @param hint used to initialize the search when shape is a ConvexBase /// object. /// @tparam _SupportOptions see `set(const ShapeBase*, const /// ShapeBase*)` for more details. template inline Vec3s support0(const Vec3s& dir, int& hint) const { return getSupport<_SupportOptions>(shapes[0], dir, hint); } /// @brief support function for shape1. /// The output vector is expressed in the local frame of shape0. /// This is mandatory because in the end we are interested in support points /// of the Minkowski difference; hence the supports of shape0 and shape1 must /// live in the same frame. /// @return a point which belongs to the set {tf * argmax_{v in shape1} /// v.dot(R^T * dir)}, i.e. the support of shape1 in direction dir (tf is the /// tranform from shape1 to shape0). /// @param dir support direction. /// @param hint used to initialize the search when shape is a ConvexBase. /// @tparam _SupportOptions see `set(const ShapeBase*, const /// ShapeBase*)` for more details. template inline Vec3s support1(const Vec3s& dir, int& hint) const { // clang-format off return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1; // clang-format on } /// @brief Support function for the pair of shapes. This method assumes `set` /// has already been called. /// @param[in] dir the support direction. /// @param[out] supp0 support of shape0 in direction dir, expressed in the /// frame of shape0. /// @param[out] supp1 support of shape1 in direction -dir, expressed in the /// frame of shape0. /// @param[in/out] hint used to initialize the search when shape is a /// ConvexBase object. inline void support(const Vec3s& dir, Vec3s& supp0, Vec3s& supp1, support_func_guess_t& hint) const { assert(getSupportFunc != NULL); getSupportFunc(*this, dir, supp0, supp1, hint, const_cast(data)); } }; } // namespace details } // namespace coal #endif // COAL_MINKOWSKI_DIFFERENCE_H ================================================ FILE: include/coal/narrowphase/narrowphase.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique * All rights reserved. * Copyright (c) 2021-2024, INRIA * All rights reserved. * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan, Florent Lamiraux */ #ifndef COAL_NARROWPHASE_H #define COAL_NARROWPHASE_H #include #include "coal/narrowphase/gjk.h" #include "coal/collision_data.h" #include "coal/narrowphase/narrowphase_defaults.h" #include "coal/logging.h" namespace coal { /// @brief collision and distance solver based on the GJK and EPA algorithms. /// Originally, GJK and EPA were implemented in fcl which itself took /// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA /// have been largely modified to be faster and more robust to numerical /// accuracy and edge cases. struct COAL_DLLAPI GJKSolver { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @brief GJK algorithm mutable details::GJK gjk; /// @brief maximum number of iterations of GJK size_t gjk_max_iterations; /// @brief tolerance of GJK Scalar gjk_tolerance; /// @brief which warm start to use for GJK GJKInitialGuess gjk_initial_guess; /// @brief Whether smart guess can be provided /// @Deprecated Use gjk_initial_guess instead COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) bool enable_cached_guess; /// @brief smart guess mutable Vec3s cached_guess; /// @brief smart guess for the support function mutable support_func_guess_t support_func_cached_guess; /// @brief If GJK can guarantee that the distance between the shapes is /// greater than this value, it will early stop. Scalar distance_upper_bound; /// @brief Variant of the GJK algorithm (Default, Nesterov or Polyak). GJKVariant gjk_variant; /// @brief Convergence criterion for GJK GJKConvergenceCriterion gjk_convergence_criterion; /// @brief Absolute or relative convergence criterion for GJK GJKConvergenceCriterionType gjk_convergence_criterion_type; /// @brief EPA algorithm mutable details::EPA epa; /// @brief maximum number of iterations of EPA size_t epa_max_iterations; /// @brief tolerance of EPA Scalar epa_tolerance; /// @brief Minkowski difference used by GJK and EPA algorithms mutable details::MinkowskiDiff minkowski_difference; private: // Used internally for assertion checks. static constexpr Scalar m_dummy_precision = Scalar(1e-6); public: COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS /// @brief Default constructor for GJK algorithm /// By default, we don't want EPA to allocate memory because /// certain functions of the `GJKSolver` class have specializations /// which don't use EPA (and/or GJK). /// So we give EPA's constructor a max number of iterations of zero. /// Only the functions that need EPA will reset the algorithm and allocate /// memory if needed. GJKSolver() : gjk(GJK_DEFAULT_MAX_ITERATIONS, GJK_DEFAULT_TOLERANCE), gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS), gjk_tolerance(GJK_DEFAULT_TOLERANCE), gjk_initial_guess(GJKInitialGuess::DefaultGuess), enable_cached_guess(false), // Use gjk_initial_guess instead cached_guess(Vec3s(1, 0, 0)), support_func_cached_guess(support_func_guess_t::Zero()), distance_upper_bound((std::numeric_limits::max)()), gjk_variant(GJKVariant::DefaultGJK), gjk_convergence_criterion(GJKConvergenceCriterion::Default), gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute), epa(0, EPA_DEFAULT_TOLERANCE), epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS), epa_tolerance(EPA_DEFAULT_TOLERANCE) {} /// @brief Constructor from a DistanceRequest /// /// \param[in] request DistanceRequest input /// /// See the default constructor; by default, we don't want /// EPA to allocate memory so we call EPA's constructor with 0 max /// number of iterations. /// However, the `set` method stores the actual values of the request. /// EPA will thus allocate memory only if needed. explicit GJKSolver(const DistanceRequest& request) : gjk(request.gjk_max_iterations, request.gjk_tolerance), epa(0, request.epa_tolerance) { this->cached_guess = Vec3s(1, 0, 0); this->support_func_cached_guess = support_func_guess_t::Zero(); set(request); } /// @brief setter from a DistanceRequest /// /// \param[in] request DistanceRequest input /// void set(const DistanceRequest& request) { // --------------------- // GJK settings this->gjk_initial_guess = request.gjk_initial_guess; this->enable_cached_guess = request.enable_cached_gjk_guess; if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || this->enable_cached_guess) { this->cached_guess = request.cached_gjk_guess; this->support_func_cached_guess = request.cached_support_func_guess; } this->gjk_max_iterations = request.gjk_max_iterations; this->gjk_tolerance = request.gjk_tolerance; // For distance computation, we don't want GJK to early stop this->distance_upper_bound = (std::numeric_limits::max)(); this->gjk_variant = request.gjk_variant; this->gjk_convergence_criterion = request.gjk_convergence_criterion; this->gjk_convergence_criterion_type = request.gjk_convergence_criterion_type; // --------------------- // EPA settings this->epa_max_iterations = request.epa_max_iterations; this->epa_tolerance = request.epa_tolerance; // --------------------- // Reset GJK and EPA status this->epa.status = details::EPA::Status::DidNotRun; this->gjk.status = details::GJK::Status::DidNotRun; } /// @brief Constructor from a CollisionRequest /// /// \param[in] request CollisionRequest input /// /// See the default constructor; by default, we don't want /// EPA to allocate memory so we call EPA's constructor with 0 max /// number of iterations. /// However, the `set` method stores the actual values of the request. /// EPA will thus allocate memory only if needed. explicit GJKSolver(const CollisionRequest& request) : gjk(request.gjk_max_iterations, request.gjk_tolerance), epa(0, request.epa_tolerance) { this->cached_guess = Vec3s(1, 0, 0); this->support_func_cached_guess = support_func_guess_t::Zero(); set(request); } /// @brief setter from a CollisionRequest /// /// \param[in] request CollisionRequest input /// void set(const CollisionRequest& request) { // --------------------- // GJK settings this->gjk_initial_guess = request.gjk_initial_guess; this->enable_cached_guess = request.enable_cached_gjk_guess; if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || this->enable_cached_guess) { this->cached_guess = request.cached_gjk_guess; this->support_func_cached_guess = request.cached_support_func_guess; } this->gjk_tolerance = request.gjk_tolerance; this->gjk_max_iterations = request.gjk_max_iterations; // The distance upper bound should be at least greater to the requested // security margin. Otherwise, we will likely miss some collisions. this->distance_upper_bound = (std::max)(Scalar(0), (std::max)(request.distance_upper_bound, request.security_margin)); this->gjk_variant = request.gjk_variant; this->gjk_convergence_criterion = request.gjk_convergence_criterion; this->gjk_convergence_criterion_type = request.gjk_convergence_criterion_type; // --------------------- // EPA settings this->epa_max_iterations = request.epa_max_iterations; this->epa_tolerance = request.epa_tolerance; // --------------------- // Reset GJK and EPA status this->gjk.status = details::GJK::Status::DidNotRun; this->epa.status = details::EPA::Status::DidNotRun; } /// @brief Copy constructor GJKSolver(const GJKSolver& other) = default; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool operator==(const GJKSolver& other) const { COAL_EQUAL_OPERATOR_CHECK(this->enable_cached_guess == other.enable_cached_guess); COAL_EQUAL_OPERATOR_CHECK(this->cached_guess == other.cached_guess); COAL_EQUAL_OPERATOR_CHECK(this->support_func_cached_guess == other.support_func_cached_guess); COAL_EQUAL_OPERATOR_CHECK(this->gjk_max_iterations == other.gjk_max_iterations); COAL_EQUAL_OPERATOR_CHECK(this->gjk_tolerance == other.gjk_tolerance); COAL_EQUAL_OPERATOR_CHECK(this->distance_upper_bound == other.distance_upper_bound); COAL_EQUAL_OPERATOR_CHECK(this->gjk_variant == other.gjk_variant); COAL_EQUAL_OPERATOR_CHECK(this->gjk_convergence_criterion == other.gjk_convergence_criterion); COAL_EQUAL_OPERATOR_CHECK(this->gjk_convergence_criterion_type == other.gjk_convergence_criterion_type); COAL_EQUAL_OPERATOR_CHECK(this->gjk_initial_guess == other.gjk_initial_guess); COAL_EQUAL_OPERATOR_CHECK(this->epa_max_iterations == other.epa_max_iterations); COAL_EQUAL_OPERATOR_CHECK(this->epa_tolerance == other.epa_tolerance); return true; } COAL_COMPILER_DIAGNOSTIC_POP bool operator!=(const GJKSolver& other) const { return !(*this == other); } /// @brief Helper to return the precision of the solver on the distance /// estimate, depending on whether or not `compute_penetration` is true. Scalar getDistancePrecision(const bool compute_penetration) const { return compute_penetration ? (std::max)(this->gjk_tolerance, this->epa_tolerance) : this->gjk_tolerance; } /// @brief Uses GJK and EPA to compute the distance between two shapes. /// @param `s1` the first shape. /// @param `tf1` the transformation of the first shape. /// @param `s2` the second shape. /// @param `tf2` the transformation of the second shape. /// @param `compute_penetration` if true and GJK finds the shape in collision, /// the EPA algorithm is also ran to compute penetration information. /// @param[out] `p1` the witness point on the first shape. /// @param[out] `p2` the witness point on the second shape. /// @param[out] `normal` the normal of the collision, pointing from the first /// to the second shape. /// @return the estimate of the distance between the two shapes. /// /// @note: if `this->distance_upper_bound` is set to a positive value, GJK /// will early stop if it finds the distance to be above this value. The /// distance returned by `this->shapeDistance` will be a lower bound on the /// distance between the two shapes. /// /// @note: the variables `this->gjk.status` and `this->epa.status` can be used /// to examine the status of GJK and EPA. /// /// @note: GJK and EPA give an estimate of the distance between the two /// shapes. This estimate is precise up to the tolerance of the algorithms: /// - If `compute_penetration` is false, the distance is precise up to /// `gjk_tolerance`. /// - If `compute_penetration` is true, the distance is precise up to /// `std::max(gjk_tolerance, epa_tolerance)` /// It's up to the user to decide whether the shapes are in collision or not, /// based on that estimate. template Scalar shapeDistance(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2, const bool compute_penetration, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { constexpr bool relative_transformation_already_computed = false; Scalar distance; this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal, relative_transformation_already_computed); return distance; } /// @brief Partial specialization of `shapeDistance` for the case where the /// second shape is a triangle. It is more efficient to pre-compute the /// relative transformation between the two shapes before calling GJK/EPA. template Scalar shapeDistance(const S1& s1, const Transform3s& tf1, const TriangleP& s2, const Transform3s& tf2, const bool compute_penetration, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { const Transform3s tf_1M2(tf1.inverseTimes(tf2)); TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b), tf_1M2.transform(s2.c)); if (this->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { tri.computeLocalAABB(); } constexpr bool relative_transformation_already_computed = true; Scalar distance; this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1, p2, normal, relative_transformation_already_computed); return distance; } /// @brief See other partial template specialization of shapeDistance above. template Scalar shapeDistance(const TriangleP& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2, const bool compute_penetration, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { Scalar distance = this->shapeDistance( s2, tf2, s1, tf1, compute_penetration, p2, p1, normal); normal = -normal; return distance; } protected: /// @brief initialize GJK. /// This method assumes `minkowski_difference` has been set. template void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3s& guess, support_func_guess_t& support_hint, const Vec3s& default_guess = Vec3s(1, 0, 0)) const { // There is no reason not to warm-start the support function, so we always // do it. support_hint = this->support_func_cached_guess; // The following switch takes care of the GJK warm-start. switch (gjk_initial_guess) { case GJKInitialGuess::DefaultGuess: guess = default_guess; break; case GJKInitialGuess::CachedGuess: guess = this->cached_guess; break; case GJKInitialGuess::BoundingVolumeGuess: if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { COAL_THROW_PRETTY( "computeLocalAABB must have been called on the shapes before " "using " "GJKInitialGuess::BoundingVolumeGuess.", std::logic_error); } guess.noalias() = s1.aabb_local.center() - (this->minkowski_difference.oR1 * s2.aabb_local.center() + this->minkowski_difference.ot1); break; default: COAL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); } // TODO: use gjk_initial_guess instead COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (this->enable_cached_guess) { guess = this->cached_guess; } COAL_COMPILER_DIAGNOSTIC_POP } /// @brief Runs the GJK algorithm. /// @param `s1` the first shape. /// @param `tf1` the transformation of the first shape. /// @param `s2` the second shape. /// @param `tf2` the transformation of the second shape. /// @param `compute_penetration` if true and if the shapes are in found in /// collision, the EPA algorithm is also ran to compute penetration /// information. /// @param[out] `distance` the distance between the two shapes. /// @param[out] `p1` the witness point on the first shape. /// @param[out] `p2` the witness point on the second shape. /// @param[out] `normal` the normal of the collision, pointing from the first /// to the second shape. /// @param `relative_transformation_already_computed` whether the relative /// transformation between the two shapes has already been computed. /// @tparam SupportOptions, see `MinkowskiDiff::set`. Whether the support /// computations should take into account the shapes' swept-sphere radii /// during the iterations of GJK and EPA. Please leave this default value to /// `false` unless you know what you are doing. This template parameter is /// only used for debugging/testing purposes. In short, there is no need to /// take into account the swept sphere radius when computing supports in the /// iterations of GJK and EPA. GJK and EPA will correct the solution once they /// have converged. /// @return the estimate of the distance between the two shapes. /// /// @note: The variables `this->gjk.status` and `this->epa.status` can be used /// to examine the status of GJK and EPA. template void runGJKAndEPA( const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2, const bool compute_penetration, Scalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal, const bool relative_transformation_already_computed = false) const { // Reset internal state of GJK algorithm if (relative_transformation_already_computed) this->minkowski_difference.set<_SupportOptions>(&s1, &s2); else this->minkowski_difference.set<_SupportOptions>(&s1, &s2, tf1, tf2); this->gjk.reset(this->gjk_max_iterations, this->gjk_tolerance); this->gjk.setDistanceEarlyBreak(this->distance_upper_bound); this->gjk.gjk_variant = this->gjk_variant; this->gjk.convergence_criterion = this->gjk_convergence_criterion; this->gjk.convergence_criterion_type = this->gjk_convergence_criterion_type; this->epa.status = details::EPA::Status::DidNotRun; // Get initial guess for GJK: default, cached or bounding volume guess Vec3s guess; support_func_guess_t support_hint; getGJKInitialGuess(*(this->minkowski_difference.shapes[0]), *(this->minkowski_difference.shapes[1]), guess, support_hint); this->gjk.evaluate(this->minkowski_difference, guess.cast(), support_hint); switch (this->gjk.status) { case details::GJK::DidNotRun: COAL_ASSERT(false, "GJK did not run. It should have!", std::logic_error); this->cached_guess = Vec3s(1, 0, 0); this->support_func_cached_guess.setZero(); distance = -(std::numeric_limits::max)(); p1 = p2 = normal = Vec3s::Constant(std::numeric_limits::quiet_NaN()); break; case details::GJK::Failed: // // GJK ran out of iterations. COAL_LOG_WARNING("GJK ran out of iterations."); GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::GJK::NoCollisionEarlyStopped: // // Case where GJK early stopped because the distance was found to be // above the `distance_upper_bound`. // The two witness points have no meaning. GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); COAL_ASSERT(distance >= this->gjk.distance_upper_bound - this->m_dummy_precision, "The distance should be bigger than GJK's " "`distance_upper_bound`.", std::logic_error); break; case details::GJK::NoCollision: // // Case where GJK converged and proved that the shapes are not in // collision, i.e their distance is above GJK's tolerance (default // 1e-6). GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); COAL_ASSERT(std::abs((p1 - p2).norm() - distance) <= this->gjk.getTolerance() + this->m_dummy_precision, "The distance found by GJK should coincide with the " "distance between the closest points.", std::logic_error); break; // // Next are the cases where GJK found the shapes to be in collision, i.e. // their distance is below GJK's tolerance (default 1e-6). case details::GJK::CollisionWithPenetrationInformation: GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); COAL_ASSERT( distance <= this->gjk.getTolerance() + this->m_dummy_precision, "The distance found by GJK should be negative or at " "least below GJK's tolerance.", std::logic_error); break; case details::GJK::Collision: if (!compute_penetration) { // Skip EPA and set the witness points and the normal to inf. GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); } else { // // GJK was not enough to recover the penetration information. // We need to run the EPA algorithm to find the witness points, // penetration depth and the normal. // Reset EPA algorithm. Potentially allocate memory if // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's // current storage. this->epa.reset(this->epa_max_iterations, this->epa_tolerance); // TODO: understand why EPA's performance is so bad on cylinders and // cones. this->epa.evaluate(this->gjk, (-guess).cast()); switch (epa.status) { // // In the following switch cases, until the "Valid" case, // EPA either ran out of iterations, of faces or of vertices. // The depth, witness points and the normal are still valid, // simply not at the precision of EPA's tolerance. // The flag `COAL_ENABLE_LOGGING` enables feebdack on these // cases. // // TODO: Remove OutOfFaces and OutOfVertices statuses and simply // compute the upper bound on max faces and max vertices as a // function of the number of iterations. case details::EPA::OutOfFaces: COAL_LOG_WARNING("EPA ran out of faces."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::OutOfVertices: COAL_LOG_WARNING("EPA ran out of vertices."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::Failed: COAL_LOG_WARNING("EPA ran out of iterations."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::Valid: case details::EPA::AccuracyReached: COAL_ASSERT( -epa.depth <= epa.getTolerance() + this->m_dummy_precision, "EPA's penetration distance should be negative (or " "at least below EPA's tolerance).", std::logic_error); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::Degenerated: COAL_LOG_WARNING( "EPA warning: created a polytope with a degenerated face."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::NonConvex: COAL_LOG_WARNING( "EPA warning: EPA got called onto non-convex shapes."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::InvalidHull: COAL_LOG_WARNING("EPA warning: created an invalid polytope."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::DidNotRun: COAL_ASSERT(false, "EPA did not run. It should have!", std::logic_error); COAL_LOG_ERROR("EPA error: did not run. It should have."); EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::FallBack: COAL_ASSERT( false, "EPA went into fallback mode. It should never do that.", std::logic_error); COAL_LOG_ERROR("EPA error: FallBack."); EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; } } break; // End of case details::GJK::Collision } } void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3s& tf1, Scalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { COAL_UNUSED_VARIABLE(tf1); // Cache gjk result for potential future call to this GJKSolver. this->cached_guess = this->gjk.ray.cast(); this->support_func_cached_guess = this->gjk.support_hint; distance = Scalar(this->gjk.distance); // we set to max as a default value p1 = p2 = normal = Vec3s::Constant(std::numeric_limits::max()); // If we absolutely want to return some witness points, we could use // the following code (or simply merge the early stopped case with the // valid case below): // gjk.getWitnessPointsAndNormal(minkowski_difference, p1, p2, normal); // p1 = tf1.transform(p1); // p2 = tf1.transform(p2); // normal = tf1.getRotation() * normal; } void GJKExtractWitnessPointsAndNormal(const Transform3s& tf1, Scalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { // Apart from early stopping, there are two cases where GJK says there is no // collision: // 1. GJK proved the distance is above its tolerance (default 1e-6). // 2. GJK ran out of iterations. // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus // it can safely be normalized. COAL_ASSERT(this->gjk.ray.norm() > this->gjk.getTolerance() - this->m_dummy_precision, "The norm of GJK's ray should be bigger than GJK's tolerance.", std::logic_error); // Cache gjk result for potential future call to this GJKSolver. this->cached_guess = this->gjk.ray.cast(); this->support_func_cached_guess = this->gjk.support_hint; distance = Scalar(this->gjk.distance); // TODO: On degenerated case, the closest points may be non-unique. // (i.e. an object face normal is colinear to `gjk.ray`) Vec3ps p1_, p2_, normal_; gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1_, p2_, normal_); p1 = p1_.cast(); p2 = p2_.cast(); normal = normal_.cast(); Vec3s p = tf1.transform(0.5 * (p1 + p2)); normal = tf1.getRotation() * normal; p1.noalias() = p - 0.5 * distance * normal; p2.noalias() = p + 0.5 * distance * normal; } void GJKCollisionExtractWitnessPointsAndNormal(const Transform3s& tf1, Scalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { COAL_UNUSED_VARIABLE(tf1); COAL_ASSERT(this->gjk.distance <= this->gjk.getTolerance() + this->m_dummy_precision, "The distance should be lower than GJK's tolerance.", std::logic_error); // Because GJK has returned the `Collision` status and EPA has not run, // we purposefully do not cache the result of GJK, because ray is zero. // However, we can cache the support function hint. // this->cached_guess = this->gjk.ray; this->support_func_cached_guess = this->gjk.support_hint; distance = Scalar(this->gjk.distance); p1 = p2 = normal = Vec3s::Constant(std::numeric_limits::max()); } void EPAExtractWitnessPointsAndNormal(const Transform3s& tf1, Scalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { // Cache EPA result for potential future call to this GJKSolver. // This caching allows to warm-start the next GJK call. this->cached_guess = -(this->epa.depth * this->epa.normal).cast(); this->support_func_cached_guess = this->epa.support_hint; distance = (std::min)(Scalar(0), -Scalar(this->epa.depth)); Vec3ps p1_, p2_, normal_; this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1_, p2_, normal_); p1 = p1_.cast(); p2 = p2_.cast(); normal = normal_.cast(); // The following is very important to understand why EPA can sometimes // return a normal that is not colinear to the vector $p_1 - p_2$ when // working with tolerances like $\epsilon = 10^{-3}$. // It can be resumed with a simple idea: // EPA is an algorithm meant to find the penetration depth and the // normal. It is not meant to find the closest points. // Again, the issue here is **not** the normal, it's $p_1$ and $p_2$. // // More details: // We'll denote $S_1$ and $S_2$ the two shapes, $n$ the normal and $p_1$ and // $p_2$ the witness points. In theory, when EPA converges to $\epsilon = // 0$, the normal and witness points verify the following property (P): // - $p_1 \in \partial \sigma_{S_1}(n)$, // - $p_2 \in \partial \sigma_{S_2}(-n), // where $\sigma_{S_1}$ and $\sigma_{S_2}$ are the support functions of // $S_1$ and $S_2$. The $\partial \sigma(n)$ simply denotes the support set // of the support function in the direction $n$. (Note: I am leaving out the // details of frame choice for the support function, to avoid making the // mathematical notation too heavy.) // --> In practice, EPA converges to $\epsilon > 0$. // On polytopes and the likes, this does not change much and the property // given above is still valid. // --> However, this is very different on curved surfaces, such as // ellipsoids, cylinders, cones, capsules etc. For these shapes, converging // at $\epsilon = 10^{-6}$ or to $\epsilon = 10^{-3}$ does not change the // normal much, but the property (P) given above is no longer valid, which // means that the points $p_1$ and $p_2$ do not necessarily belong to the // support sets in the direction of $n$ and thus $n$ and $p_1 - p_2$ are not // colinear. // // Do not panic! This is fine. // Although the property above is not verified, it's almost verified, // meaning that $p_1$ and $p_2$ belong to support sets in directions that // are very close to $n$. // // Solution to compute better $p_1$ and $p_2$: // We compute the middle points of the current $p_1$ and $p_2$ and we use // the normal and the distance given by EPA to compute the new $p_1$ and // $p_2$. Vec3s p = tf1.transform(0.5 * (p1 + p2)); normal = tf1.getRotation() * normal; p1.noalias() = p - 0.5 * distance * normal; p2.noalias() = p + 0.5 * distance * normal; } void EPAFailedExtractWitnessPointsAndNormal(const Transform3s& tf1, Scalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { this->cached_guess = Vec3s(1, 0, 0); this->support_func_cached_guess.setZero(); COAL_UNUSED_VARIABLE(tf1); distance = -(std::numeric_limits::max)(); p1 = p2 = normal = Vec3s::Constant(std::numeric_limits::quiet_NaN()); } }; } // namespace coal #endif ================================================ FILE: include/coal/narrowphase/narrowphase_defaults.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /// This file defines different macros used to characterize the default behavior /// of the narrowphase algorithms GJK and EPA. #ifndef COAL_NARROWPHASE_DEFAULTS_H #define COAL_NARROWPHASE_DEFAULTS_H #include "coal/data_types.h" namespace coal { /// GJK constexpr size_t GJK_DEFAULT_MAX_ITERATIONS = 128; constexpr Scalar GJK_DEFAULT_TOLERANCE = Scalar(1e-6); /// Note: if the considered shapes are on the order of the meter, and the /// convergence criterion of GJK is the default VDB criterion, /// setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to /// the micro-meter. /// The same is true for EPA. constexpr Scalar GJK_MINIMUM_TOLERANCE = Scalar(1e-6); /// EPA /// EPA build a polytope which maximum size is: /// - `#iterations + 4` vertices /// - `2 x #iterations + 4` faces constexpr size_t EPA_DEFAULT_MAX_ITERATIONS = 64; constexpr Scalar EPA_DEFAULT_TOLERANCE = Scalar(1e-6); constexpr Scalar EPA_MINIMUM_TOLERANCE = Scalar(1e-6); } // namespace coal #endif // COAL_NARROWPHASE_DEFAULTS_H ================================================ FILE: include/coal/narrowphase/support_data.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ #ifndef COAL_SUPPORT_DATA_H #define COAL_SUPPORT_DATA_H #include "coal/data_types.h" #include "coal/collision_data.h" namespace coal { namespace details { /// @brief Options for the computation of support points. /// `NoSweptSphere` option is used when the support function is called /// by GJK or EPA. In this case, the swept sphere radius is not taken into /// account in the support function. It is used by GJK and EPA after they have /// converged to correct the solution. /// `WithSweptSphere` option is used when the support function is called /// directly by the user. In this case, the swept sphere radius is taken into /// account in the support function. enum SupportOptions { NoSweptSphere = 0, WithSweptSphere = 0x1, }; /// @brief Stores temporary data for the computation of support points. struct ShapeSupportData { // @brief Tracks which points have been visited in a ConvexBase. std::vector visited; // @brief Tracks the last support direction used on this shape; used to // warm-start the ConvexBase support function. Vec3s last_dir = Vec3s::Zero(); // @brief Temporary set used to compute the convex-hull of a support set. // Only used for ConvexBase and Box. std::vector polygon; }; } // namespace details } // namespace coal #endif // COAL_SUPPORT_DATA_H ================================================ FILE: include/coal/narrowphase/support_functions.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ #ifndef COAL_SUPPORT_FUNCTIONS_H #define COAL_SUPPORT_FUNCTIONS_H #include "coal/shape/geometric_shapes.h" #include "coal/math/transform.h" #include "coal/collision_data.h" #include "coal/narrowphase/support_data.h" namespace coal { namespace details { // ============================================================================ // ============================ SUPPORT FUNCTIONS ============================= // ============================================================================ /// @brief the support function for shape. /// The output support point is expressed in the local frame of the shape. /// @return a point which belongs to the set {argmax_{v in shape} v.dot(dir)}. /// @param shape the shape. /// @param dir support direction. /// @param hint used to initialize the search when shape is a ConvexBase object. /// @tparam _SupportOptions is a value of the SupportOptions enum. If set to /// `WithSweptSphere`, the support functions take into account the shapes' swept /// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const /// ShapeBase*)` for more details. template Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint); /// @brief Triangle support function. template void getShapeSupport(const TriangleP* triangle, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Box support function. template void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Sphere support function. template void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Ellipsoid support function. template void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Capsule support function. template void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Cone support function. template void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Cylinder support function. template void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief ConvexBase support function. /// @note See @ref LargeConvex and SmallConvex to see how to optimize /// ConvexBase's support computation. template void getShapeSupport(const ConvexBaseTpl* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& /*unused*/); /// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of /// `getShapeSupport`. This is **much** faster than the linear version of /// `getShapeSupport` when a `ConvexBase` has more than a few dozen of vertices. /// @note WARNING: when using a LargeConvex, the neighbors in `ConvexBase` must /// have been constructed! Otherwise the support function will segfault. template struct LargeConvex : ConvexBaseTpl {}; typedef LargeConvex LargeConvex16; typedef LargeConvex LargeConvex32; /// @brief See @ref LargeConvex. template struct SmallConvex : ConvexBaseTpl {}; typedef SmallConvex SmallConvex16; typedef SmallConvex SmallConvex32; /// @brief Support function for large ConvexBase (>32 vertices). template void getShapeSupport(const SmallConvex* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& data); /// @brief Support function for small ConvexBase (<32 vertices). template void getShapeSupport(const LargeConvex* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& support_data); // ============================================================================ // ========================== SUPPORT SET FUNCTIONS =========================== // ============================================================================ /// @brief Computes the support set for shape. /// This function assumes the frame of the support set has already been /// computed and that this frame is expressed w.r.t the local frame of the /// shape (i.e. the local frame of the shape is the WORLD frame of the support /// set). The support direction used to compute the support set is the positive /// z-axis if the support set has the DEFAULT direction; negative z-axis if it /// has the INVERTED direction. (In short, a shape's support set is has the /// DEFAULT direction if the shape is the first shape in a collision pair. It /// has the INVERTED direction if the shape is the second one in the collision /// pair). /// @return an approximation of the set {argmax_{v in shape} v.dot(dir)}, where /// dir is the support set's support direction. /// The support set is a plane passing by the origin of the support set frame /// and supported by the direction dir. As a consequence, any point added to the /// set is automatically projected onto this plane. /// @param[in] shape the shape. /// @param[in/out] support_set of shape. /// @param[in/out] hint used to initialize the search when shape is a ConvexBase /// object. /// @param[in] num_sampled_supports is only used for shapes with smooth /// non-strictly convex bases like cones and cylinders (their bases are /// circles). In such a case, if the support direction points to their base, we /// have to choose which points we want to add to the set. This is not needed /// for boxes or ConvexBase for example. Indeed, because their support sets are /// always polygons, we can characterize the entire support set with the /// vertices of the polygon. /// @param[in] tol given a point v on the shape, if /// `max_{p in shape}(p.dot(dir)) - v.dot(dir) <= tol`, where dir is the set's /// support direction, then v is added to the support set. /// Otherwise said, if a point p of the shape is at a distance `tol` from the /// support plane, it is added to the set. Thus, `tol` can be seen as the /// "thickness" of the support plane. /// @tparam _SupportOptions is a value of the SupportOptions enum. If set to /// `WithSweptSphere`, the support functions take into account the shapes' swept /// sphere radii. template void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, size_t num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief Same as @ref getSupportSet(const ShapeBase*, const Scalar, /// SupportSet&, const int) but also constructs the support set frame from /// `dir`. /// @note The support direction `dir` is expressed in the local frame of the /// shape. /// @note This function automatically deals with the `direction` of the /// SupportSet. template void getSupportSet(const ShapeBase* shape, const Vec3s& dir, SupportSet& support_set, int& hint, size_t num_sampled_supports = 6, Scalar tol = Scalar(1e-3)) { support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir); const Vec3s& support_dir = support_set.getNormal(); const Vec3s support = getSupport<_SupportOptions>(shape, support_dir, hint); getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports, tol); } /// @brief Triangle support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief Box support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const Box* box, SupportSet& support_set, int& /*unused*/, ShapeSupportData& support_data, size_t /*unused*/ num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief Sphere support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, Scalar /*unused*/ tol = Scalar(1e-3)); /// @brief Ellipsoid support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, Scalar /*unused*/ tol = Scalar(1e-3)); /// @brief Capsule support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief Cone support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const Cone* cone, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief Cylinder support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief ConvexBase support set function. /// Assumes the support set frame has already been computed. /// @note See @ref LargeConvex and SmallConvex to see how to optimize /// ConvexBase's support computation. template void getShapeSupportSet(const ConvexBaseTpl* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t /*unused*/ num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief Support set function for large ConvexBase (>32 vertices). /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief Support set function for small ConvexBase (<32 vertices). /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t /*unused*/ num_sampled_supports = 6, Scalar tol = Scalar(1e-3)); /// @brief Computes the convex-hull of support_set. For now, this function is /// only needed for Box and ConvexBase. /// @param[in] cloud data which contains the 2d points of the support set which /// convex-hull we want to compute. /// @param[out] 2d points of the the support set's convex-hull. COAL_DLLAPI void computeSupportSetConvexHull(const std::vector& cloud, std::vector& cvx_hull); } // namespace details } // namespace coal #endif // COAL_SUPPORT_FUNCTIONS_H ================================================ FILE: include/coal/octree.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2022-2024, Inria * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_OCTREE_H #define COAL_OCTREE_H #include #include #include #include "coal/fwd.hh" #include "coal/BV/AABB.h" #include "coal/collision_object.h" namespace coal { /// @brief Octree is one type of collision geometry which can encode uncertainty /// information in the sensor data. class COAL_DLLAPI OcTree : public CollisionGeometry { protected: shared_ptr tree; Scalar default_occupancy; Scalar occupancy_threshold; Scalar free_threshold; public: typedef octomap::OcTreeNode OcTreeNode; /// @brief construct octree with a given resolution explicit OcTree(Scalar resolution) : tree(shared_ptr( new octomap::OcTree(resolution))) { default_occupancy = Scalar(tree->getOccupancyThres()); // default occupancy/free threshold is consistent with default setting from // octomap occupancy_threshold = Scalar(tree->getOccupancyThres()); free_threshold = 0; } /// @brief construct octree from octomap explicit OcTree(const shared_ptr& tree_) : tree(tree_) { default_occupancy = Scalar(tree->getOccupancyThres()); // default occupancy/free threshold is consistent with default setting from // octomap occupancy_threshold = Scalar(tree->getOccupancyThres()); free_threshold = 0; } ///  \brief Copy constructor OcTree(const OcTree& other) : CollisionGeometry(other), tree(other.tree), default_occupancy(other.default_occupancy), occupancy_threshold(other.occupancy_threshold), free_threshold(other.free_threshold) {} /// \brief Clone *this into a new Octree OcTree* clone() const override { return new OcTree(*this); } /// \brief Returns the tree associated to the underlying octomap OcTree. shared_ptr getTree() const { return tree; } void exportAsObjFile(const std::string& filename) const; /// @brief compute the AABB for the octree in its local coordinate system void computeLocalAABB() override { typedef Eigen::Matrix Vec3float; Vec3float max_extent, min_extent; octomap::OcTree::iterator it = tree->begin((unsigned char)tree->getTreeDepth()); octomap::OcTree::iterator end = tree->end(); if (it == end) return; { max_extent.fill(std::numeric_limits::lowest()); min_extent.fill(std::numeric_limits::max()); for (; it != end; ++it) { const octomap::point3d& coord = it.getCoordinate(); const Vec3float pos = Eigen::Map(&coord.x()); // Account for the size of the box. const Vec3float max_pos = pos + Vec3float::Constant(float(it.getSize() / 2.)); const Vec3float min_pos = pos - Vec3float::Constant(float(it.getSize() / 2.)); max_extent.array() = max_extent.array().max(max_pos.array()); min_extent.array() = min_extent.array().min(min_pos.array()); } } aabb_local = AABB(min_extent.cast(), max_extent.cast()); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } /// @brief get the bounding volume for the root AABB getRootBV() const { Scalar delta = Scalar((1 << tree->getTreeDepth()) * tree->getResolution() / 2); // std::cout << "octree size " << delta << std::endl; return AABB(Vec3s(-delta, -delta, -delta), Vec3s(delta, delta, delta)); } /// @brief Returns the depth of the octree unsigned int getTreeDepth() const { return tree->getTreeDepth(); } /// @brief Returns the size of the octree unsigned long size() const { return tree->size(); } /// @brief Returns the resolution of the octree Scalar getResolution() const { return Scalar(tree->getResolution()); } /// @brief get the root node of the octree OcTreeNode* getRoot() const { return tree->getRoot(); } /// @brief whether one node is completely occupied bool isNodeOccupied(const OcTreeNode* node) const { // return tree->isNodeOccupied(node); return node->getOccupancy() >= occupancy_threshold; } /// @brief whether one node is completely free bool isNodeFree(const OcTreeNode* node) const { // return false; // default no definitely free node return node->getOccupancy() <= free_threshold; } /// @brief whether one node is uncertain bool isNodeUncertain(const OcTreeNode* node) const { return (!isNodeOccupied(node)) && (!isNodeFree(node)); } /// @brief transform the octree into a bunch of boxes; uncertainty information /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the /// boxes whose occupied probability is higher enough). std::vector toBoxes() const { std::vector boxes; boxes.reserve(tree->size() / 2); for (octomap::OcTree::iterator it = tree->begin((unsigned char)tree->getTreeDepth()), end = tree->end(); it != end; ++it) { // if(tree->isNodeOccupied(*it)) if (isNodeOccupied(&*it)) { Scalar x = Scalar(it.getX()); Scalar y = Scalar(it.getY()); Scalar z = Scalar(it.getZ()); Scalar size = Scalar(it.getSize()); Scalar c = Scalar((*it).getOccupancy()); Scalar t = Scalar(tree->getOccupancyThres()); Vec6s box; box << x, y, z, size, c, t; boxes.push_back(box); } } return boxes; } /// \brief Returns a byte description of *this std::vector tobytes() const { typedef Eigen::Matrix Vec3sloat; const size_t total_size = (tree->size() * sizeof(Scalar) * 3) / 2; std::vector bytes; bytes.reserve(total_size); for (octomap::OcTree::iterator it = tree->begin((unsigned char)tree->getTreeDepth()), end = tree->end(); it != end; ++it) { const Vec3s box_pos = Eigen::Map(&it.getCoordinate().x()).cast(); if (isNodeOccupied(&*it)) std::copy(box_pos.data(), box_pos.data() + sizeof(Scalar) * 3, std::back_inserter(bytes)); } return bytes; } /// @brief the threshold used to decide whether one node is occupied, this is /// NOT the octree occupied_thresold Scalar getOccupancyThres() const { return occupancy_threshold; } /// @brief the threshold used to decide whether one node is free, this is NOT /// the octree free_threshold Scalar getFreeThres() const { return free_threshold; } Scalar getDefaultOccupancy() const { return default_occupancy; } void setCellDefaultOccupancy(Scalar d) { default_occupancy = d; } void setOccupancyThres(Scalar d) { occupancy_threshold = d; } void setFreeThres(Scalar d) { free_threshold = d; } /// @return ptr to child number childIdx of node OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) { return tree->getNodeChild(node, childIdx); } /// @return const ptr to child number childIdx of node const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const { return tree->getNodeChild(node, childIdx); } /// @brief return true if the child at childIdx exists bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const { return tree->nodeChildExists(node, childIdx); } /// @brief return true if node has at least one child bool nodeHasChildren(const OcTreeNode* node) const { return tree->nodeHasChildren(node); } /// @brief return object type, it is an octree OBJECT_TYPE getObjectType() const override { return OT_OCTREE; } /// @brief return node type, it is an octree NODE_TYPE getNodeType() const override { return GEOM_OCTREE; } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const OcTree* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const OcTree& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(tree.get() == other.tree.get() || toBoxes() == other.toBoxes()); COAL_EQUAL_OPERATOR_CHECK(default_occupancy == other.default_occupancy); COAL_EQUAL_OPERATOR_CHECK(occupancy_threshold == other.occupancy_threshold); COAL_EQUAL_OPERATOR_CHECK(free_threshold == other.free_threshold); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief compute the bounding volume of an octree node's i-th child static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv) { const Scalar half = Scalar(0.5); if (i & 1) { child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * half; child_bv.max_[0] = root_bv.max_[0]; } else { child_bv.min_[0] = root_bv.min_[0]; child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * half; } if (i & 2) { child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * half; child_bv.max_[1] = root_bv.max_[1]; } else { child_bv.min_[1] = root_bv.min_[1]; child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * half; } if (i & 4) { child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * half; child_bv.max_[2] = root_bv.max_[2]; } else { child_bv.min_[2] = root_bv.min_[2]; child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * half; } } /// /// \brief Build an OcTree from a point cloud and a given resolution /// /// \param[in] point_cloud The input points to insert in the OcTree /// \param[in] resolution of the octree. /// /// \returns An OcTree that can be used for collision checking and more. /// COAL_DLLAPI OcTreePtr_t makeOctree(const Eigen::Matrix& point_cloud, const Scalar resolution); } // namespace coal #endif ================================================ FILE: include/coal/serialization/AABB.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_AABB_H #define COAL_SERIALIZATION_AABB_H #include "coal/BV/AABB.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::AABB& aabb, const unsigned int /*version*/) { ar& make_nvp("min_", aabb.min_); ar& make_nvp("max_", aabb.max_); } } // namespace serialization } // namespace boost #endif // ifndef COAL_SERIALIZATION_AABB_H ================================================ FILE: include/coal/serialization/BVH_model.h ================================================ // // Copyright (c) 2021-2022 INRIA // #ifndef COAL_SERIALIZATION_BVH_MODEL_H #define COAL_SERIALIZATION_BVH_MODEL_H #include "coal/BVH/BVH_model.h" #include "coal/serialization/fwd.h" #include "coal/serialization/BV_node.h" #include "coal/serialization/BV_splitter.h" #include "coal/serialization/collision_object.h" #include "coal/serialization/memory.h" #include "coal/serialization/triangle.h" namespace boost { namespace serialization { namespace internal { struct BVHModelBaseAccessor : coal::BVHModelBase { typedef coal::BVHModelBase Base; using Base::num_tris_allocated; using Base::num_vertices_allocated; }; } // namespace internal template void save(Archive& ar, const coal::BVHModelBase& bvh_model, const unsigned int /*version*/) { using namespace coal; if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED || bvh_model.build_state == BVH_BUILD_STATE_UPDATED) && (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) { COAL_THROW_PRETTY( "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or " "BVH_BUILD_STATE_UPDATED state.\n" "The BVHModel could not be serialized.", std::invalid_argument); } ar& make_nvp( "base", boost::serialization::base_object(bvh_model)); ar& make_nvp("num_vertices", bvh_model.num_vertices); ar& make_nvp("vertices", bvh_model.vertices); ar& make_nvp("num_tris", bvh_model.num_tris); ar& make_nvp("tri_indices", bvh_model.tri_indices); ar& make_nvp("build_state", bvh_model.build_state); ar& make_nvp("prev_vertices", bvh_model.prev_vertices); // if(bvh_model.convex) // { // const bool has_convex = true; // ar << make_nvp("has_convex",has_convex); // } // else // { // const bool has_convex = false; // ar << make_nvp("has_convex",has_convex); // } } template void load(Archive& ar, coal::BVHModelBase& bvh_model, const unsigned int /*version*/) { using namespace coal; ar >> make_nvp("base", boost::serialization::base_object( bvh_model)); ar >> make_nvp("num_vertices", bvh_model.num_vertices); ar >> make_nvp("vertices", bvh_model.vertices); ar >> make_nvp("num_tris", bvh_model.num_tris); ar >> make_nvp("tri_indices", bvh_model.tri_indices); ar >> make_nvp("build_state", bvh_model.build_state); ar >> make_nvp("prev_vertices", bvh_model.prev_vertices); // bool has_convex = true; // ar >> make_nvp("has_convex",has_convex); } COAL_SERIALIZATION_SPLIT(coal::BVHModelBase) namespace internal { template struct BVHModelAccessor : coal::BVHModel { typedef coal::BVHModel Base; using Base::bvs; using Base::num_bvs; using Base::num_bvs_allocated; using Base::primitive_indices; }; } // namespace internal template void serialize(Archive& ar, coal::BVHModel& bvh_model, const unsigned int version) { split_free(ar, bvh_model, version); } template void save(Archive& ar, const coal::BVHModel& bvh_model_, const unsigned int /*version*/) { using namespace coal; typedef internal::BVHModelAccessor Accessor; typedef BVNode Node; const Accessor& bvh_model = reinterpret_cast(bvh_model_); ar& make_nvp("base", boost::serialization::base_object(bvh_model)); // if(bvh_model.primitive_indices) // { // const bool with_primitive_indices = true; // ar & make_nvp("with_primitive_indices",with_primitive_indices); // // int num_primitives = 0; // switch(bvh_model.getModelType()) // { // case BVH_MODEL_TRIANGLES: // num_primitives = bvh_model.num_tris; // break; // case BVH_MODEL_POINTCLOUD: // num_primitives = bvh_model.num_vertices; // break; // default: // ; // } // // ar & make_nvp("num_primitives",num_primitives); // if(num_primitives > 0) // { // typedef Eigen::Matrix // AsPrimitiveIndexVector; const Eigen::Map // primitive_indices_map(reinterpret_cast(bvh_model.primitive_indices),1,num_primitives); ar & // make_nvp("primitive_indices",primitive_indices_map); //// ar & /// make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); // } // } // else // { // const bool with_primitive_indices = false; // ar & make_nvp("with_primitive_indices",with_primitive_indices); // } // if (bvh_model.bvs.get()) { const bool with_bvs = true; ar& make_nvp("with_bvs", with_bvs); ar& make_nvp("num_bvs", bvh_model.num_bvs); ar& make_nvp( "bvs", make_array( reinterpret_cast(bvh_model.bvs->data()), sizeof(Node) * (std::size_t)bvh_model.num_bvs)); // Assuming BVs are POD. } else { const bool with_bvs = false; ar& make_nvp("with_bvs", with_bvs); } } template void load(Archive& ar, coal::BVHModel& bvh_model_, const unsigned int /*version*/) { using namespace coal; typedef internal::BVHModelAccessor Accessor; typedef BVNode Node; Accessor& bvh_model = reinterpret_cast(bvh_model_); ar >> make_nvp("base", boost::serialization::base_object(bvh_model)); // bool with_primitive_indices; // ar >> make_nvp("with_primitive_indices",with_primitive_indices); // if(with_primitive_indices) // { // int num_primitives; // ar >> make_nvp("num_primitives",num_primitives); // // delete[] bvh_model.primitive_indices; // if(num_primitives > 0) // { // bvh_model.primitive_indices = new unsigned int[num_primitives]; // ar & // make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); // } // else // bvh_model.primitive_indices = NULL; // } bool with_bvs; ar >> make_nvp("with_bvs", with_bvs); if (with_bvs) { unsigned int num_bvs; ar >> make_nvp("num_bvs", num_bvs); if (num_bvs != bvh_model.num_bvs) { bvh_model.bvs.reset(); bvh_model.num_bvs = num_bvs; if (num_bvs > 0) bvh_model.bvs.reset(new typename BVHModel::bv_node_vector_t(num_bvs)); } if (num_bvs > 0) { ar >> make_nvp("bvs", make_array(reinterpret_cast(bvh_model.bvs->data()), sizeof(Node) * (std::size_t)num_bvs)); } else bvh_model.bvs.reset(); } } } // namespace serialization } // namespace boost namespace coal { namespace internal { template struct memory_footprint_evaluator<::coal::BVHModel> { static size_t run(const ::coal::BVHModel& bvh_model) { return static_cast(bvh_model.memUsage(false)); } }; } // namespace internal } // namespace coal COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::AABB>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBB>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::RSS>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBBRSS>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::kIOS>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<16>>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<18>>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<24>>) #endif // ifndef COAL_SERIALIZATION_BVH_MODEL_H ================================================ FILE: include/coal/serialization/BV_node.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_BV_NODE_H #define COAL_SERIALIZATION_BV_NODE_H #include "coal/BV/BV_node.h" #include "coal/serialization/fwd.h" #include "coal/serialization/OBBRSS.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::BVNodeBase& node, const unsigned int /*version*/) { ar& make_nvp("first_child", node.first_child); ar& make_nvp("first_primitive", node.first_primitive); ar& make_nvp("num_primitives", node.num_primitives); } template void serialize(Archive& ar, coal::BVNode& node, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(node)); ar& make_nvp("bv", node.bv); } } // namespace serialization } // namespace boost #endif // ifndef COAL_SERIALIZATION_BV_NODE_H ================================================ FILE: include/coal/serialization/BV_splitter.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_BV_SPLITTER_H #define COAL_SERIALIZATION_BV_SPLITTER_H #include "coal/internal/BV_splitter.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { namespace internal { template struct BVSplitterAccessor : coal::BVSplitter { typedef coal::BVSplitter Base; using Base::split_axis; using Base::split_method; using Base::split_value; using Base::split_vector; using Base::tri_indices; using Base::type; using Base::vertices; }; } // namespace internal template void save(Archive& ar, const coal::BVSplitter& splitter_, const unsigned int /*version*/) { using namespace coal; typedef internal::BVSplitterAccessor Accessor; const Accessor& splitter = reinterpret_cast(splitter_); ar& make_nvp("split_axis", splitter.split_axis); ar& make_nvp("split_vector", splitter.split_vector); ar& make_nvp("split_value", splitter.split_value); ar& make_nvp("type", splitter.type); ar& make_nvp("split_method", splitter.split_method); } template void load(Archive& ar, coal::BVSplitter& splitter_, const unsigned int /*version*/) { using namespace coal; typedef internal::BVSplitterAccessor Accessor; Accessor& splitter = reinterpret_cast(splitter_); ar >> make_nvp("split_axis", splitter.split_axis); ar >> make_nvp("split_vector", splitter.split_vector); ar >> make_nvp("split_value", splitter.split_value); ar >> make_nvp("type", splitter.type); ar >> make_nvp("split_method", splitter.split_method); splitter.vertices = NULL; splitter.tri_indices = NULL; } } // namespace serialization } // namespace boost #endif // ifndef COAL_SERIALIZATION_BV_SPLITTER_H ================================================ FILE: include/coal/serialization/OBB.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_OBB_H #define COAL_SERIALIZATION_OBB_H #include "coal/BV/OBB.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::OBB& bv, const unsigned int /*version*/) { ar& make_nvp("axes", bv.axes); ar& make_nvp("To", bv.To); ar& make_nvp("extent", bv.extent); } } // namespace serialization } // namespace boost #endif // ifndef COAL_SERIALIZATION_OBB_H ================================================ FILE: include/coal/serialization/OBBRSS.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_OBBRSS_H #define COAL_SERIALIZATION_OBBRSS_H #include "coal/BV/OBBRSS.h" #include "coal/serialization/fwd.h" #include "coal/serialization/OBB.h" #include "coal/serialization/RSS.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::OBBRSS& bv, const unsigned int /*version*/) { ar& make_nvp("obb", bv.obb); ar& make_nvp("rss", bv.rss); } } // namespace serialization } // namespace boost #endif // ifndef COAL_SERIALIZATION_OBBRSS_H ================================================ FILE: include/coal/serialization/RSS.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_RSS_H #define COAL_SERIALIZATION_RSS_H #include "coal/BV/RSS.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::RSS& bv, const unsigned int /*version*/) { ar& make_nvp("axes", bv.axes); ar& make_nvp("Tr", bv.Tr); ar& make_nvp("length", make_array(bv.length, 2)); ar& make_nvp("radius", bv.radius); } } // namespace serialization } // namespace boost #endif // ifndef COAL_SERIALIZATION_RSS_H ================================================ FILE: include/coal/serialization/archive.h ================================================ // // Copyright (c) 2017-2024 CNRS INRIA // This file was borrowed from the Pinocchio library: // https://github.com/stack-of-tasks/pinocchio // #ifndef COAL_SERIALIZATION_ARCHIVE_H #define COAL_SERIALIZATION_ARCHIVE_H #include "coal/fwd.hh" #include #include #include #include #include #include #include #include #include #include #include #if BOOST_VERSION / 100 % 1000 == 78 && __APPLE__ // See https://github.com/qcscine/utilities/issues/5#issuecomment-1246897049 for // further details #ifndef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC #define DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC #define BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC #endif #include #ifdef DEFINE_BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC #undef BOOST_ASIO_DISABLE_STD_ALIGNED_ALLOC #endif #else #include #endif #include #include #include // Handle NAN inside TXT or XML archives #include namespace coal { namespace serialization { /// /// \brief Loads an object from a TXT file. /// /// \tparam T Type of the object to deserialize. /// /// \param[out] object Object in which the loaded data are copied. /// \param[in] filename Name of the file containing the serialized data. /// template inline void loadFromText(T& object, const std::string& filename) { std::ifstream ifs(filename.c_str()); if (ifs) { std::locale const new_loc(ifs.getloc(), new boost::math::nonfinite_num_get); ifs.imbue(new_loc); boost::archive::text_iarchive ia(ifs, boost::archive::no_codecvt); ia >> object; } else { const std::string exception_message(filename + " does not seem to be a valid file."); throw std::invalid_argument(exception_message); } } /// /// \brief Saves an object inside a TXT file. /// /// \tparam T Type of the object to deserialize. /// /// \param[in] object Object in which the loaded data are copied. /// \param[in] filename Name of the file containing the serialized data. /// template inline void saveToText(const T& object, const std::string& filename) { std::ofstream ofs(filename.c_str()); if (ofs) { boost::archive::text_oarchive oa(ofs); oa & object; } else { const std::string exception_message(filename + " does not seem to be a valid file."); throw std::invalid_argument(exception_message); } } /// /// \brief Loads an object from a std::stringstream. /// /// \tparam T Type of the object to deserialize. /// /// \param[out] object Object in which the loaded data are copied. /// \param[in] is string stream constaining the serialized content of the /// object. /// template inline void loadFromStringStream(T& object, std::istringstream& is) { std::locale const new_loc(is.getloc(), new boost::math::nonfinite_num_get); is.imbue(new_loc); boost::archive::text_iarchive ia(is, boost::archive::no_codecvt); ia >> object; } /// /// \brief Saves an object inside a std::stringstream. /// /// \tparam T Type of the object to deserialize. /// /// \param[in] object Object in which the loaded data are copied. /// \param[out] ss String stream constaining the serialized content of the /// object. /// template inline void saveToStringStream(const T& object, std::stringstream& ss) { boost::archive::text_oarchive oa(ss); oa & object; } /// /// \brief Loads an object from a std::string /// /// \tparam T Type of the object to deserialize. /// /// \param[out] object Object in which the loaded data are copied. /// \param[in] str string constaining the serialized content of the object. /// template inline void loadFromString(T& object, const std::string& str) { std::istringstream is(str); loadFromStringStream(object, is); } /// /// \brief Saves an object inside a std::string /// /// \tparam T Type of the object to deserialize. /// /// \param[in] object Object in which the loaded data are copied. /// /// \returns a string constaining the serialized content of the object. /// template inline std::string saveToString(const T& object) { std::stringstream ss; saveToStringStream(object, ss); return ss.str(); } /// /// \brief Loads an object from a XML file. /// /// \tparam T Type of the object to deserialize. /// /// \param[out] object Object in which the loaded data are copied. /// \param[in] filename Name of the file containing the serialized data. /// \param[in] tag_name XML Tag for the given object. /// template inline void loadFromXML(T& object, const std::string& filename, const std::string& tag_name) { if (filename.empty()) { COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument); } std::ifstream ifs(filename.c_str()); if (ifs) { std::locale const new_loc(ifs.getloc(), new boost::math::nonfinite_num_get); ifs.imbue(new_loc); boost::archive::xml_iarchive ia(ifs, boost::archive::no_codecvt); ia >> boost::serialization::make_nvp(tag_name.c_str(), object); } else { const std::string exception_message(filename + " does not seem to be a valid file."); throw std::invalid_argument(exception_message); } } /// /// \brief Saves an object inside a XML file. /// /// \tparam T Type of the object to deserialize. /// /// \param[in] object Object in which the loaded data are copied. /// \param[in] filename Name of the file containing the serialized data. /// \param[in] tag_name XML Tag for the given object. /// template inline void saveToXML(const T& object, const std::string& filename, const std::string& tag_name) { if (filename.empty()) { COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument); } std::ofstream ofs(filename.c_str()); if (ofs) { boost::archive::xml_oarchive oa(ofs); oa& boost::serialization::make_nvp(tag_name.c_str(), object); } else { const std::string exception_message(filename + " does not seem to be a valid file."); throw std::invalid_argument(exception_message); } } /// /// \brief Loads an object from a binary file. /// /// \tparam T Type of the object to deserialize. /// /// \param[out] object Object in which the loaded data are copied. /// \param[in] filename Name of the file containing the serialized data. /// template inline void loadFromBinary(T& object, const std::string& filename) { std::ifstream ifs(filename.c_str(), std::ios::binary); if (ifs) { boost::archive::binary_iarchive ia(ifs); ia >> object; } else { const std::string exception_message(filename + " does not seem to be a valid file."); throw std::invalid_argument(exception_message); } } /// /// \brief Saves an object inside a binary file. /// /// \tparam T Type of the object to deserialize. /// /// \param[in] object Object in which the loaded data are copied. /// \param[in] filename Name of the file containing the serialized data. /// template void saveToBinary(const T& object, const std::string& filename) { std::ofstream ofs(filename.c_str(), std::ios::binary); if (ofs) { boost::archive::binary_oarchive oa(ofs); oa & object; } else { const std::string exception_message(filename + " does not seem to be a valid file."); throw std::invalid_argument(exception_message); } } /// /// \brief Loads an object from a binary buffer. /// /// \tparam T Type of the object to deserialize. /// /// \param[out] object Object in which the loaded data are copied. /// \param[in] buffer Input buffer containing the serialized data. /// template inline void loadFromBuffer(T& object, boost::asio::streambuf& buffer) { boost::archive::binary_iarchive ia(buffer); ia >> object; } /// /// \brief Saves an object to a binary buffer. /// /// \tparam T Type of the object to serialize. /// /// \param[in] object Object in which the loaded data are copied. /// \param[out] buffer Output buffer containing the serialized data. /// template void saveToBuffer(const T& object, boost::asio::streambuf& buffer) { boost::archive::binary_oarchive oa(buffer); oa & object; } } // namespace serialization } // namespace coal #endif // ifndef COAL_SERIALIZATION_ARCHIVE_H ================================================ FILE: include/coal/serialization/collision_data.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_COLLISION_DATA_H #define COAL_SERIALIZATION_COLLISION_DATA_H #include "coal/collision_data.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { template void save(Archive& ar, const coal::Contact& contact, const unsigned int /*version*/) { ar& make_nvp("b1", contact.b1); ar& make_nvp("b2", contact.b2); ar& make_nvp("normal", contact.normal); ar& make_nvp("nearest_points", contact.nearest_points); ar& make_nvp("pos", contact.pos); ar& make_nvp("penetration_depth", contact.penetration_depth); } template void load(Archive& ar, coal::Contact& contact, const unsigned int /*version*/) { ar >> make_nvp("b1", contact.b1); ar >> make_nvp("b2", contact.b2); ar >> make_nvp("normal", contact.normal); std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); contact.nearest_points[0] = nearest_points[0]; contact.nearest_points[1] = nearest_points[1]; ar >> make_nvp("pos", contact.pos); ar >> make_nvp("penetration_depth", contact.penetration_depth); contact.o1 = NULL; contact.o2 = NULL; } COAL_SERIALIZATION_SPLIT(coal::Contact) template void serialize(Archive& ar, coal::QueryRequest& query_request, const unsigned int /*version*/) { ar& make_nvp("gjk_initial_guess", query_request.gjk_initial_guess); // TODO: use gjk_initial_guess instead COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS ar& make_nvp("enable_cached_gjk_guess", query_request.enable_cached_gjk_guess); COAL_COMPILER_DIAGNOSTIC_POP ar& make_nvp("cached_gjk_guess", query_request.cached_gjk_guess); ar& make_nvp("cached_support_func_guess", query_request.cached_support_func_guess); ar& make_nvp("gjk_max_iterations", query_request.gjk_max_iterations); ar& make_nvp("gjk_tolerance", query_request.gjk_tolerance); ar& make_nvp("gjk_variant", query_request.gjk_variant); ar& make_nvp("gjk_convergence_criterion", query_request.gjk_convergence_criterion); ar& make_nvp("gjk_convergence_criterion_type", query_request.gjk_convergence_criterion_type); ar& make_nvp("epa_max_iterations", query_request.epa_max_iterations); ar& make_nvp("epa_tolerance", query_request.epa_tolerance); ar& make_nvp("collision_distance_threshold", query_request.collision_distance_threshold); ar& make_nvp("enable_timings", query_request.enable_timings); } template void serialize(Archive& ar, coal::QueryResult& query_result, const unsigned int /*version*/) { ar& make_nvp("cached_gjk_guess", query_result.cached_gjk_guess); ar& make_nvp("cached_support_func_guess", query_result.cached_support_func_guess); } template void serialize(Archive& ar, coal::CollisionRequest& collision_request, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object( collision_request)); ar& make_nvp("num_max_contacts", collision_request.num_max_contacts); ar& make_nvp("enable_contact", collision_request.enable_contact); COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS ar& make_nvp("enable_distance_lower_bound", collision_request.enable_distance_lower_bound); COAL_COMPILER_DIAGNOSTIC_POP ar& make_nvp("security_margin", collision_request.security_margin); ar& make_nvp("break_distance", collision_request.break_distance); ar& make_nvp("distance_upper_bound", collision_request.distance_upper_bound); } template void save(Archive& ar, const coal::CollisionResult& collision_result, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object( collision_result)); ar& make_nvp("contacts", collision_result.getContacts()); ar& make_nvp("distance_lower_bound", collision_result.distance_lower_bound); ar& make_nvp("nearest_points", collision_result.nearest_points); ar& make_nvp("normal", collision_result.normal); } template void load(Archive& ar, coal::CollisionResult& collision_result, const unsigned int /*version*/) { ar >> make_nvp("base", boost::serialization::base_object( collision_result)); std::vector contacts; ar >> make_nvp("contacts", contacts); collision_result.clear(); for (size_t k = 0; k < contacts.size(); ++k) collision_result.addContact(contacts[k]); ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound); std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); collision_result.nearest_points[0] = nearest_points[0]; collision_result.nearest_points[1] = nearest_points[1]; ar >> make_nvp("normal", collision_result.normal); } COAL_SERIALIZATION_SPLIT(coal::CollisionResult) template void serialize(Archive& ar, coal::DistanceRequest& distance_request, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object( distance_request)); COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points); COAL_COMPILER_DIAGNOSTIC_POP ar& make_nvp("enable_signed_distance", distance_request.enable_signed_distance); ar& make_nvp("rel_err", distance_request.rel_err); ar& make_nvp("abs_err", distance_request.abs_err); } template void save(Archive& ar, const coal::DistanceResult& distance_result, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object( distance_result)); ar& make_nvp("min_distance", distance_result.min_distance); ar& make_nvp("nearest_points", distance_result.nearest_points); ar& make_nvp("normal", distance_result.normal); ar& make_nvp("b1", distance_result.b1); ar& make_nvp("b2", distance_result.b2); } template void load(Archive& ar, coal::DistanceResult& distance_result, const unsigned int /*version*/) { ar >> make_nvp("base", boost::serialization::base_object( distance_result)); ar >> make_nvp("min_distance", distance_result.min_distance); std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); distance_result.nearest_points[0] = nearest_points[0]; distance_result.nearest_points[1] = nearest_points[1]; ar >> make_nvp("normal", distance_result.normal); ar >> make_nvp("b1", distance_result.b1); ar >> make_nvp("b2", distance_result.b2); distance_result.o1 = NULL; distance_result.o2 = NULL; } COAL_SERIALIZATION_SPLIT(coal::DistanceResult) } // namespace serialization } // namespace boost COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionRequest) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionResult) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceRequest) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceResult) #endif // ifndef COAL_SERIALIZATION_COLLISION_DATA_H ================================================ FILE: include/coal/serialization/collision_object.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H #define COAL_SERIALIZATION_COLLISION_OBJECT_H #include "coal/collision_object.h" #include "coal/serialization/fwd.h" #include "coal/serialization/AABB.h" namespace boost { namespace serialization { template void save(Archive& ar, const coal::CollisionGeometry& collision_geometry, const unsigned int /*version*/) { ar& make_nvp("aabb_center", collision_geometry.aabb_center); ar& make_nvp("aabb_radius", collision_geometry.aabb_radius); ar& make_nvp("aabb_local", collision_geometry.aabb_local); ar& make_nvp("cost_density", collision_geometry.cost_density); ar& make_nvp("threshold_occupied", collision_geometry.threshold_occupied); ar& make_nvp("threshold_free", collision_geometry.threshold_free); } template void load(Archive& ar, coal::CollisionGeometry& collision_geometry, const unsigned int /*version*/) { ar >> make_nvp("aabb_center", collision_geometry.aabb_center); ar >> make_nvp("aabb_radius", collision_geometry.aabb_radius); ar >> make_nvp("aabb_local", collision_geometry.aabb_local); ar >> make_nvp("cost_density", collision_geometry.cost_density); ar >> make_nvp("threshold_occupied", collision_geometry.threshold_occupied); ar >> make_nvp("threshold_free", collision_geometry.threshold_free); collision_geometry.user_data = NULL; // no way to recover this } COAL_SERIALIZATION_SPLIT(coal::CollisionGeometry) } // namespace serialization } // namespace boost namespace coal { // fwd declaration template class HeightField; template class ConvexTpl; struct OBB; struct OBBRSS; class AABB; class OcTree; class Box; class Sphere; class Ellipsoid; class Capsule; class Cone; class TriangleP; class Cylinder; class Halfspace; class Plane; namespace serialization { template <> struct register_type { template static void on(Archive& ar) { ar.template register_type(); ar.template register_type(); ar.template register_type(); ar.template register_type(); ar.template register_type(); ar.template register_type(); ar.template register_type(); ar.template register_type(); ar.template register_type(); ar.template register_type(); ar.template register_type>(); ar.template register_type>(); ar.template register_type>(); ar.template register_type>(); ; } }; } // namespace serialization } // namespace coal #endif // ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H ================================================ FILE: include/coal/serialization/contact_patch.h ================================================ // // Copyright (c) 2024 INRIA // #ifndef COAL_SERIALIZATION_CONTACT_PATCH_H #define COAL_SERIALIZATION_CONTACT_PATCH_H #include "coal/collision_data.h" #include "coal/serialization/fwd.h" #include "coal/serialization/transform.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::ContactPatch& contact_patch, const unsigned int /*version*/) { using namespace coal; typedef Eigen::Matrix PolygonPoints; size_t patch_size = contact_patch.size(); ar& make_nvp("patch_size", patch_size); if (patch_size > 0) { if (Archive::is_loading::value) { contact_patch.points().resize(patch_size); } Eigen::Map points_map( reinterpret_cast(contact_patch.points().data()), 2, static_cast(patch_size)); ar& make_nvp("points", points_map); } ar& make_nvp("penetration_depth", contact_patch.penetration_depth); ar& make_nvp("direction", contact_patch.direction); ar& make_nvp("tf", contact_patch.tf); } template void serialize(Archive& ar, coal::ContactPatchRequest& request, const unsigned int /*version*/) { using namespace coal; ar& make_nvp("max_num_patch", request.max_num_patch); size_t num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); Scalar patch_tolerance = request.getPatchTolerance(); ar& make_nvp("num_samples_curved_shapes", num_samples_curved_shapes); ar& make_nvp("patch_tolerance", num_samples_curved_shapes); if (Archive::is_loading::value) { request.setNumSamplesCurvedShapes(num_samples_curved_shapes); request.setPatchTolerance(patch_tolerance); } } template void serialize(Archive& ar, coal::ContactPatchResult& result, const unsigned int /*version*/) { using namespace coal; size_t num_patches = result.numContactPatches(); ar& make_nvp("num_patches", num_patches); std::vector patches; patches.resize(num_patches); if (Archive::is_loading::value) { ar& make_nvp("patches", patches); const ContactPatchRequest request(num_patches); result.set(request); for (size_t i = 0; i < num_patches; ++i) { ContactPatch& patch = result.getUnusedContactPatch(); patch = patches[i]; } } else { patches.clear(); for (size_t i = 0; i < num_patches; ++i) { patches.emplace_back(result.getContactPatch(i)); } ar& make_nvp("patches", patches); } } } // namespace serialization } // namespace boost #endif // COAL_SERIALIZATION_CONTACT_PATCH_H ================================================ FILE: include/coal/serialization/convex.h ================================================ // // Copyright (c) 2022-2024 INRIA // #ifndef COAL_SERIALIZATION_CONVEX_H #define COAL_SERIALIZATION_CONVEX_H #include "coal/shape/convex.h" #include "coal/serialization/fwd.h" #include "coal/serialization/geometric_shapes.h" #include "coal/serialization/memory.h" #include "coal/serialization/triangle.h" #include "coal/serialization/quadrilateral.h" namespace boost { namespace serialization { namespace internal { template struct ConvexBaseAccessor : coal::ConvexBaseTpl { typedef coal::ConvexBaseTpl Base; }; } // namespace internal template void serialize(Archive& ar, coal::ConvexBaseTpl& convex_base, const unsigned int /*version*/) { using namespace coal; ar& make_nvp("base", boost::serialization::base_object(convex_base)); const unsigned int num_points_previous = convex_base.num_points; ar& make_nvp("num_points", convex_base.num_points); const unsigned int num_normals_and_offsets_previous = convex_base.num_normals_and_offsets; ar& make_nvp("num_normals_and_offsets", convex_base.num_normals_and_offsets); const int num_warm_start_supports_previous = static_cast(convex_base.support_warm_starts.points.size()); assert(num_warm_start_supports_previous == static_cast(convex_base.support_warm_starts.indices.size())); int num_warm_start_supports = num_warm_start_supports_previous; ar& make_nvp("num_warm_start_supports", num_warm_start_supports); if (Archive::is_loading::value) { if (num_points_previous != convex_base.num_points) { convex_base.points.reset(); if (convex_base.num_points > 0) convex_base.points.reset( new std::vector(convex_base.num_points)); } if (num_normals_and_offsets_previous != convex_base.num_normals_and_offsets) { convex_base.normals.reset(); convex_base.offsets.reset(); if (convex_base.num_normals_and_offsets > 0) { convex_base.normals.reset( new std::vector(convex_base.num_normals_and_offsets)); convex_base.offsets.reset( new std::vector(convex_base.num_normals_and_offsets)); } } if (num_warm_start_supports_previous != num_warm_start_supports) { convex_base.support_warm_starts.points.resize( static_cast(num_warm_start_supports)); convex_base.support_warm_starts.indices.resize( static_cast(num_warm_start_supports)); } } typedef Eigen::Matrix MatrixPoints; if (convex_base.num_points > 0) { Eigen::Map points_map( reinterpret_cast(convex_base.points->data()), 3, convex_base.num_points); ar& make_nvp("points", points_map); } typedef Eigen::Matrix VecOfReals; if (convex_base.num_normals_and_offsets > 0) { Eigen::Map normals_map( reinterpret_cast(convex_base.normals->data()), 3, convex_base.num_normals_and_offsets); ar& make_nvp("normals", normals_map); Eigen::Map offsets_map( reinterpret_cast(convex_base.offsets->data()), 1, convex_base.num_normals_and_offsets); ar& make_nvp("offsets", offsets_map); } typedef Eigen::Matrix VecOfInts; if (num_warm_start_supports > 0) { Eigen::Map warm_start_support_points_map( reinterpret_cast( convex_base.support_warm_starts.points.data()), 3, num_warm_start_supports); ar& make_nvp("warm_start_support_points", warm_start_support_points_map); Eigen::Map warm_start_support_indices_map( reinterpret_cast(convex_base.support_warm_starts.indices.data()), 1, num_warm_start_supports); ar& make_nvp("warm_start_support_indices", warm_start_support_indices_map); } ar& make_nvp("center", convex_base.center); // We don't save neighbors as they will be computed directly by calling // fillNeighbors. } namespace internal { template struct ConvexAccessor : coal::ConvexTpl { typedef coal::ConvexTpl Base; using Base::fillNeighbors; }; } // namespace internal template void serialize(Archive& ar, coal::ConvexTpl& convex_, const unsigned int /*version*/) { using namespace coal; typedef internal::ConvexAccessor Accessor; typedef ConvexBaseTpl Base; Accessor& convex = reinterpret_cast(convex_); ar& make_nvp("base", boost::serialization::base_object(convex_)); const unsigned int num_polygons_previous = convex.num_polygons; ar& make_nvp("num_polygons", convex.num_polygons); if (Archive::is_loading::value) { if (num_polygons_previous != convex.num_polygons) { convex.polygons.reset(new std::vector(convex.num_polygons)); } } ar& make_array(convex.polygons->data(), convex.num_polygons); if (Archive::is_loading::value) convex.fillNeighbors(); } } // namespace serialization } // namespace boost COAL_SERIALIZATION_DECLARE_EXPORT(coal::ConvexTpl) COAL_SERIALIZATION_DECLARE_EXPORT(coal::ConvexTpl) COAL_SERIALIZATION_DECLARE_EXPORT(coal::ConvexTpl) COAL_SERIALIZATION_DECLARE_EXPORT(coal::ConvexTpl) namespace coal { // namespace internal { // template // struct memory_footprint_evaluator< ::coal::BVHModel > { // static size_t run(const ::coal::BVHModel &bvh_model) { // return static_cast(bvh_model.memUsage(false)); // } // }; // } // namespace internal } // namespace coal #endif // ifndef COAL_SERIALIZATION_CONVEX_H ================================================ FILE: include/coal/serialization/eigen.h ================================================ // // Copyright (c) 2017-2021 CNRS INRIA // /* Code adapted from Pinocchio and https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp Copyright (c) 2015 Michael Tao */ #ifndef COAL_SERIALIZATION_EIGEN_H #define COAL_SERIALIZATION_EIGEN_H #ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL #ifdef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION #define COAL_SKIP_EIGEN_BOOST_SERIALIZATION #endif #endif #ifndef COAL_SKIP_EIGEN_BOOST_SERIALIZATION #include #include #include #include // Workaround a bug in GCC >= 7 and C++17 // ref. https://gitlab.com/libeigen/eigen/-/issues/1676 #ifdef __GNUC__ #if __GNUC__ >= 7 && __cplusplus >= 201703L namespace boost { namespace serialization { struct U; } } // namespace boost namespace Eigen { namespace internal { template <> struct traits { enum { Flags = 0 }; }; } // namespace internal } // namespace Eigen #endif #endif // There is the same kind of bug with CL when building // nanobind bindings. // This doesn't happen when building Boost.Python bindings. // We only activate the workaround when C++17 is enable since // it's mandatory to build nanobind. #if defined(_MSVC_LANG) && _MSVC_LANG >= 201703L namespace boost { namespace archive { class xml_iarchive; class text_iarchive; class binary_iarchive; } // namespace archive } // namespace boost namespace Eigen { namespace internal { template <> struct traits { enum { Flags = 0 }; }; template <> struct traits { enum { Flags = 0 }; }; template <> struct traits { enum { Flags = 0 }; }; } // namespace internal } // namespace Eigen #endif namespace boost { namespace serialization { template void save(Archive& ar, const Eigen::Matrix& m, const unsigned int /*version*/) { Eigen::DenseIndex rows(m.rows()), cols(m.cols()); if (Rows == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows); if (Cols == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(cols); ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); } template void load(Archive& ar, Eigen::Matrix& m, const unsigned int /*version*/) { Eigen::DenseIndex rows = Rows, cols = Cols; if (Rows == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows); if (Cols == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(cols); m.resize(rows, cols); ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); } template void serialize(Archive& ar, Eigen::Matrix& m, const unsigned int version) { split_free(ar, m, version); } template void save(Archive& ar, const Eigen::Map& m, const unsigned int /*version*/) { Eigen::DenseIndex rows(m.rows()), cols(m.cols()); if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows); if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(cols); ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); } template void load(Archive& ar, Eigen::Map& m, const unsigned int /*version*/) { Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime, cols = PlainObjectBase::ColsAtCompileTime; if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows); if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(cols); m.resize(rows, cols); ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); } template void serialize(Archive& ar, Eigen::Map& m, const unsigned int version) { split_free(ar, m, version); } } // namespace serialization } // namespace boost // #endif // ifned COAL_SKIP_EIGEN_BOOST_SERIALIZATION #endif // ifndef COAL_SERIALIZATION_EIGEN_H ================================================ FILE: include/coal/serialization/fwd.h ================================================ // // Copyright (c) 2021-2024 INRIA // #ifndef COAL_SERIALIZATION_FWD_H #define COAL_SERIALIZATION_FWD_H #include #include #include #include #include #include #include #include #include #include #include "coal/fwd.hh" #include "coal/serialization/eigen.h" #define COAL_SERIALIZATION_SPLIT(Type) \ template \ void serialize(Archive& ar, Type& value, const unsigned int version) { \ split_free(ar, value, version); \ } #define COAL_SERIALIZATION_DECLARE_EXPORT(T) \ BOOST_CLASS_EXPORT_KEY(T) \ namespace boost { \ namespace archive { \ namespace detail { \ namespace extra_detail { \ template <> \ struct init_guid { \ static guid_initializer const& g; \ }; \ } \ } \ } \ } \ /**/ #define COAL_SERIALIZATION_DEFINE_EXPORT(T) \ namespace boost { \ namespace archive { \ namespace detail { \ namespace extra_detail { \ guid_initializer const& init_guid::g = \ ::boost::serialization::singleton< \ guid_initializer >::get_mutable_instance() \ .export_guid(); \ } \ } \ } \ } \ /**/ namespace coal { namespace serialization { namespace detail { template struct init_cast_register {}; template struct cast_register_initializer { void init(std::true_type) const { boost::serialization::void_cast_register(); } void init(std::false_type) const {} cast_register_initializer const& init() const { #ifndef _WIN32 COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic ignored \"-Wconversion\"") #endif BOOST_STATIC_WARNING((std::is_base_of::value)); #ifndef _WIN32 COAL_COMPILER_DIAGNOSTIC_POP #endif init(std::is_base_of()); return *this; } }; } // namespace detail template struct register_type { template static void on(Archive& /*ar*/) {} }; } // namespace serialization } // namespace coal #define COAL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ namespace coal { \ namespace serialization { \ namespace detail { \ template <> \ struct init_cast_register { \ static cast_register_initializer const& g; \ }; \ cast_register_initializer const& init_cast_register< \ Derived, Base>::g = \ ::boost::serialization::singleton< \ cast_register_initializer >::get_mutable_instance() \ .init(); \ } \ } \ } #endif // ifndef COAL_SERIALIZATION_FWD_H ================================================ FILE: include/coal/serialization/geometric_shapes.h ================================================ // // Copyright (c) 2021-2024 INRIA // #ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H #define COAL_SERIALIZATION_GEOMETRIC_SHAPES_H #include "coal/shape/geometric_shapes.h" #include "coal/serialization/fwd.h" #include "coal/serialization/collision_object.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::ShapeBase& shape_base, const unsigned int /*version*/) { ar& make_nvp( "base", boost::serialization::base_object(shape_base)); ::coal::Scalar radius = shape_base.getSweptSphereRadius(); ar& make_nvp("swept_sphere_radius", radius); if (Archive::is_loading::value) { shape_base.setSweptSphereRadius(radius); } } template void serialize(Archive& ar, coal::TriangleP& triangle, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(triangle)); ar& make_nvp("a", triangle.a); ar& make_nvp("b", triangle.b); ar& make_nvp("c", triangle.c); } template void serialize(Archive& ar, coal::Box& box, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(box)); ar& make_nvp("halfSide", box.halfSide); } template void serialize(Archive& ar, coal::Sphere& sphere, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(sphere)); ar& make_nvp("radius", sphere.radius); } template void serialize(Archive& ar, coal::Ellipsoid& ellipsoid, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(ellipsoid)); ar& make_nvp("radii", ellipsoid.radii); } template void serialize(Archive& ar, coal::Capsule& capsule, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(capsule)); ar& make_nvp("radius", capsule.radius); ar& make_nvp("halfLength", capsule.halfLength); } template void serialize(Archive& ar, coal::Cone& cone, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(cone)); ar& make_nvp("radius", cone.radius); ar& make_nvp("halfLength", cone.halfLength); } template void serialize(Archive& ar, coal::Cylinder& cylinder, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(cylinder)); ar& make_nvp("radius", cylinder.radius); ar& make_nvp("halfLength", cylinder.halfLength); } template void serialize(Archive& ar, coal::Halfspace& half_space, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(half_space)); ar& make_nvp("n", half_space.n); ar& make_nvp("d", half_space.d); } template void serialize(Archive& ar, coal::Plane& plane, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(plane)); ar& make_nvp("n", plane.n); ar& make_nvp("d", plane.d); } } // namespace serialization } // namespace boost COAL_SERIALIZATION_DECLARE_EXPORT(::coal::ShapeBase) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionGeometry) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::TriangleP) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Box) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Sphere) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Ellipsoid) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Capsule) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cone) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cylinder) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Halfspace) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Plane) #endif // ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H ================================================ FILE: include/coal/serialization/hfield.h ================================================ // // Copyright (c) 2021-2024 INRIA // #ifndef COAL_SERIALIZATION_HFIELD_H #define COAL_SERIALIZATION_HFIELD_H #include "coal/hfield.h" #include "coal/serialization/fwd.h" #include "coal/serialization/OBBRSS.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::HFNodeBase& node, const unsigned int /*version*/) { ar& make_nvp("first_child", node.first_child); ar& make_nvp("x_id", node.x_id); ar& make_nvp("x_size", node.x_size); ar& make_nvp("y_id", node.y_id); ar& make_nvp("y_size", node.y_size); ar& make_nvp("max_height", node.max_height); ar& make_nvp("contact_active_faces", node.contact_active_faces); } template void serialize(Archive& ar, coal::HFNode& node, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object(node)); ar& make_nvp("bv", node.bv); } namespace internal { template struct HeightFieldAccessor : coal::HeightField { typedef coal::HeightField Base; using Base::bvs; using Base::heights; using Base::max_height; using Base::min_height; using Base::num_bvs; using Base::x_dim; using Base::x_grid; using Base::y_dim; using Base::y_grid; }; } // namespace internal template void serialize(Archive& ar, coal::HeightField& hf_model, const unsigned int /*version*/) { ar& make_nvp( "base", boost::serialization::base_object(hf_model)); typedef internal::HeightFieldAccessor Accessor; Accessor& access = reinterpret_cast(hf_model); ar& make_nvp("x_dim", access.x_dim); ar& make_nvp("y_dim", access.y_dim); ar& make_nvp("heights", access.heights); ar& make_nvp("min_height", access.min_height); ar& make_nvp("max_height", access.max_height); ar& make_nvp("x_grid", access.x_grid); ar& make_nvp("y_grid", access.y_grid); ar& make_nvp("bvs", access.bvs); ar& make_nvp("num_bvs", access.num_bvs); } } // namespace serialization } // namespace boost COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::AABB>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBB>) COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBBRSS>) #endif // ifndef COAL_SERIALIZATION_HFIELD_H ================================================ FILE: include/coal/serialization/kDOP.h ================================================ // // Copyright (c) 2024 INRIA // #ifndef COAL_SERIALIZATION_kDOP_H #define COAL_SERIALIZATION_kDOP_H #include "coal/BV/kDOP.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { namespace internal { template struct KDOPAccessor : coal::KDOP { typedef coal::KDOP Base; using Base::dist_; }; } // namespace internal template void serialize(Archive& ar, coal::KDOP& bv_, const unsigned int /*version*/) { typedef internal::KDOPAccessor Accessor; Accessor& access = reinterpret_cast(bv_); ar& make_nvp("distances", make_array(access.dist_.data(), N)); } } // namespace serialization } // namespace boost #endif // COAL_SERIALIZATION_kDOP_H ================================================ FILE: include/coal/serialization/kIOS.h ================================================ // // Copyright (c) 2024 INRIA // #ifndef COAL_SERIALIZATION_kIOS_H #define COAL_SERIALIZATION_kIOS_H #include "coal/BV/kIOS.h" #include "coal/serialization/OBB.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::kIOS& bv, const unsigned int version) { split_free(ar, bv, version); } template void save(Archive& ar, const coal::kIOS& bv, const unsigned int /*version*/) { // Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres ar& make_nvp("num_spheres", bv.num_spheres); std::array centers{}; std::array radii; for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { centers[i] = bv.spheres[i].o; radii[i] = bv.spheres[i].r; } ar& make_nvp("centers", make_array(centers.data(), centers.size())); ar& make_nvp("radii", make_array(radii.data(), radii.size())); ar& make_nvp("obb", bv.obb); } template void load(Archive& ar, coal::kIOS& bv, const unsigned int /*version*/) { ar >> make_nvp("num_spheres", bv.num_spheres); std::array centers; std::array radii; ar >> make_nvp("centers", make_array(centers.data(), centers.size())); ar >> make_nvp("radii", make_array(radii.data(), radii.size())); for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { bv.spheres[i].o = centers[i]; bv.spheres[i].r = radii[i]; } ar >> make_nvp("obb", bv.obb); } } // namespace serialization } // namespace boost #endif // COAL_SERIALIZATION_kIOS_H ================================================ FILE: include/coal/serialization/memory.h ================================================ // // Copyright (c) 2021 INRIA // #ifndef COAL_SERIALIZATION_MEMORY_H #define COAL_SERIALIZATION_MEMORY_H namespace coal { namespace internal { template struct memory_footprint_evaluator { static size_t run(const T&) { return sizeof(T); } }; } // namespace internal /// \brief Returns the memory footpring of the input object. /// For POD objects, this function returns the result of sizeof(T) /// /// \param[in] object whose memory footprint needs to be evaluated. /// /// \return the memory footprint of the input object. template size_t computeMemoryFootprint(const T& object) { return internal::memory_footprint_evaluator::run(object); } } // namespace coal #endif // ifndef COAL_SERIALIZATION_MEMORY_H ================================================ FILE: include/coal/serialization/octree.h ================================================ // // Copyright (c) 2023-2024 INRIA // #ifndef COAL_SERIALIZATION_OCTREE_H #define COAL_SERIALIZATION_OCTREE_H #include #include #include #include "coal/octree.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { namespace internal { struct OcTreeAccessor : coal::OcTree { typedef coal::OcTree Base; using Base::default_occupancy; using Base::free_threshold; using Base::occupancy_threshold; using Base::tree; }; } // namespace internal template void save_construct_data(Archive& ar, const coal::OcTree* octree_ptr, const unsigned int /*version*/) { const coal::Scalar resolution = octree_ptr->getResolution(); ar << make_nvp("resolution", resolution); } template void save(Archive& ar, const coal::OcTree& octree, const unsigned int /*version*/) { typedef internal::OcTreeAccessor Accessor; const Accessor& access = reinterpret_cast(octree); std::ostringstream stream; access.tree->write(stream); const std::string stream_str = stream.str(); auto size = stream_str.size(); // We can't directly serialize stream_str because it contains binary data. // This create a bug on Windows with text_archive ar << make_nvp("tree_data_size", size); ar << make_nvp("tree_data", make_array(stream_str.c_str(), stream_str.size())); ar << make_nvp("base", base_object(octree)); ar << make_nvp("default_occupancy", access.default_occupancy); ar << make_nvp("occupancy_threshold", access.occupancy_threshold); ar << make_nvp("free_threshold", access.free_threshold); } template void load_construct_data(Archive& ar, coal::OcTree* octree_ptr, const unsigned int /*version*/) { coal::Scalar resolution; ar >> make_nvp("resolution", resolution); new (octree_ptr) coal::OcTree(resolution); } template void load(Archive& ar, coal::OcTree& octree, const unsigned int /*version*/) { typedef internal::OcTreeAccessor Accessor; Accessor& access = reinterpret_cast(octree); std::size_t tree_data_size; ar >> make_nvp("tree_data_size", tree_data_size); std::string stream_str; stream_str.resize(tree_data_size); /// TODO use stream_str.data in C++17 assert(tree_data_size > 0 && "tree_data_size should be greater than 0"); ar >> make_nvp("tree_data", make_array(&stream_str[0], tree_data_size)); std::istringstream stream(stream_str); octomap::AbstractOcTree* new_tree = octomap::AbstractOcTree::read(stream); access.tree = std::shared_ptr( dynamic_cast(new_tree)); ar >> make_nvp("base", base_object(octree)); ar >> make_nvp("default_occupancy", access.default_occupancy); ar >> make_nvp("occupancy_threshold", access.occupancy_threshold); ar >> make_nvp("free_threshold", access.free_threshold); } template void serialize(Archive& ar, coal::OcTree& octree, const unsigned int version) { split_free(ar, octree, version); } } // namespace serialization } // namespace boost COAL_SERIALIZATION_DECLARE_EXPORT(::coal::OcTree) #endif // ifndef COAL_SERIALIZATION_OCTREE_H ================================================ FILE: include/coal/serialization/quadrilateral.h ================================================ // // Copyright (c) 2022 INRIA // #ifndef COAL_SERIALIZATION_QUADRILATERAL_H #define COAL_SERIALIZATION_QUADRILATERAL_H #include "coal/data_types.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::QuadrilateralTpl& quadrilateral, const unsigned int /*version*/) { ar& make_nvp("p0", quadrilateral[0]); ar& make_nvp("p1", quadrilateral[1]); ar& make_nvp("p2", quadrilateral[2]); ar& make_nvp("p3", quadrilateral[3]); } } // namespace serialization } // namespace boost #endif // ifndef COAL_SERIALIZATION_QUADRILATERAL_H ================================================ FILE: include/coal/serialization/serializer.h ================================================ // // Copyright (c) 2024 INRIA // #ifndef COAL_SERIALIZATION_SERIALIZER_H #define COAL_SERIALIZATION_SERIALIZER_H #include "coal/serialization/archive.h" namespace coal { namespace serialization { struct Serializer { /// \brief Loads an object from a text file. template static void loadFromText(T& object, const std::string& filename) { ::coal::serialization::loadFromText(object, filename); } /// \brief Saves an object as a text file. template static void saveToText(const T& object, const std::string& filename) { ::coal::serialization::saveToText(object, filename); } /// \brief Loads an object from a stream string. template static void loadFromStringStream(T& object, std::istringstream& is) { ::coal::serialization::loadFromStringStream(object, is); } /// \brief Saves an object to a string stream. template static void saveToStringStream(const T& object, std::stringstream& ss) { ::coal::serialization::saveToStringStream(object, ss); } /// \brief Loads an object from a string. template static void loadFromString(T& object, const std::string& str) { ::coal::serialization::loadFromString(object, str); } /// \brief Saves a Derived object to a string. template static std::string saveToString(const T& object) { return ::coal::serialization::saveToString(object); } /// \brief Loads an object from an XML file. template static void loadFromXML(T& object, const std::string& filename, const std::string& tag_name) { ::coal::serialization::loadFromXML(object, filename, tag_name); } /// \brief Saves an object as an XML file. template static void saveToXML(const T& object, const std::string& filename, const std::string& tag_name) { ::coal::serialization::saveToXML(object, filename, tag_name); } /// \brief Loads a Derived object from an binary file. template static void loadFromBinary(T& object, const std::string& filename) { ::coal::serialization::loadFromBinary(object, filename); } /// \brief Saves a Derived object as an binary file. template static void saveToBinary(const T& object, const std::string& filename) { ::coal::serialization::saveToBinary(object, filename); } /// \brief Loads an object from a binary buffer. template static void loadFromBuffer(T& object, boost::asio::streambuf& container) { ::coal::serialization::loadFromBuffer(object, container); } /// \brief Saves an object as a binary buffer. template static void saveToBuffer(const T& object, boost::asio::streambuf& container) { ::coal::serialization::saveToBuffer(object, container); } }; } // namespace serialization } // namespace coal #endif // ifndef COAL_SERIALIZATION_SERIALIZER_H ================================================ FILE: include/coal/serialization/transform.h ================================================ // // Copyright (c) 2024 INRIA // #ifndef COAL_SERIALIZATION_TRANSFORM_H #define COAL_SERIALIZATION_TRANSFORM_H #include "coal/math/transform.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::Transform3s& tf, const unsigned int /*version*/) { ar& make_nvp("R", tf.rotation()); ar& make_nvp("T", tf.translation()); } } // namespace serialization } // namespace boost #endif // COAL_SERIALIZATION_TRANSFORM_H ================================================ FILE: include/coal/serialization/triangle.h ================================================ // // Copyright (c) 2021-2022 INRIA // #ifndef COAL_SERIALIZATION_TRIANGLE_H #define COAL_SERIALIZATION_TRIANGLE_H #include "coal/data_types.h" #include "coal/serialization/fwd.h" namespace boost { namespace serialization { template void serialize(Archive& ar, coal::TriangleTpl& triangle, const unsigned int /*version*/) { ar& make_nvp("p0", triangle[0]); ar& make_nvp("p1", triangle[1]); ar& make_nvp("p2", triangle[2]); } } // namespace serialization } // namespace boost #endif // ifndef COAL_SERIALIZATION_TRIANGLE_H ================================================ FILE: include/coal/shape/convex.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_SHAPE_CONVEX_H #define COAL_SHAPE_CONVEX_H #include "coal/shape/geometric_shapes.h" #include namespace coal { /// @brief Convex polytope /// @tparam PolygonT the polygon class. It must have method \c size() and /// \c operator[](int i) template class ConvexTpl : public ConvexBaseTpl { public: typedef typename PolygonT::IndexType IndexType; typedef ConvexBaseTpl Base; typedef typename Base::Neighbors Neighbors; using Base::neighbors; using Base::num_points; using Base::points; /// @brief Construct an uninitialized convex object ConvexTpl() : Base(), num_polygons(0) {} ~ConvexTpl() {} /// @brief Constructing a convex, providing normal and offset of each polytype /// surface, and the points and shape topology information /// \param points_ list of 3D points /// \param num_points_ number of 3D points /// \param polygons_ \copydoc Convex::polygons /// \param num_polygons_ the number of polygons. /// \note num_polygons_ is not the allocated size of polygons_. ConvexTpl(std::shared_ptr> points_, unsigned int num_points_, std::shared_ptr> polygons_, unsigned int num_polygons_); /// @brief Cast Convex to ConvexBaseTpl. /// This method should never be marked as virtual Base& base() { return static_cast(*this); } /// @brief Const cast Convex to ConvexBaseTpl. /// This method should never be marked as virtual const Base& base() const { return static_cast(*this); } /// @brief Copy constructor. /// The copy constructor only shallow copies the data (it copies the shared /// pointers but does not deep clones the data). ConvexTpl(const ConvexTpl& other) { *this = other; } /// @brief Copy operator. /// The copy operator only shallow copies the data (it copies the shared /// pointers but does not deep clones the data). ConvexTpl& operator=(const ConvexTpl& other); // Clone (deep copy). COAL_DEPRECATED_MESSAGE(Use deepcopy instead.) ConvexTpl* clone() const override { return this->deepcopy(); }; // Deep copy of a Convex. // This method deep copies every field of the class. ConvexTpl* deepcopy() const override { ConvexTpl* copy = new ConvexTpl(); deepcopy(this, copy); return copy; } /// @brief Cast this Convex vertex indices to OtherIndexType. /// This effectively deep copies this Convex into a new one. template ConvexTpl cast() const { ConvexTpl res; deepcopy(this, &res); return res; } /// based on http://number-none.com/blow/inertia/bb_inertia.doc virtual Matrix3s computeMomentofInertia() const override; virtual Vec3s computeCOM() const override; virtual Scalar computeVolume() const override; /// /// @brief Set the current Convex from a list of points and polygons. /// /// \param points list of 3D points /// \param num_points number of 3D points /// \param polygons \copydoc Convex::polygons /// \param num_polygons the number of polygons. /// \note num_polygons is not the allocated size of polygons. /// void set(std::shared_ptr> points, unsigned int num_points, std::shared_ptr> polygons, unsigned int num_polygons); /// @brief An array of PolygonT object. /// PolygonT should contains a list of vertices for each polygon, /// in counter clockwise order. std::shared_ptr> polygons; unsigned int num_polygons; protected: void fillNeighbors(); // Deep copy of a Convex. // This method deep copies every field of the class. template static void deepcopy(const ConvexTpl* source, ConvexTpl* copy); using Base::nneighbors_; private: virtual bool isEqual(const CollisionGeometry& _other) const override { const ConvexTpl* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; COAL_EQUAL_OPERATOR_CHECK(Base::isEqual(_other)); COAL_EQUAL_OPERATOR_CHECK( shared_ptrs_are_equal(polygons, other_ptr->polygons)); return true; } }; template using Convex = ConvexTpl; } // namespace coal #include "coal/shape/convex.hxx" #endif ================================================ FILE: include/coal/shape/convex.hxx ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2019, CNRS - LAAS * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Joseph Mirabel */ #ifndef COAL_SHAPE_CONVEX_HXX #define COAL_SHAPE_CONVEX_HXX #include #include #include #include "coal/shape/convex.h" namespace coal { template ConvexTpl::ConvexTpl(std::shared_ptr> points_, unsigned int num_points_, std::shared_ptr> polygons_, unsigned int num_polygons_) : Base(), polygons(polygons_), num_polygons(num_polygons_) { this->initialize(points_, num_points_); this->fillNeighbors(); this->buildSupportWarmStart(); } template ConvexTpl& ConvexTpl::operator=(const ConvexTpl& other) { if (this != &other) { // Copy the base this->base() = other.base(); // Shallow copy the polygons this->num_polygons = other.num_polygons; this->polygons = other.polygons; } return *this; } template template void ConvexTpl::deepcopy(const ConvexTpl* source, ConvexTpl* copy) { if (source == nullptr || copy == nullptr) { return; } // Deep copy the base Base::deepcopy(source, copy); // Deep copy the polygons typedef typename OtherPolygonT::IndexType OtherIndexType; copy->num_polygons = source->num_polygons; if (source->polygons != nullptr) { const std::vector& source_polygons = *(source->polygons); copy->polygons.reset( new std::vector(source_polygons.size())); std::vector& copy_polygons = *(copy->polygons); for (std::size_t i = 0; i < source_polygons.size(); ++i) { copy_polygons[i] = source_polygons[i].template cast(); } } else { copy->polygons.reset(); } } template void ConvexTpl::set(std::shared_ptr> points_, unsigned int num_points_, std::shared_ptr> polygons_, unsigned int num_polygons_) { Base::set(points_, num_points_); this->num_polygons = num_polygons_; this->polygons = polygons_; this->fillNeighbors(); this->buildSupportWarmStart(); } template Matrix3s ConvexTpl::computeMomentofInertia() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::IndexType IndexType; Matrix3s C = Matrix3s::Zero(); Matrix3s C_canonical; C_canonical << Scalar(1 / 60.0), // Scalar(1 / 120.0), // Scalar(1 / 120.0), // Scalar(1 / 120.0), // Scalar(1 / 60.0), // Scalar(1 / 120.0), // Scalar(1 / 120.0), // Scalar(1 / 120.0), // Scalar(1 / 60.0); if (!(points.get())) { std::cerr << "Error in `ConvexTpl::computeMomentofInertia`! ConvexTpl has " "no vertices." << std::endl; return C; } const std::vector& points_ = *points; if (!(polygons.get())) { std::cerr << "Error in `ConvexTpl::computeMomentofInertia`! ConvexTpl has " "no polygons." << std::endl; return C; } const std::vector& polygons_ = *polygons; for (unsigned int i = 0; i < num_polygons; ++i) { const PolygonT& polygon = polygons_[i]; // compute the center of the polygon Vec3s plane_center(0, 0, 0); for (size_type j = 0; j < polygon.size(); ++j) plane_center += points_[polygon[(IndexType)j]]; plane_center /= Scalar(polygon.size()); // compute the volume of tetrahedron making by neighboring two points, the // plane center and the reference point (zero) of the convex shape const Vec3s& v3 = plane_center; for (size_type j = 0; j < polygon.size(); ++j) { IndexType e_first = polygon[static_cast(j)]; IndexType e_second = polygon[static_cast((j + 1) % polygon.size())]; const Vec3s& v1 = points_[e_first]; const Vec3s& v2 = points_[e_second]; Matrix3s A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); } } return C.trace() * Matrix3s::Identity() - C; } template Vec3s ConvexTpl::computeCOM() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::IndexType IndexType; Vec3s com(0, 0, 0); Scalar vol = 0; if (!(points.get())) { std::cerr << "Error in `ConvexTpl::computeCOM`! ConvexTpl has no vertices." << std::endl; return com; } const std::vector& points_ = *points; if (!(polygons.get())) { std::cerr << "Error in `ConvexTpl::computeCOM`! ConvexTpl has no polygons." << std::endl; return com; } const std::vector& polygons_ = *polygons; for (unsigned int i = 0; i < num_polygons; ++i) { const PolygonT& polygon = polygons_[i]; // compute the center of the polygon Vec3s plane_center(0, 0, 0); for (size_type j = 0; j < polygon.size(); ++j) plane_center += points_[polygon[(IndexType)j]]; plane_center /= Scalar(polygon.size()); // compute the volume of tetrahedron making by neighboring two points, the // plane center and the reference point (zero) of the convex shape const Vec3s& v3 = plane_center; for (size_type j = 0; j < polygon.size(); ++j) { IndexType e_first = polygon[static_cast(j)]; IndexType e_second = polygon[static_cast((j + 1) % polygon.size())]; const Vec3s& v1 = points_[e_first]; const Vec3s& v2 = points_[e_second]; Scalar d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol; } } return com / (vol * 4); // here we choose zero as the reference } template Scalar ConvexTpl::computeVolume() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::IndexType IndexType; Scalar vol = 0; if (!(points.get())) { std::cerr << "Error in `ConvexTpl::computeVolume`! ConvexTpl has no vertices." << std::endl; return vol; } const std::vector& points_ = *points; if (!(polygons.get())) { std::cerr << "Error in `ConvexTpl::computeVolume`! ConvexTpl has no polygons." << std::endl; return vol; } const std::vector& polygons_ = *polygons; for (unsigned int i = 0; i < num_polygons; ++i) { const PolygonT& polygon = polygons_[i]; // compute the center of the polygon Vec3s plane_center(0, 0, 0); for (size_type j = 0; j < polygon.size(); ++j) plane_center += points_[polygon[(IndexType)j]]; plane_center /= Scalar(polygon.size()); // compute the volume of tetrahedron making by neighboring two points, the // plane center and the reference point (zero point) of the convex shape const Vec3s& v3 = plane_center; for (size_type j = 0; j < polygon.size(); ++j) { IndexType e_first = polygon[static_cast(j)]; IndexType e_second = polygon[static_cast((j + 1) % polygon.size())]; const Vec3s& v1 = points_[e_first]; const Vec3s& v2 = points_[e_second]; Scalar d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; } } return vol / 6; } template void ConvexTpl::fillNeighbors() { neighbors.reset(new std::vector(num_points)); typedef typename PolygonT::size_type size_type; typedef typename PolygonT::IndexType IndexType; std::vector> nneighbors(num_points); unsigned int c_nneighbors = 0; if (!(polygons.get())) { std::cerr << "Error in `ConvexTpl::fillNeighbors`! ConvexTpl has no polygons." << std::endl; } const std::vector& polygons_ = *polygons; for (unsigned int l = 0; l < num_polygons; ++l) { const PolygonT& polygon = polygons_[l]; const size_type n = polygon.size(); for (size_type j = 0; j < polygon.size(); ++j) { size_type i = (j == 0) ? n - 1 : j - 1; size_type k = (j == n - 1) ? 0 : j + 1; IndexType pi = polygon[(IndexType)i], pj = polygon[(IndexType)j], pk = polygon[(IndexType)k]; // Update neighbors of pj; if (nneighbors[pj].count(pi) == 0) { c_nneighbors++; nneighbors[pj].insert(pi); } if (nneighbors[pj].count(pk) == 0) { c_nneighbors++; nneighbors[pj].insert(pk); } } } nneighbors_.reset(new std::vector(c_nneighbors)); std::vector& neighbors_ = *neighbors; std::vector& nneighbors__ = *(nneighbors_); IndexType begin_id = 0; for (unsigned int i = 0; i < num_points; ++i) { Neighbors& n = neighbors_[i]; n.count = static_cast(nneighbors[i].size()); n.begin_id = begin_id; IndexType j = 0; for (IndexType idx : nneighbors[i]) { nneighbors__[n.begin_id + j] = idx; j++; } begin_id += n.count; } } } // namespace coal #endif ================================================ FILE: include/coal/shape/geometric_shape_to_BVH_model.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H #define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H #include "coal/shape/geometric_shapes.h" #include "coal/BVH/BVH_model.h" #include namespace coal { /// @brief Generate BVH model from box template void generateBVHModel(BVHModel& model, const Box& shape, const Transform3s& pose) { Scalar a = shape.halfSide[0]; Scalar b = shape.halfSide[1]; Scalar c = shape.halfSide[2]; std::vector points(8); std::vector tri_indices(12); points[0] = Vec3s(a, -b, c); points[1] = Vec3s(a, b, c); points[2] = Vec3s(-a, b, c); points[3] = Vec3s(-a, -b, c); points[4] = Vec3s(a, -b, -c); points[5] = Vec3s(a, b, -c); points[6] = Vec3s(-a, b, -c); points[7] = Vec3s(-a, -b, -c); tri_indices[0].set(0, 4, 1); tri_indices[1].set(1, 4, 5); tri_indices[2].set(2, 6, 3); tri_indices[3].set(3, 6, 7); tri_indices[4].set(3, 0, 2); tri_indices[5].set(2, 0, 1); tri_indices[6].set(6, 5, 7); tri_indices[7].set(7, 5, 4); tri_indices[8].set(1, 5, 2); tri_indices[9].set(2, 5, 6); tri_indices[10].set(3, 7, 0); tri_indices[11].set(0, 7, 4); for (unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]).eval(); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from sphere, given the number of segments along /// longitude and number of rings along latitude. template void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3s& pose, unsigned int seg, unsigned int ring) { std::vector points; std::vector tri_indices; Scalar r = shape.radius; Scalar phi, phid; const Scalar pi = boost::math::constants::pi(); phid = pi * 2 / Scalar(seg); phi = 0; Scalar theta, thetad; thetad = pi / Scalar(ring + 1); theta = 0; for (unsigned int i = 0; i < ring; ++i) { Scalar theta_ = theta + thetad * Scalar(i + 1); for (unsigned int j = 0; j < seg; ++j) { points.push_back(Vec3s(r * sin(theta_) * cos(phi + Scalar(j) * phid), r * sin(theta_) * sin(phi + Scalar(j) * phid), r * cos(theta_))); } } points.push_back(Vec3s(0, 0, r)); points.push_back(Vec3s(0, 0, -r)); for (unsigned int i = 0; i < ring - 1; ++i) { for (unsigned int j = 0; j < seg; ++j) { unsigned int a, b, c, d; a = i * seg + j; b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); c = (i + 1) * seg + j; d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); tri_indices.push_back(Triangle32(a, c, b)); tri_indices.push_back(Triangle32(b, c, d)); } } for (unsigned int j = 0; j < seg; ++j) { unsigned int a, b; a = j; b = (j == seg - 1) ? 0 : (j + 1); tri_indices.push_back(Triangle32(ring * seg, a, b)); a = (ring - 1) * seg + j; b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); tri_indices.push_back(Triangle32(a, ring * seg + 1, b)); } for (unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from sphere /// The difference between generateBVHModel is that it gives the number of /// triangles faces N for a sphere with unit radius. For sphere of radius r, /// then the number of triangles is r * r * N so that the area represented by a /// single triangle is approximately the same.s template void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3s& pose, unsigned int n_faces_for_unit_sphere) { Scalar r = shape.radius; Scalar n_low_bound = std::sqrt((Scalar)n_faces_for_unit_sphere / Scalar(2.)) * r * r; unsigned int ring = (unsigned int)ceil(n_low_bound); unsigned int seg = (unsigned int)ceil(n_low_bound); generateBVHModel(model, shape, pose, seg, ring); } /// @brief Generate BVH model from cylinder, given the number of segments along /// circle and the number of segments along axis. template void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3s& pose, unsigned int tot, unsigned int h_num) { std::vector points; std::vector tri_indices; Scalar r = shape.radius; Scalar h = shape.halfLength; Scalar phi, phid; const Scalar pi = boost::math::constants::pi(); phid = pi * 2 / Scalar(tot); phi = 0; Scalar hd = 2 * h / Scalar(h_num); for (unsigned int i = 0; i < tot; ++i) points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)), r * sin(phi + phid * Scalar(i)), h)); for (unsigned int i = 0; i < h_num - 1; ++i) { for (unsigned int j = 0; j < tot; ++j) { points.push_back(Vec3s(r * cos(phi + phid * Scalar(j)), r * sin(phi + phid * Scalar(j)), h - Scalar(i + 1) * hd)); } } for (unsigned int i = 0; i < tot; ++i) points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)), r * sin(phi + phid * Scalar(i)), -h)); points.push_back(Vec3s(0, 0, h)); points.push_back(Vec3s(0, 0, -h)); for (unsigned int i = 0; i < tot; ++i) { Triangle32 tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); tri_indices.push_back(tmp); } for (unsigned int i = 0; i < tot; ++i) { Triangle32 tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); tri_indices.push_back(tmp); } for (unsigned int i = 0; i < h_num; ++i) { for (unsigned int j = 0; j < tot; ++j) { unsigned int a, b, c, d; a = j; b = (j == tot - 1) ? 0 : (j + 1); c = j + tot; d = (j == tot - 1) ? tot : (j + 1 + tot); unsigned int start = i * tot; tri_indices.push_back(Triangle32(start + b, start + a, start + c)); tri_indices.push_back(Triangle32(start + b, start + c, start + d)); } } for (unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from cylinder /// Difference from generateBVHModel: is that it gives the circle split number /// tot for a cylinder with unit radius. For cylinder with larger radius, the /// number of circle split number is r * tot. template void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3s& pose, unsigned int tot_for_unit_cylinder) { Scalar r = shape.radius; Scalar h = 2 * shape.halfLength; const Scalar pi = boost::math::constants::pi(); unsigned int tot = (unsigned int)(Scalar(tot_for_unit_cylinder) * r); Scalar phid = pi * 2 / Scalar(tot); Scalar circle_edge = phid * r; unsigned int h_num = (unsigned int)ceil(h / circle_edge); generateBVHModel(model, shape, pose, tot, h_num); } /// @brief Generate BVH model from cone, given the number of segments along /// circle and the number of segments along axis. template void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3s& pose, unsigned int tot, unsigned int h_num) { std::vector points; std::vector tri_indices; Scalar r = shape.radius; Scalar h = shape.halfLength; Scalar phi, phid; const Scalar pi = boost::math::constants::pi(); phid = pi * 2 / Scalar(tot); phi = 0; Scalar hd = 2 * h / Scalar(h_num); for (unsigned int i = 0; i < h_num - 1; ++i) { Scalar h_i = h - Scalar(i + 1) * hd; Scalar rh = r * (Scalar(0.5) - h_i / h / 2); for (unsigned int j = 0; j < tot; ++j) { points.push_back(Vec3s(rh * cos(phi + phid * Scalar(j)), rh * sin(phi + phid * Scalar(j)), h_i)); } } for (unsigned int i = 0; i < tot; ++i) points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)), r * sin(phi + phid * Scalar(i)), -h)); points.push_back(Vec3s(0, 0, h)); points.push_back(Vec3s(0, 0, -h)); for (unsigned int i = 0; i < tot; ++i) { Triangle32 tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); tri_indices.push_back(tmp); } for (unsigned int i = 0; i < tot; ++i) { Triangle32 tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i); tri_indices.push_back(tmp); } for (unsigned int i = 0; i < h_num - 1; ++i) { for (unsigned int j = 0; j < tot; ++j) { unsigned int a, b, c, d; a = j; b = (j == tot - 1) ? 0 : (j + 1); c = j + tot; d = (j == tot - 1) ? tot : (j + 1 + tot); unsigned int start = i * tot; tri_indices.push_back(Triangle32(start + b, start + a, start + c)); tri_indices.push_back(Triangle32(start + b, start + c, start + d)); } } for (unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from cone /// Difference from generateBVHModel: is that it gives the circle split number /// tot for a cylinder with unit radius. For cone with larger radius, the number /// of circle split number is r * tot. template void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3s& pose, unsigned int tot_for_unit_cone) { Scalar r = shape.radius; Scalar h = 2 * shape.halfLength; const Scalar pi = boost::math::constants::pi(); unsigned int tot = (unsigned int)(Scalar(tot_for_unit_cone) * r); Scalar phid = pi * 2 / Scalar(tot); Scalar circle_edge = phid * r; unsigned int h_num = (unsigned int)ceil(h / circle_edge); generateBVHModel(model, shape, pose, tot, h_num); } } // namespace coal #endif ================================================ FILE: include/coal/shape/geometric_shapes.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_GEOMETRIC_SHAPES_H #define COAL_GEOMETRIC_SHAPES_H #include #include #include #include "coal/collision_object.h" #include "coal/data_types.h" #include "coal/shared_ptr_comparison.h" #ifdef COAL_HAS_QHULL namespace orgQhull { class Qhull; } #endif namespace coal { /// @brief Base class for all basic geometric shapes class COAL_DLLAPI ShapeBase : public CollisionGeometry { public: ShapeBase() {} ///  \brief Copy constructor ShapeBase(const ShapeBase& other) : CollisionGeometry(other), m_swept_sphere_radius(other.m_swept_sphere_radius) {} ShapeBase& operator=(const ShapeBase& other) = default; virtual ~ShapeBase() {}; /// @brief Get object type: a geometric shape OBJECT_TYPE getObjectType() const override { return OT_GEOM; } /// @brief Set radius of sphere swept around the shape. /// Must be >= 0. void setSweptSphereRadius(Scalar radius) { if (radius < 0) { COAL_THROW_PRETTY("Swept-sphere radius must be positive.", std::invalid_argument); } this->m_swept_sphere_radius = radius; } /// @brief Get radius of sphere swept around the shape. /// This radius is always >= 0. Scalar getSweptSphereRadius() const { return this->m_swept_sphere_radius; } protected: bool isEqual(const CollisionGeometry& _other) const override { const ShapeBase* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const ShapeBase& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(getSweptSphereRadius() == other.getSweptSphereRadius()); return true; } /// \brief Radius of the sphere swept around the shape. /// Default value is 0. /// Note: this property differs from `inflated` method of certain /// derived classes (e.g. Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder) /// in the sense that inflated returns a new shape which can be inflated but /// also deflated. /// Also, an inflated shape is not rounded. It simply has a different size. /// Sweeping a shape with a sphere is a different operation (a Minkowski sum), /// which rounds the sharp corners of a shape. /// The swept sphere radius is a property of the shape itself and can be /// manually updated between collision checks. Scalar m_swept_sphere_radius{0}; }; /// @defgroup Geometric_Shapes Geometric shapes /// Classes of different types of geometric shapes. /// @{ /// @brief Triangle stores the points instead of only indices of points class COAL_DLLAPI TriangleP : public ShapeBase { public: TriangleP() {}; TriangleP(const Vec3s& a_, const Vec3s& b_, const Vec3s& c_) : ShapeBase(), a(a_), b(b_), c(c_) {} TriangleP(const TriangleP& other) : ShapeBase(other), a(other.a), b(other.b), c(other.c) {} /// @brief Clone *this into a new TriangleP virtual TriangleP* clone() const override { return new TriangleP(*this); }; /// @brief virtual function of compute AABB in local coordinate void computeLocalAABB() override; NODE_TYPE getNodeType() const override { return GEOM_TRIANGLE; } // std::pair inflated(const Scalar value) const // { // if (value == 0) return std::make_pair(new TriangleP(*this), // Transform3s()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize(); // BC.normalize(); // CA.normalize(); // // Vec3s new_a(a + value * Vec3s(-AB + CA).normalized()); // Vec3s new_b(b + value * Vec3s(-BC + AB).normalized()); // Vec3s new_c(c + value * Vec3s(-CA + BC).normalized()); // // return std::make_pair(new TriangleP(new_a, new_b, new_c), // Transform3s()); // } // // Scalar minInflationValue() const // { // return (std::numeric_limits::max)(); // TODO(jcarpent): // implement // } Vec3s a, b, c; private: virtual bool isEqual(const CollisionGeometry& _other) const override { const TriangleP* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const TriangleP& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(a == other.a); COAL_EQUAL_OPERATOR_CHECK(b == other.b); COAL_EQUAL_OPERATOR_CHECK(c == other.c); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Center at zero point, axis aligned box class COAL_DLLAPI Box : public ShapeBase { public: Box(Scalar x, Scalar y, Scalar z) : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {} Box(const Vec3s& side_) : ShapeBase(), halfSide(side_ / 2) {} Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {} Box& operator=(const Box& other) { if (this == &other) return *this; this->halfSide = other.halfSide; return *this; } /// @brief Clone *this into a new Box virtual Box* clone() const override { return new Box(*this); }; /// @brief Default constructor Box() {} /// @brief box side half-length Vec3s halfSide; /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: a box NODE_TYPE getNodeType() const override { return GEOM_BOX; } Scalar computeVolume() const override { return 8 * halfSide.prod(); } Matrix3s computeMomentofInertia() const override { Scalar V = computeVolume(); Vec3s s(halfSide.cwiseAbs2() * V); return (Vec3s(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); } Scalar minInflationValue() const { return -halfSide.minCoeff(); } /// \brief Inflate the box by an amount given by `value`. /// This value can be positive or negative but must always >= /// `minInflationValue()`. /// /// \param[in] value of the shape inflation. /// /// \returns a new inflated box and the related transform to account for the /// change of shape frame std::pair inflated(const Scalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") " << "is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Box(2 * (halfSide + Vec3s::Constant(value))), Transform3s()); } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const Box* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const Box& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(halfSide == other.halfSide); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Center at zero point sphere class COAL_DLLAPI Sphere : public ShapeBase { public: /// @brief Default constructor Sphere() {} explicit Sphere(Scalar radius_) : ShapeBase(), radius(radius_) {} Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {} /// @brief Clone *this into a new Sphere virtual Sphere* clone() const override { return new Sphere(*this); }; /// @brief Radius of the sphere Scalar radius; /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: a sphere NODE_TYPE getNodeType() const override { return GEOM_SPHERE; } Matrix3s computeMomentofInertia() const override { Scalar I = Scalar(0.4) * radius * radius * computeVolume(); return I * Matrix3s::Identity(); } Scalar computeVolume() const override { return 4 * boost::math::constants::pi() * radius * radius * radius / 3; } Scalar minInflationValue() const { return -radius; } /// \brief Inflate the sphere by an amount given by `value`. /// This value can be positive or negative but must always >= /// `minInflationValue()`. /// /// \param[in] value of the shape inflation. /// /// \returns a new inflated sphere and the related transform to account for /// the change of shape frame std::pair inflated(const Scalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Sphere(radius + value), Transform3s()); } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const Sphere* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const Sphere& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(radius == other.radius); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Ellipsoid centered at point zero class COAL_DLLAPI Ellipsoid : public ShapeBase { public: /// @brief Default constructor Ellipsoid() {} Ellipsoid(Scalar rx, Scalar ry, Scalar rz) : ShapeBase(), radii(rx, ry, rz) {} explicit Ellipsoid(const Vec3s& radii) : radii(radii) {} Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {} /// @brief Clone *this into a new Ellipsoid virtual Ellipsoid* clone() const override { return new Ellipsoid(*this); }; /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2 /// + z^2/rz^2 = 1) Vec3s radii; /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: an ellipsoid NODE_TYPE getNodeType() const override { return GEOM_ELLIPSOID; } Matrix3s computeMomentofInertia() const override { Scalar V = computeVolume(); Scalar a2 = V * radii[0] * radii[0]; Scalar b2 = V * radii[1] * radii[1]; Scalar c2 = V * radii[2] * radii[2]; Scalar alpha = Scalar(0.2); return (Matrix3s() << alpha * (b2 + c2), 0, 0, 0, alpha * (a2 + c2), 0, 0, 0, alpha * (a2 + b2)) .finished(); } Scalar computeVolume() const override { return 4 * boost::math::constants::pi() * radii[0] * radii[1] * radii[2] / 3; } Scalar minInflationValue() const { return -radii.minCoeff(); } /// \brief Inflate the ellipsoid by an amount given by `value`. /// This value can be positive or negative but must always >= /// `minInflationValue()`. /// /// \param[in] value of the shape inflation. /// /// \returns a new inflated ellipsoid and the related transform to account for /// the change of shape frame std::pair inflated(const Scalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Ellipsoid(radii + Vec3s::Constant(value)), Transform3s()); } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const Ellipsoid* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const Ellipsoid& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(radii == other.radii); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Capsule /// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$ /// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule /// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$. class COAL_DLLAPI Capsule : public ShapeBase { public: /// @brief Default constructor Capsule() {} Capsule(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } Capsule(const Capsule& other) : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} /// @brief Clone *this into a new Capsule virtual Capsule* clone() const override { return new Capsule(*this); }; /// @brief Radius of capsule Scalar radius; /// @brief Half Length along z axis Scalar halfLength; /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: a capsule NODE_TYPE getNodeType() const override { return GEOM_CAPSULE; } Scalar computeVolume() const override { return boost::math::constants::pi() * radius * radius * ((halfLength * 2) + radius * 4 / Scalar(3)); } Matrix3s computeMomentofInertia() const override { Scalar v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi(); Scalar v_sph = radius * radius * radius * boost::math::constants::pi() * 4 / Scalar(3); Scalar h2 = halfLength * halfLength; Scalar r2 = radius * radius; Scalar ix = v_cyl * (h2 / Scalar(3) + r2 / Scalar(4)) + v_sph * (Scalar(0.4) * r2 + h2 + Scalar(0.75) * radius * halfLength); Scalar iz = (Scalar(0.5) * v_cyl + Scalar(0.4) * v_sph) * radius * radius; return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } Scalar minInflationValue() const { return -radius; } /// \brief Inflate the capsule by an amount given by `value`. /// This value can be positive or negative but must always >= /// `minInflationValue()`. /// /// \param[in] value of the shape inflation. /// /// \returns a new inflated capsule and the related transform to account for /// the change of shape frame std::pair inflated(const Scalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Capsule(radius + value, 2 * halfLength), Transform3s()); } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const Capsule* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const Capsule& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(radius == other.radius); COAL_EQUAL_OPERATOR_CHECK(halfLength == other.halfLength); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Cone /// The base of the cone is at \f$ z = - halfLength \f$ and the top is at /// \f$ z = halfLength \f$. class COAL_DLLAPI Cone : public ShapeBase { public: /// @brief Default constructor Cone() {} Cone(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } Cone(const Cone& other) : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} /// @brief Clone *this into a new Cone virtual Cone* clone() const override { return new Cone(*this); }; /// @brief Radius of the cone Scalar radius; /// @brief Half Length along z axis Scalar halfLength; /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: a cone NODE_TYPE getNodeType() const override { return GEOM_CONE; } Scalar computeVolume() const override { return boost::math::constants::pi() * radius * radius * (halfLength * 2) / 3; } Matrix3s computeMomentofInertia() const override { Scalar V = computeVolume(); Scalar ix = V * (Scalar(0.4) * halfLength * halfLength + 3 * radius * radius / 20); Scalar iz = Scalar(0.3) * V * radius * radius; return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } Vec3s computeCOM() const override { return Vec3s(0, 0, -Scalar(0.5) * halfLength); } Scalar minInflationValue() const { return -(std::min)(radius, halfLength); } /// \brief Inflate the cone by an amount given by `value`. /// This value can be positive or negative but must always >= /// `minInflationValue()`. /// /// \param[in] value of the shape inflation. /// /// \returns a new inflated cone and the related transform to account for the /// change of shape frame std::pair inflated(const Scalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); // tan(alpha) = 2*halfLength/radius; const Scalar tan_alpha = 2 * halfLength / radius; const Scalar sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha); const Scalar top_inflation = value / sin_alpha; const Scalar bottom_inflation = value; const Scalar new_lz = 2 * halfLength + top_inflation + bottom_inflation; const Scalar new_cz = (top_inflation + bottom_inflation) / Scalar(2); const Scalar new_radius = new_lz / tan_alpha; return std::make_pair(Cone(new_radius, new_lz), Transform3s(Vec3s(0., 0., new_cz))); } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const Cone* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const Cone& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(radius == other.radius); COAL_EQUAL_OPERATOR_CHECK(halfLength == other.halfLength); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Cylinder along Z axis. /// The cylinder is defined at its centroid. class COAL_DLLAPI Cylinder : public ShapeBase { public: /// @brief Default constructor Cylinder() {} Cylinder(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } Cylinder(const Cylinder& other) : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} Cylinder& operator=(const Cylinder& other) { if (this == &other) return *this; this->radius = other.radius; this->halfLength = other.halfLength; return *this; } /// @brief Clone *this into a new Cylinder virtual Cylinder* clone() const override { return new Cylinder(*this); }; /// @brief Radius of the cylinder Scalar radius; /// @brief Half Length along z axis Scalar halfLength; /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: a cylinder NODE_TYPE getNodeType() const override { return GEOM_CYLINDER; } Scalar computeVolume() const override { return boost::math::constants::pi() * radius * radius * (halfLength * 2); } Matrix3s computeMomentofInertia() const override { Scalar V = computeVolume(); Scalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3); Scalar iz = V * radius * radius / 2; return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } Scalar minInflationValue() const { return -(std::min)(radius, halfLength); } /// \brief Inflate the cylinder by an amount given by `value`. /// This value can be positive or negative but must always >= /// `minInflationValue()`. /// /// \param[in] value of the shape inflation. /// /// \returns a new inflated cylinder and the related transform to account for /// the change of shape frame std::pair inflated(const Scalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)), Transform3s()); } private: virtual bool isEqual(const CollisionGeometry& _other) const override { const Cylinder* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const Cylinder& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(radius == other.radius); COAL_EQUAL_OPERATOR_CHECK(halfLength == other.halfLength); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; template struct ConvexBaseTplNeighbors { typedef _IndexType IndexType; IndexType count; IndexType begin_id; bool operator==(const ConvexBaseTplNeighbors& other) const { COAL_EQUAL_OPERATOR_CHECK(count == other.count); COAL_EQUAL_OPERATOR_CHECK(begin_id == other.begin_id); return true; } bool operator!=(const ConvexBaseTplNeighbors& other) const { return !(*this == other); } }; // The support warm start polytope contains certain points of `this` // which are support points in specific directions of space. // This struct is used to warm start the support function computation for // large meshes (`num_points` > 32). template struct ConvexBaseTplSupportWarmStartPolytope { typedef _IndexType IndexType; // Array of support points to warm start the support function // computation. std::vector points; // Indices of the support points warm starts. // These are the indices of the real convex, not the indices of points in // the warm start polytope. std::vector indices; // Cast to a different index type. template ConvexBaseTplSupportWarmStartPolytope cast() const { typedef ConvexBaseTplSupportWarmStartPolytope ResType; ResType res; res.points = this->points; res.indices.clear(); for (size_t i = 0; i < this->indices.size(); ++i) { res.indices.push_back(OtherIndexType(this->indices[i])); } return res; } }; /// @brief Base for convex polytope. /// @tparam _IndexType type of vertices indexes. /// @note Inherited classes are responsible for filling ConvexBase::neighbors; template class ConvexBaseTpl : public ShapeBase { public: // clang-format off COAL_DEPRECATED_MESSAGE(Use IndexType) typedef _IndexType index_type; // clang-format on typedef _IndexType IndexType; typedef ShapeBase Base; template friend class ConvexBaseTpl; /// @brief Build a convex hull based on Qhull library /// and store the vertices and optionally the triangles /// \param points, num_points the points whose convex hull should be computed. /// \param keepTriangles if \c true, returns a Convex object which /// contains the triangle of the shape. /// \param qhullCommand the command sent to qhull. /// - if \c keepTriangles is \c true, this parameter should include /// "Qt". If \c NULL, "Qt" is passed to Qhull. /// - if \c keepTriangles is \c false, an empty string is passed to /// Qhull. /// \note Coal must have been compiled with option \c COAL_HAS_QHULL set /// to \c ON. static COAL_DLLAPI ConvexBaseTpl* convexHull( std::shared_ptr>& points, unsigned int num_points, bool keepTriangles, const char* qhullCommand = NULL); // TODO(louis): put this method in private sometime in the future. COAL_DEPRECATED static COAL_DLLAPI ConvexBaseTpl* convexHull( const Vec3s* points, unsigned int num_points, bool keepTriangles, const char* qhullCommand = NULL); virtual ~ConvexBaseTpl() {} /// @brief Cast ConvexBaseTpl to ShapeBase. /// This method should never be marked as virtual Base& base() { return static_cast(*this); } /// @brief Const cast ConvexBaseTpl to ShapeBase. /// This method should never be marked as virtual const Base& base() const { return static_cast(*this); } /// @brief Copy constructor. /// The copy constructor only shallow copies the data (it copies the shared /// pointers but does not deep clones the data). ConvexBaseTpl(const ConvexBaseTpl& other) { *this = other; } /// @brief Copy assignment operator. /// The copy assignment operator shallow copies the data, just as the copy /// constructor. ConvexBaseTpl& operator=(const ConvexBaseTpl& other); /// @brief Clone (deep copy). COAL_DEPRECATED_MESSAGE(Use deepcopy instead.) virtual ConvexBaseTpl* clone() const override { return this->deepcopy(); } /// @brief Deep copy of the ConvexBaseTpl. /// This method deep copies every field of the class. virtual ConvexBaseTpl* deepcopy() const { ConvexBaseTpl* copy = new ConvexBaseTpl(); deepcopy(this, copy); return copy; } /// @brief Cast this ConvexBase vertex indices to OtherIndexType. /// This effectively deep copies this ConvexBaseTpl into a new one. template ConvexBaseTpl cast() const { ConvexBaseTpl res; deepcopy(this, &res); return res; } /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: a convex polytope NODE_TYPE getNodeType() const override; #ifdef COAL_HAS_QHULL /// @brief Builds the double description of the convex polytope, i.e. the set /// of hyperplanes which intersection form the polytope. void COAL_DLLAPI buildDoubleDescription(); #endif using Neighbors = coal::ConvexBaseTplNeighbors; /// @brief Get the index of the j-th neighbor of the i-th vertex. IndexType neighbor(IndexType i, IndexType j) const { assert(i < IndexType(num_points)); const std::vector& nns = *neighbors; IndexType begin_id = nns[i].begin_id; #ifndef NDEBUG IndexType count = nns[i].count; assert(j < count); #endif const std::vector& nns_vec = *nneighbors_; return nns_vec[begin_id + j]; } /// @brief Above this threshold, the convex polytope is considered large. /// This influcences the way the support function is computed. static constexpr size_t num_vertices_large_convex_threshold = 32; /// @brief An array of the points of the polygon. std::shared_ptr> points; unsigned int num_points; /// @brief An array of the normals of the polygon. std::shared_ptr> normals; /// @brief An array of the offsets to the normals of the polygon. /// Note: there are as many offsets as normals. std::shared_ptr> offsets; unsigned int num_normals_and_offsets; /// @brief Neighbors of each vertex. /// It is an array of size num_points. For each vertex, it contains the number /// of neighbors and a list of indices pointing to them. std::shared_ptr> neighbors; /// @brief center of the convex polytope, this is used for collision: center /// is guaranteed in the internal of the polytope (as it is convex) Vec3s center; using SupportWarmStartPolytope = ConvexBaseTplSupportWarmStartPolytope; /// @brief Number of support warm starts. static constexpr size_t num_support_warm_starts = 14; /// @brief Support warm start polytopes. SupportWarmStartPolytope support_warm_starts; protected: /// @brief Construct an uninitialized convex object /// Initialization is done with ConvexBase::initialize. ConvexBaseTpl() : ShapeBase(), num_points(0), num_normals_and_offsets(0), center(Vec3s::Zero()) {} /// @brief Initialize the points of the convex shape /// This also initializes the ConvexBase::center. /// /// \param points_ list of 3D points /// /// \param num_points_ number of 3D points void initialize(std::shared_ptr> points_, unsigned int num_points_); /// @brief Set the points of the convex shape. /// /// \param points_ list of 3D points /// /// \param num_points_ number of 3D points void set(std::shared_ptr> points_, unsigned int num_points_); #ifdef COAL_HAS_QHULL void COAL_DLLAPI buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh); #endif /// @brief Build the support points warm starts. void COAL_DLLAPI buildSupportWarmStart(); /// @brief Array of indices of the neighbors of each vertex. /// Since we don't know a priori the number of neighbors of each vertex, we /// store the indices of the neighbors in a single array. /// The `neighbors` attribute, an array of `Neighbors`, is used to point each /// vertex to the right indices in the `nneighbors_` array. std::shared_ptr> nneighbors_; protected: /// @brief Deep copy of a ConvexBaseTpl. /// This method deep copies every field of the class. template static void deepcopy(const ConvexBaseTpl* source, ConvexBaseTpl* copy); void computeCenter(); virtual bool isEqual(const CollisionGeometry& _other) const override { const ConvexBaseTpl* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const ConvexBaseTpl& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(num_points == other.num_points); COAL_EQUAL_OPERATOR_CHECK(shared_ptrs_are_equal(points, other.points)); COAL_EQUAL_OPERATOR_CHECK( shared_ptrs_are_equal(neighbors, other.neighbors)); COAL_EQUAL_OPERATOR_CHECK( shared_ptrs_are_equal(nneighbors_, other.nneighbors_)); COAL_EQUAL_OPERATOR_CHECK(shared_ptrs_are_equal(normals, other.normals)); COAL_EQUAL_OPERATOR_CHECK(shared_ptrs_are_equal(offsets, other.offsets)); COAL_EQUAL_OPERATOR_CHECK(support_warm_starts.points == other.support_warm_starts.points); COAL_EQUAL_OPERATOR_CHECK(support_warm_starts.indices == other.support_warm_starts.indices); COAL_EQUAL_OPERATOR_CHECK(center == other.center); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; typedef ConvexBaseTpl ConvexBase16; typedef ConvexBaseTpl ConvexBase32; COAL_DEPRECATED_MESSAGE(Use ConvexBase32 instead.) typedef ConvexBase32 ConvexBase; template class ConvexTpl; /// @brief Half Space: this is equivalent to the Plane in ODE. /// A Half space has a priviledged direction: the direction of the normal. /// The separation plane is defined as n * x = d; Points in the negative side of /// the separation plane (i.e. {x | n * x < d}) are inside the half space and /// points in the positive side of the separation plane (i.e. {x | n * x > d}) /// are outside the half space. /// Note: prefer using a Halfspace instead of a Plane if possible, it has better /// behavior w.r.t. collision detection algorithms. class COAL_DLLAPI Halfspace : public ShapeBase { public: /// @brief Construct a half space with normal direction and offset Halfspace(const Vec3s& n_, Scalar d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } /// @brief Construct a plane with normal direction and offset Halfspace(Scalar a, Scalar b, Scalar c, Scalar d_) : ShapeBase(), n(a, b, c), d(d_) { unitNormalTest(); } Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {} Halfspace(const Halfspace& other) : ShapeBase(other), n(other.n), d(other.d) {} /// @brief operator = Halfspace& operator=(const Halfspace& other) { n = other.n; d = other.d; return *this; } /// @brief Clone *this into a new Halfspace virtual Halfspace* clone() const override { return new Halfspace(*this); }; Scalar signedDistance(const Vec3s& p) const { return n.dot(p) - (d + this->getSweptSphereRadius()); } Scalar distance(const Vec3s& p) const { return std::abs(this->signedDistance(p)); } /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: a half space NODE_TYPE getNodeType() const override { return GEOM_HALFSPACE; } Scalar minInflationValue() const { return std::numeric_limits::lowest(); } /// \brief Inflate the halfspace by an amount given by `value`. /// This value can be positive or negative but must always >= /// `minInflationValue()`. /// /// \param[in] value of the shape inflation. /// /// \returns a new inflated halfspace and the related transform to account for /// the change of shape frame std::pair inflated(const Scalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Halfspace(n, d + value), Transform3s()); } /// @brief Plane normal Vec3s n; /// @brief Plane offset Scalar d; protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); private: virtual bool isEqual(const CollisionGeometry& _other) const override { const Halfspace* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const Halfspace& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(n == other.n); COAL_EQUAL_OPERATOR_CHECK(d == other.d); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Infinite plane. /// A plane can be viewed as two half spaces; it has no priviledged direction. /// Note: prefer using a Halfspace instead of a Plane if possible, it has better /// behavior w.r.t. collision detection algorithms. class COAL_DLLAPI Plane : public ShapeBase { public: /// @brief Construct a plane with normal direction and offset Plane(const Vec3s& n_, Scalar d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } /// @brief Construct a plane with normal direction and offset Plane(Scalar a, Scalar b, Scalar c, Scalar d_) : ShapeBase(), n(a, b, c), d(d_) { unitNormalTest(); } Plane() : ShapeBase(), n(1, 0, 0), d(0) {} Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {} /// @brief operator = Plane& operator=(const Plane& other) { n = other.n; d = other.d; return *this; } /// @brief Clone *this into a new Plane virtual Plane* clone() const override { return new Plane(*this); }; Scalar signedDistance(const Vec3s& p) const { const Scalar dist = n.dot(p) - d; Scalar signed_dist = std::abs(n.dot(p) - d) - this->getSweptSphereRadius(); if (dist >= 0) { return signed_dist; } if (signed_dist >= 0) { return -signed_dist; } return signed_dist; } Scalar distance(const Vec3s& p) const { return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius()); } /// @brief Compute AABB void computeLocalAABB() override; /// @brief Get node type: a plane NODE_TYPE getNodeType() const override { return GEOM_PLANE; } /// @brief Plane normal Vec3s n; /// @brief Plane offset Scalar d; protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); private: virtual bool isEqual(const CollisionGeometry& _other) const override { const Plane* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const Plane& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK(ShapeBase::isEqual(other)); COAL_EQUAL_OPERATOR_CHECK(n == other.n); COAL_EQUAL_OPERATOR_CHECK(d == other.d); return true; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** @} */ // end of Geometric_Shapes } // namespace coal #include "coal/shape/geometric_shapes.hxx" #endif ================================================ FILE: include/coal/shape/geometric_shapes.hxx ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_GEOMETRIC_SHAPES_HXX #define COAL_GEOMETRIC_SHAPES_HXX #include "coal/shape/convex.h" #include "coal/shape/geometric_shapes.h" #include "coal/narrowphase/support_data.h" #include namespace coal { template NODE_TYPE ConvexBaseTpl::getNodeType() const { COAL_THROW_PRETTY("Uknown vertex index type for ConvexBase.", std::runtime_error); return NODE_COUNT; } template <> inline NODE_TYPE ConvexBaseTpl::getNodeType() const { return GEOM_CONVEX16; } template <> inline NODE_TYPE ConvexBaseTpl::getNodeType() const { return GEOM_CONVEX32; } template void ConvexBaseTpl::initialize( std::shared_ptr> points_, unsigned int num_points_) { this->points = points_; this->num_points = num_points_; COAL_ASSERT(this->points->size() == this->num_points, "The number of points is not consistent with the size of the " "points vector", std::logic_error); this->num_normals_and_offsets = 0; this->normals.reset(); this->offsets.reset(); this->computeCenter(); } template void ConvexBaseTpl::set(std::shared_ptr> points_, unsigned int num_points_) { initialize(points_, num_points_); } template ConvexBaseTpl& ConvexBaseTpl::operator=( const ConvexBaseTpl& other) { if (this != &other) { // Copy the base this->base() = other.base(); // Shallow copy the rest of the data this->points = other.points; this->num_points = other.num_points; this->normals = other.normals; this->offsets = other.offsets; this->num_normals_and_offsets = other.num_normals_and_offsets; this->neighbors = other.neighbors; this->nneighbors_ = other.nneighbors_; this->center = other.center; this->support_warm_starts = other.support_warm_starts; } return *this; } template template void ConvexBaseTpl::deepcopy(const ConvexBaseTpl* source, ConvexBaseTpl* copy) { if (source == nullptr || copy == nullptr) { return; } // Copy the base copy->base() = source->base(); // Copy the non-templated data if (source->points != nullptr) { copy->points.reset(new std::vector(*source->points)); } else { copy->points.reset(); assert(source->num_points == 0); } copy->num_points = source->num_points; if (source->normals != nullptr) { copy->normals.reset(new std::vector(*source->normals)); } else { copy->normals.reset(); assert(source->num_normals_and_offsets == 0); } if (source->offsets != nullptr) { copy->offsets.reset(new std::vector(*source->offsets)); } else { copy->offsets.reset(); assert(source->num_normals_and_offsets == 0); } copy->num_normals_and_offsets = source->num_normals_and_offsets; copy->center = source->center; copy->support_warm_starts = source->support_warm_starts.template cast(); // Convert neighbors to new type if (source->points->size() >= (std::size_t)(std::numeric_limits::max())) { COAL_THROW_PRETTY( "The source has more points than the max of OtherIndexType.", std::runtime_error); } if (source->nneighbors_ != nullptr) { const std::vector& source_nneighbors = *(source->nneighbors_); copy->nneighbors_.reset( new std::vector(source_nneighbors.size())); std::vector& copy_nneighbors = *(copy->nneighbors_); for (std::size_t i = 0; i < source_nneighbors.size(); ++i) { copy_nneighbors[i] = OtherIndexType(source_nneighbors[i]); } } else { copy->nneighbors_.reset(); } if (source->neighbors != nullptr) { typedef typename ConvexBaseTpl::Neighbors OtherNeighbors; assert(source->neighbors->size() == source->points->size()); const std::vector& source_neighbors = *(source->neighbors); copy->neighbors.reset( new std::vector(source_neighbors.size())); std::vector& copy_neighbors = *(copy->neighbors); for (std::size_t i = 0; i < source_neighbors.size(); ++i) { copy_neighbors[i].count = OtherIndexType(source_neighbors[i].count); copy_neighbors[i].begin_id = OtherIndexType(source_neighbors[i].begin_id); } } else { copy->neighbors.reset(); } } template void ConvexBaseTpl::computeCenter() { center.setZero(); const std::vector& points_ = *points; for (std::size_t i = 0; i < num_points; ++i) center += points_[i]; // TODO(jcarpent): vectorization center /= Scalar(num_points); } // forward declaration for ConvexBase template void computeBV(const S& s, const Transform3s& tf, BV& bv); template void ConvexBaseTpl::computeLocalAABB() { computeBV>(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } // Reorders `tri` such that the dot product between the normal of triangle and // the vector `triangle barycentre - convex_tri.center` is positive. template void reorderTriangle(const ConvexTpl>* convex_tri, TriangleTpl& tri) { Vec3s p0, p1, p2; p0 = (*(convex_tri->points))[tri[0]]; p1 = (*(convex_tri->points))[tri[1]]; p2 = (*(convex_tri->points))[tri[2]]; Vec3s barycentre_tri, center_barycenter; barycentre_tri = (p0 + p1 + p2) / 3; center_barycenter = barycentre_tri - convex_tri->center; Vec3s edge_tri1, edge_tri2, n_tri; edge_tri1 = p1 - p0; edge_tri2 = p2 - p1; n_tri = edge_tri1.cross(edge_tri2); if (center_barycenter.dot(n_tri) < 0) { tri.set(tri[1], tri[0], tri[2]); } } } // namespace coal #endif // COAL_GEOMETRIC_SHAPES_HXX ================================================ FILE: include/coal/shape/geometric_shapes_traits.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2015-2022, CNRS, Inria * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H #define COAL_GEOMETRIC_SHAPES_TRAITS_H #include "coal/shape/geometric_shapes.h" namespace coal { struct shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = false, HasInflatedSupportFunction = false, IsStrictlyConvex = false }; }; template struct shape_traits : shape_traits_base {}; template <> struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, HasInflatedSupportFunction = false, IsStrictlyConvex = false }; }; template <> struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, HasInflatedSupportFunction = false, IsStrictlyConvex = false }; }; template <> struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, HasInflatedSupportFunction = false, IsStrictlyConvex = true }; }; template <> struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, HasInflatedSupportFunction = false, IsStrictlyConvex = true }; }; template <> struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, HasInflatedSupportFunction = false, IsStrictlyConvex = false }; }; template <> struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, HasInflatedSupportFunction = false, IsStrictlyConvex = false }; }; template <> struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, HasInflatedSupportFunction = false, IsStrictlyConvex = false }; }; template struct shape_traits> : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = true, IsInflatable = true, HasInflatedSupportFunction = true, IsStrictlyConvex = false }; }; template <> struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, HasInflatedSupportFunction = false, IsStrictlyConvex = false }; }; } // namespace coal #endif // ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H ================================================ FILE: include/coal/shape/geometric_shapes_utility.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_GEOMETRIC_SHAPES_UTILITY_H #define COAL_GEOMETRIC_SHAPES_UTILITY_H #include #include "coal/shape/geometric_shapes.h" #include "coal/BV/BV.h" #include "coal/internal/BV_fitter.h" namespace coal { /// @cond IGNORE namespace details { /// @brief get the vertices of some convex shape which can bound the given shape /// in a specific configuration COAL_DLLAPI std::vector getBoundVertices(const Box& box, const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Cone& cone, const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, const Transform3s& tf); template std::vector getBoundVertices(const ConvexBaseTpl& convex, const Transform3s& tf) { std::vector result(convex.num_points); const std::vector& points_ = *(convex.points); for (std::size_t i = 0; i < convex.num_points; ++i) { result[i] = tf.transform(points_[i]); } return result; } } // namespace details /// @endcond /// @brief calculate a bounding volume for a shape in a specific configuration template inline void computeBV(const S& s, const Transform3s& tf, BV& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } std::vector convex_bound_vertices = details::getBoundVertices(s, tf); fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(), bv); } template <> COAL_DLLAPI void computeBV(const Box& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Sphere& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Ellipsoid& e, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Capsule& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Cone& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Cylinder& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const ConvexBase32& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const ConvexBase16& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const TriangleP& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Box& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Sphere& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Capsule& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Cone& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Cylinder& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const ConvexBase32& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const ConvexBase16& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, const Transform3s& tf, RSS& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, const Transform3s& tf, OBBRSS& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, const Transform3s& tf, kIOS& bv); template <> COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<16>& bv); template <> COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<18>& bv); template <> COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<24>& bv); template <> COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, RSS& bv); template <> COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, OBBRSS& bv); template <> COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, kIOS& bv); template <> COAL_DLLAPI void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<16>& bv); template <> COAL_DLLAPI void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<18>& bv); template <> COAL_DLLAPI void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<24>& bv); /// @brief construct a box shape (with a configuration) from a given bounding /// volume COAL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const AABB& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const kIOS& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf); COAL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf); COAL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3s& tf); COAL_DLLAPI Plane transform(const Plane& a, const Transform3s& tf); COAL_DLLAPI std::array transformToHalfspaces( const Plane& a, const Transform3s& tf); } // namespace coal #endif ================================================ FILE: include/coal/shared_ptr_comparison.h ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2026, Inria // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of the copyright holder nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #ifndef COAL_SHARED_PTR_COMPARISON_H #define COAL_SHARED_PTR_COMPARISON_H #include // The content of this file is adapted from Pinocchio. namespace coal { /// @brief Compares two std::shared_ptr by value. /// Returns true if both are null, both point to the same object, /// or both are non-null and their pointees compare equal. template bool shared_ptrs_are_equal(const std::shared_ptr& ptr1, const std::shared_ptr& ptr2) { if (ptr1 == ptr2) return true; if (ptr1 && ptr2) return *ptr1 == *ptr2; return false; } } // namespace coal #endif // COAL_SHARED_PTR_COMPARISON_H ================================================ FILE: include/coal/third_party/boost/core/data.hpp ================================================ /* Copyright 2023 Glen Joseph Fernandes (glenjofe@gmail.com) Distributed under the Boost Software License, Version 1.0. (http://www.boost.org/LICENSE_1_0.txt) */ #ifndef BOOST_CORE_DATA_HPP #define BOOST_CORE_DATA_HPP #include // Note: MSVC doesn't define __cpp_lib_nonmember_container_access but supports // the feature even in C++14 mode #if (defined(__cpp_lib_nonmember_container_access) && \ (__cpp_lib_nonmember_container_access >= 201411l)) || \ (defined(_MSC_VER) && (_MSC_VER >= 1900)) namespace boost { using std::data; } // namespace boost #else // (defined(__cpp_lib_nonmember_container_access) ... #include #include namespace boost { template inline constexpr auto data(C& c) noexcept(noexcept(c.data())) -> decltype(c.data()) { return c.data(); } template inline constexpr auto data(const C& c) noexcept(noexcept(c.data())) -> decltype(c.data()) { return c.data(); } template inline constexpr T* data(T (&a)[N]) noexcept { return a; } template inline constexpr const T* data(std::initializer_list l) noexcept { return l.begin(); } } // namespace boost #endif // (defined(__cpp_lib_nonmember_container_access) ... #endif ================================================ FILE: include/coal/third_party/boost/core/detail/assert.hpp ================================================ /* Copyright 2025 Glen Joseph Fernandes (glenjofe@gmail.com) Distributed under the Boost Software License, Version 1.0. (http://www.boost.org/LICENSE_1_0.txt) */ #undef BOOST_CORE_DETAIL_ASSERT #if !defined(__clang__) && !defined(__INTEL_COMPILER) && defined(__GNUC__) && \ (__GNUC__ < 5) #define BOOST_CORE_DETAIL_ASSERT(expr) void(0) #else #include #define BOOST_CORE_DETAIL_ASSERT(expr) BOOST_ASSERT(expr) #endif ================================================ FILE: include/coal/third_party/boost/core/make_span.hpp ================================================ /* Copyright 2023 Glen Joseph Fernandes (glenjofe@gmail.com) Distributed under the Boost Software License, Version 1.0. (http://www.boost.org/LICENSE_1_0.txt) */ #ifndef BOOST_CORE_MAKE_SPAN_HPP #define BOOST_CORE_MAKE_SPAN_HPP #include "boost/core/span.hpp" namespace boost { template inline constexpr span make_span(I* f, std::size_t c) noexcept { return span(f, c); } template inline constexpr span make_span(I* f, I* l) noexcept { return span(f, l); } template inline constexpr span make_span(T (&a)[N]) noexcept { return span(a); } template inline constexpr span make_span(std::array& a) noexcept { return span(a); } template inline constexpr span make_span( const std::array& a) noexcept { return span(a); } template inline span::type> make_span(R&& r) { return span::type>(std::forward(r)); } } // namespace boost #endif ================================================ FILE: include/coal/third_party/boost/core/span.hpp ================================================ /* Copyright 2019-2023 Glen Joseph Fernandes (glenjofe@gmail.com) Distributed under the Boost Software License, Version 1.0. (http://www.boost.org/LICENSE_1_0.txt) */ #ifndef BOOST_CORE_SPAN_HPP #define BOOST_CORE_SPAN_HPP #include "boost/core/detail/assert.hpp" #include "boost/core/data.hpp" #include #include #include namespace boost { constexpr std::size_t dynamic_extent = static_cast(-1); template class span; namespace detail { template struct span_convertible { static constexpr bool value = false; }; template struct span_convertible::value>::type> { static constexpr bool value = true; }; template struct span_capacity { static constexpr bool value = E == boost::dynamic_extent || E == N; }; template struct span_compatible { static constexpr bool value = span_capacity::value && span_convertible::value; }; template using span_uncvref = typename std::remove_cv::type>::type; template struct span_is_span { static constexpr bool value = false; }; template struct span_is_span> { static constexpr bool value = true; }; template struct span_is_array { static constexpr bool value = false; }; template struct span_is_array> { static constexpr bool value = true; }; template using span_ptr = decltype(boost::data(std::declval())); template struct span_data {}; template struct span_data< T, typename std::enable_if>::value>::type> { typedef typename std::remove_pointer>::type type; }; template struct span_has_data { static constexpr bool value = false; }; template struct span_has_data::type, T>::value>::type> { static constexpr bool value = true; }; template struct span_has_size { static constexpr bool value = false; }; template struct span_has_size< R, typename std::enable_if().size()), std::size_t>::value>::type> { static constexpr bool value = true; }; template struct span_is_range { static constexpr bool value = (std::is_const::value || std::is_lvalue_reference::value) && !span_is_span>::value && !span_is_array>::value && !std::is_array>::value && span_has_data::value && span_has_size::value; }; template struct span_implicit { static constexpr bool value = E == boost::dynamic_extent || N != boost::dynamic_extent; }; template struct span_copyable { static constexpr bool value = (N == boost::dynamic_extent || span_capacity::value) && span_convertible::value; }; template struct span_sub { static constexpr std::size_t value = E == boost::dynamic_extent ? boost::dynamic_extent : E - O; }; template struct span_store { constexpr span_store(T* p_, std::size_t) noexcept : p(p_) {} static constexpr std::size_t n = E; T* p; }; template struct span_store { constexpr span_store(T* p_, std::size_t n_) noexcept : p(p_), n(n_) {} T* p; std::size_t n; }; template struct span_bytes { static constexpr std::size_t value = sizeof(T) * E; }; template struct span_bytes { static constexpr std::size_t value = boost::dynamic_extent; }; } // namespace detail template class span { public: typedef T element_type; typedef typename std::remove_cv::type value_type; typedef std::size_t size_type; typedef std::ptrdiff_t difference_type; typedef T* pointer; typedef const T* const_pointer; typedef T& reference; typedef const T& const_reference; typedef T* iterator; typedef const T* const_iterator; typedef std::reverse_iterator reverse_iterator; typedef std::reverse_iterator const_reverse_iterator; static constexpr std::size_t extent = E; template < std::size_t N = E, typename std::enable_if::type = 0> constexpr span() noexcept : s_(0, 0) {} template ::value, int>::type = 0> constexpr span(I* f, size_type c) : s_(f, c) {} template ::value, int>::type = 0> explicit constexpr span(I* f, size_type c) : s_(f, c) {} template ::value, int>::type = 0> constexpr span(I* f, L* l) : s_(f, l - f) {} template ::value, int>::type = 0> explicit constexpr span(I* f, L* l) : s_(f, l - f) {} template ::value, int>::type = 0> constexpr span(typename std::enable_if::type (&a)[N]) noexcept : s_(a, N) {} template ::value, int>::type = 0> constexpr span(std::array& a) noexcept : s_(a.data(), N) {} template < class U, std::size_t N, typename std::enable_if::value, int>::type = 0> constexpr span(const std::array& a) noexcept : s_(a.data(), N) {} template ::value, int>::type = 0> constexpr span(R&& r) noexcept(noexcept(boost::data(r)) && noexcept(r.size())) : s_(boost::data(r), r.size()) {} template ::value, int>::type = 0> explicit constexpr span(R&& r) noexcept(noexcept(boost::data(r)) && noexcept(r.size())) : s_(boost::data(r), r.size()) {} template < class U, std::size_t N, typename std::enable_if::value && detail::span_copyable::value, int>::type = 0> constexpr span(const span& s) noexcept : s_(s.data(), s.size()) {} template < class U, std::size_t N, typename std::enable_if::value && detail::span_copyable::value, int>::type = 0> explicit constexpr span(const span& s) noexcept : s_(s.data(), s.size()) {} template constexpr span first() const { static_assert(C <= E, "Count <= Extent"); return span(s_.p, C); } template constexpr span last() const { static_assert(C <= E, "Count <= Extent"); return span(s_.p + (s_.n - C), C); } template constexpr typename std::enable_if::value>>::type subspan() const { static_assert(O <= E, "Offset <= Extent"); return span::value>(s_.p + O, s_.n - O); } template constexpr typename std::enable_if>::type subspan() const { static_assert(O <= E && C <= E - O, "Offset <= Extent && Count <= Extent - Offset"); return span(s_.p + O, C); } constexpr span first(size_type c) const { return BOOST_CORE_DETAIL_ASSERT(c <= size()), span(s_.p, c); } constexpr span last(size_type c) const { return BOOST_CORE_DETAIL_ASSERT(c <= size()), span(s_.p + (s_.n - c), c); } constexpr span subspan( size_type o, size_type c = dynamic_extent) const { return BOOST_CORE_DETAIL_ASSERT(o <= size() && (c == dynamic_extent || c + o <= size())), span(s_.p + o, c == dynamic_extent ? s_.n - o : c); } constexpr size_type size() const noexcept { return s_.n; } constexpr size_type size_bytes() const noexcept { return s_.n * sizeof(T); } constexpr bool empty() const noexcept { return s_.n == 0; } constexpr reference operator[](size_type i) const { return BOOST_CORE_DETAIL_ASSERT(i < size()), s_.p[i]; } constexpr reference front() const { return BOOST_CORE_DETAIL_ASSERT(!empty()), *s_.p; } constexpr reference back() const { return BOOST_CORE_DETAIL_ASSERT(!empty()), s_.p[s_.n - 1]; } constexpr pointer data() const noexcept { return s_.p; } constexpr iterator begin() const noexcept { return s_.p; } constexpr iterator end() const noexcept { return s_.p + s_.n; } constexpr reverse_iterator rbegin() const noexcept { return reverse_iterator(s_.p + s_.n); } constexpr reverse_iterator rend() const noexcept { return reverse_iterator(s_.p); } constexpr const_iterator cbegin() const noexcept { return s_.p; } constexpr const_iterator cend() const noexcept { return s_.p + s_.n; } constexpr const_reverse_iterator crbegin() const noexcept { return const_reverse_iterator(s_.p + s_.n); } constexpr const_reverse_iterator crend() const noexcept { return const_reverse_iterator(s_.p); } private: detail::span_store s_; }; #if defined(BOOST_NO_CXX17_INLINE_VARIABLES) template constexpr std::size_t span::extent; #endif #ifdef __cpp_deduction_guides template span(I*, L) -> span; template span(T (&)[N]) -> span; template span(std::array&) -> span; template span(const std::array&) -> span; template span(R&&) -> span::type>; template span(span) -> span; #endif #ifdef __cpp_lib_byte template inline span::value> as_bytes( span s) noexcept { return span::value>( reinterpret_cast(s.data()), s.size_bytes()); } template inline typename std::enable_if< !std::is_const::value, span::value>>::type as_writable_bytes(span s) noexcept { return span::value>( reinterpret_cast(s.data()), s.size_bytes()); } #endif } // namespace boost #endif ================================================ FILE: include/coal/timings.h ================================================ // // Copyright (c) 2021-2025 INRIA // #ifndef COAL_TIMINGS_FWD_H #define COAL_TIMINGS_FWD_H #include "coal/fwd.hh" #include namespace coal { struct CPUTimes { double wall; double user; double system; CPUTimes() : wall(0), user(0), system(0) {} void clear() { wall = user = system = 0; } CPUTimes& operator+=(const CPUTimes& rhs) { wall += rhs.wall; user += rhs.user; system += rhs.system; return *this; } bool operator==(const CPUTimes& rhs) const { if (this == &rhs) return true; COAL_EQUAL_OPERATOR_CHECK(wall == rhs.wall); COAL_EQUAL_OPERATOR_CHECK(user == rhs.user); COAL_EQUAL_OPERATOR_CHECK(system == rhs.system); return true; } }; inline CPUTimes operator+(CPUTimes lhs, const CPUTimes& rhs) { lhs += rhs; return lhs; } /// /// @brief This class mimics the way "boost/timer/timer.hpp" operates while /// using the modern std::chrono library. /// Importantly, this class will only have an effect for C++11 and more. /// struct COAL_DLLAPI Timer { typedef std::chrono::steady_clock clock_type; typedef clock_type::duration duration_type; /// \brief Default constructor for the timer /// /// \param[in] start_on_construction if true, the timer will be run just after /// the object is created Timer(const bool start_on_construction = true) : m_is_stopped(true) { if (start_on_construction) Timer::start(); } CPUTimes elapsed() const { if (m_is_stopped) return m_times; CPUTimes current(m_times); std::chrono::time_point current_clock = std::chrono::steady_clock::now(); current.user += static_cast( std::chrono::duration_cast( current_clock - m_start) .count()) * 1e-3; return current; } duration_type duration() const { return (m_end - m_start); } void start() { if (m_is_stopped) { m_is_stopped = false; m_times.clear(); m_start = std::chrono::steady_clock::now(); } } void stop() { if (m_is_stopped) return; m_is_stopped = true; m_end = std::chrono::steady_clock::now(); m_times.user += static_cast( std::chrono::duration_cast( m_end - m_start) .count()) * 1e-3; } void resume() { if (m_is_stopped) { m_start = std::chrono::steady_clock::now(); m_is_stopped = false; } } bool is_stopped() const { return m_is_stopped; } protected: CPUTimes m_times; bool m_is_stopped; std::chrono::time_point m_start, m_end; }; } // namespace coal #endif // ifndef COAL_TIMINGS_FWD_H ================================================ FILE: include/hpp/fcl/BV/AABB.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BV/BV.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BV/BV_node.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BV/OBB.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BV/OBBRSS.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BV/RSS.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BV/kDOP.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BV/kIOS.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BVH/BVH_front.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BVH/BVH_internal.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BVH/BVH_model.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/BVH/BVH_utility.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_SSaP.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_SaP.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_bruteforce.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_callbacks.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_collision_manager.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_interval_tree.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/broadphase_spatialhash.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/default_broadphase_callbacks.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/hierarchy_tree.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/interval_tree.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/interval_tree_node.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/morton-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/morton.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/node_base-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/node_base.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/node_base_array-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/node_base_array.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/simple_hash_table.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/simple_interval-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/simple_interval.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/sparse_hash_table.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/spatial_hash-inl.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/broadphase/detail/spatial_hash.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/coal.hpp ================================================ #ifndef HPP_FCL_COAL_HPP #define HPP_FCL_COAL_HPP #include #include #define COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL // Don't setup warning because there can be ignored by -isystem #ifndef COAL_DISABLE_HPP_FCL_WARNINGS #pragma message( \ "Please update your includes from 'hpp/fcl' to 'coal' or define COAL_DISABLE_HPP_FCL_WARNINGS") #endif #define HPP_FCL_VERSION_AT_LEAST(major, minor, patch) \ COAL_VERSION_AT_LEAST(major, minor, patch) #endif // COAL_FWD_HPP ================================================ FILE: include/hpp/fcl/collision.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/collision_data.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/collision_func_matrix.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/collision_object.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/collision_utility.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/config.hh ================================================ #include #include ================================================ FILE: include/hpp/fcl/contact_patch/contact_patch_solver.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/contact_patch/contact_patch_solver.hxx ================================================ #include #include ================================================ FILE: include/hpp/fcl/contact_patch.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/contact_patch_func_matrix.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/data_types.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/deprecated.hh ================================================ #include #include ================================================ FILE: include/hpp/fcl/distance.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/distance_func_matrix.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/fwd.hh ================================================ #include #include ================================================ FILE: include/hpp/fcl/hfield.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/BV_fitter.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/BV_splitter.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/intersect.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/shape_shape_contact_patch_func.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/shape_shape_func.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/tools.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal_node_base.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal_node_bvh_shape.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal_node_bvhs.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal_node_hfield_shape.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal_node_octree.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal_node_setup.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal_node_shapes.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/internal/traversal_recurse.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/logging.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/math/matrix_3f.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/math/transform.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/math/types.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/math/vec_3f.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/mesh_loader/assimp.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/mesh_loader/loader.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/narrowphase/gjk.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/narrowphase/minkowski_difference.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/narrowphase/narrowphase.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/narrowphase/narrowphase_defaults.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/narrowphase/support_data.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/narrowphase/support_functions.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/narrowphase/support_functions.hxx ================================================ #include #include ================================================ FILE: include/hpp/fcl/octree.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/AABB.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/BVH_model.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/BV_node.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/BV_splitter.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/OBB.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/OBBRSS.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/RSS.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/archive.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/collision_data.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/collision_object.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/contact_patch.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/convex.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/eigen.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/fwd.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/geometric_shapes.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/hfield.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/kDOP.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/kIOS.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/memory.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/octree.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/quadrilateral.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/serializer.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/transform.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/serialization/triangle.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/shape/convex.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/shape/convex.hxx ================================================ #include #include ================================================ FILE: include/hpp/fcl/shape/geometric_shape_to_BVH_model.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/shape/geometric_shapes.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/shape/geometric_shapes.hxx ================================================ #include #include ================================================ FILE: include/hpp/fcl/shape/geometric_shapes_traits.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/shape/geometric_shapes_utility.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/timings.h ================================================ #include #include ================================================ FILE: include/hpp/fcl/warning.hh ================================================ #include #include ================================================ FILE: package.xml ================================================ coal 3.0.3 An extension of the Flexible Collision Library. Joseph Mirabel Justin Carpentier Louis Montaut Wolfgang Merkt Guilhem Saurel BSD-3-Clause https://github.com/coal-library/coal git doxygen jrl_cmakemodules python3-lxml doxygen python3-lxml python3 python3-numpy eigen boost assimp liboctomap-dev eigenpy catkin cmake cmake ================================================ FILE: pixi.toml ================================================ [workspace] name = "coal" version = "3.0.3" description = "An extension of the Flexible Collision Library" platforms = ["linux-64", "osx-64", "osx-arm64", "win-64"] channels = ["conda-forge"] license = "BSD-3-Clause" license-file = "LICENSE" preview = ["pixi-build"] [dependencies] ccache = ">=4.9.1" cmake = ">=3.15" cxx-compiler = ">=1.7.0" ninja = ">=1.11" pkg-config = ">=0.29.2" git = ">=2.47.0" nanobind = ">=2.5.0,<3" libboost-devel = ">=1.80.0" eigen = ">=3.4.0" assimp = ">=5.4" numpy = ">=1.22.0" python = ">=3.9.0" doxygen = ">=1.8" lxml = ">=5.3" pylatexenc = ">=2.10" nanoeigenpy = ">=0.3.0" [package] name = { workspace = true } version = { workspace = true } [package.build] backend = { name = "pixi-build-cmake", version = "0.3.*" } [package.build.config] extra-args = [ "-DGENERATE_PYTHON_STUBS=ON", "-DBUILD_TESTING=OFF", "-DCOAL_HAS_QHULL=ON", "-DCOAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL=ON", "-DCOAL_PYTHON_NANOBIND=OFF", ] [package.build.target.win-64.config] extra-args = [ "-DPYTHON_SITELIB=%PREFIX%/Lib/site-packages", "-DGENERATE_PYTHON_STUBS=ON", "-DBUILD_TESTING=OFF", "-DCOAL_HAS_QHULL=ON", "-DCOAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL=ON", "-DCOAL_PYTHON_NANOBIND=OFF", ] [package.build-dependencies] pkg-config = ">=0.29.2" git = ">=2.47.0" lxml = ">=5.3" pylatexenc = ">=2.10" doxygen = ">=1.8" [package.host-dependencies] libboost-devel = ">=1.80.0" libboost-python-devel = ">=1.80.0" eigen = ">=3.4.0" eigen-abi-devel = ">=3.4.0" assimp = ">=5.4" numpy = ">=1.22.0" python = ">=3.9.0" eigenpy = ">=3.7.0" octomap = ">=1.10" [package.run-dependencies] eigen = ">=3.4.0" [package.target.win.host-dependencies] qhull = ">=2020.2" [package.target.unix.host-dependencies] qhull = ">=2020.2" qhull-static = ">=2020.2" [target.unix.activation] scripts = ["development/scripts/pixi/activation.sh"] [target.win-64.activation] scripts = ["development/scripts/pixi/activation.bat"] [activation.env] # Setup ccache CMAKE_CXX_COMPILER_LAUNCHER = "ccache" # Create compile_commands.json for language server CMAKE_EXPORT_COMPILE_COMMANDS = "ON" # Activate color output with Ninja CMAKE_COLOR_DIAGNOSTICS = "ON" # Help ccache manage generated files and PCH (https://ccache.dev/manual/latest.html#_precompiled_headers) CCACHE_SLOPPINESS = "include_file_ctime,include_file_mtime,pch_defines,time_macros" [tasks] # We must avoid to set CMAKE_CXX_FLAGS because of WIN32 # https://discourse.cmake.org/t/strictly-appending-to-cmake-lang-flags/6478 configure = { cmd = [ "CXXFLAGS=$COAL_CXX_FLAGS", "cmake", "-G", "Ninja", "-B", "build", "-S", ".", # Don't use standard layout because it's not well implemented on Windows "-DPYTHON_STANDARD_LAYOUT=OFF", "-DPython_EXECUTABLE=$PYTHON_EXECUTABLE", "-DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX", "-DCMAKE_BUILD_TYPE=$COAL_BUILD_TYPE", "-DGENERATE_PYTHON_STUBS=$COAL_PYTHON_STUBS", "-DCOAL_HAS_QHULL=$COAL_HAS_QHULL", "-DCOAL_PYTHON_NANOBIND=$COAL_PYTHON_NANOBIND", ] } build = { cmd = "cmake --build build --target all", depends-on = ["configure"] } clean = { cmd = "rm -rf build" } test = { cmd = "ctest --test-dir build --output-on-failure", depends-on = [ "build", ] } [feature.lint] dependencies = { pre-commit = ">=3.6.2" } tasks = { lint = { cmd = "pre-commit run --all" } } # Increment the version number with COAL_VERSION variable [feature.new-version.dependencies] tomlkit = ">=0.13.2" [feature.new-version.tasks] configure-new-version = { cmd = [ "CXXFLAGS=$COAL_CXX_FLAGS", "cmake", "-G", "Ninja", "-B", "build_new_version", "-S", ".", # Don't use standard layout because it's not well implemented on Windows "-DPYTHON_STANDARD_LAYOUT=OFF", "-DPython_EXECUTABLE=$PYTHON_EXECUTABLE", "-DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX", "-DCMAKE_BUILD_TYPE=$COAL_BUILD_TYPE", "-DGENERATE_PYTHON_STUBS=ON", "-DCOAL_HAS_QHULL=ON", "-DCOAL_PYTHON_NANOBIND=$COAL_PYTHON_NANOBIND", ] } release-new-version = { cmd = "VERSION=$COAL_VERSION cmake --build build_new_version --target release", depends-on = [ "configure-new-version", ] } # QHull support [feature.qhull.target.win] dependencies = { qhull = ">=2020.2" } activation = { env = { COAL_HAS_QHULL = "ON" } } [feature.qhull.target.unix] dependencies = { qhull = ">=2020.2", qhull-static = ">=2020.2" } activation = { env = { COAL_HAS_QHULL = "ON" } } # Legacy Python bindings [feature.boost-python] dependencies = { libboost-python-devel = ">=1.80.0", eigenpy = ">=3.7.0" } activation.env = { COAL_PYTHON_NANOBIND = "OFF" } # Octomap support [feature.octomap] dependencies = { octomap = ">=1.10" } [feature.python-latest.dependencies] python = "3.14.*" [feature.python-oldest.dependencies] python = "3.10.*" # nanobind need typing_extensions on Python <3.11 typing_extensions = ">=4.13.2" # Use clang-cl on Windows # If something go wrong, we can check this repository: # https://github.com/conda-forge/clang-win-activation-feedstock/tree/main # coal-octree test is crashing on Windows when building with clang > 22.1 [feature.clang-cl] platforms = ["win-64"] dependencies = { clangxx = "<22.1", lld = "*" } # Absolute path is needed to avoid using system clang-cl [feature.clang-cl.activation.env] CC = "%CONDA_PREFIX%\\Library\\bin\\clang-cl" CXX = "%CONDA_PREFIX%\\Library\\bin\\clang-cl" # Use clang on GNU/Linux [feature.clang] platforms = ["linux-64"] activation = { env = { CC = "clang", CXX = "clang++" } } dependencies = { clangxx = "*" } # [feature.test-pixi-build] # dependencies = { coal = { path = "." }, cmake = ">=3.22", python = "*" } # [feature.test-pixi-build.tasks] # test-cmake = "cmake -S test/pixi_build -B build_test_pixi_build" # test-python = "python -c 'import coal'" # test = { depends-on = ["test-cmake", "test-python"] } [environments] default = { features = ["python-latest"], solve-group = "python-latest" } clang = { features = ["clang", "python-latest"] } lint = { features = ["lint"], solve-group = "python-latest" } qhull = { features = ["qhull", "python-latest"], solve-group = "python-latest" } octomap = { features = [ "octomap", "python-latest", ], solve-group = "python-latest" } python-oldest = { features = ["python-oldest"], solve-group = "python-oldest" } all = { features = [ "qhull", "octomap", "python-latest", ], solve-group = "python-latest" } # No solve-group, because boost.python will be legacy all-boost-python = { features = [ "qhull", "octomap", "python-latest", "boost-python", ] } all-python-oldest = { features = [ "qhull", "octomap", "python-oldest", ], solve-group = "python-oldest" } all-clang-cl = { features = [ "qhull", "octomap", "clang-cl", "python-latest", ], solve-group = "python-latest" } # Release a new software version new-version = { features = [ "new-version", "qhull", "octomap", "python-latest", ], solve-group = "python-latest" } # test-pixi-build = { features = ["test-pixi-build"], no-default-feature = true } ================================================ FILE: python/CMakeLists.txt ================================================ # # Software License Agreement (BSD License) # # Copyright (c) 2019-2023 CNRS-LAAS INRIA # Author: Joseph Mirabel # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of CNRS-LAAS. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. find_package(Boost REQUIRED) if(GENERATE_PYTHON_STUBS) include(${JRL_CMAKE_MODULES}/stubs.cmake) endif(GENERATE_PYTHON_STUBS) # Name of the Python library set(PYTHON_LIB_NAME ${PROJECT_NAME}_pywrap) set( ${PYTHON_LIB_NAME}_HEADERS fwd.hh coal.hh deprecation.hh broadphase/fwd.hh broadphase/broadphase_collision_manager.hh broadphase/broadphase_callbacks.hh pickle.hh utils/std-pair.hh serializable.hh printable.hh ) set( ENABLE_PYTHON_DOXYGEN_AUTODOC TRUE CACHE BOOL "Enable automatic documentation of Python bindings from Doxygen documentation" ) if(NOT ENABLE_PYTHON_DOXYGEN_AUTODOC OR NOT DOXYGEN_FOUND) set(ENABLE_DOXYGEN_AUTODOC FALSE) else() set(ENABLE_DOXYGEN_AUTODOC TRUE) execute_process( COMMAND ${PYTHON_EXECUTABLE} -c "import lxml" RESULT_VARIABLE _pypkg_found OUTPUT_QUIET ERROR_QUIET ) if(_pypkg_found EQUAL 0) message(STATUS "Found Python package lxml.") else() set(ENABLE_DOXYGEN_AUTODOC FALSE) message( STATUS "Python package lxml not found. Python bindings will not be documented." ) message(STATUS " You can install it with: pip install lxml") endif() execute_process( COMMAND ${PYTHON_EXECUTABLE} -c "import pylatexenc" RESULT_VARIABLE _pypkg_found OUTPUT_QUIET ERROR_QUIET ) if(NOT _pypkg_found EQUAL 0) message(STATUS "Python package pylatexenc not found.") message( STATUS " Formulas in the Python bindings documentation may look ugly." ) message(STATUS " You can install it with: pip install pylatexenc") endif() unset(_pypkg_found) endif() if(ENABLE_DOXYGEN_AUTODOC) add_custom_target( generate_doxygen_cpp_doc COMMAND ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/doc/python/doxygen_xml_parser.py ${PROJECT_BINARY_DIR}/doc/doxygen-xml/index.xml ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc > ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc.log BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc.log COMMENT "Generating Doxygen C++ documentation" ) add_dependencies(generate_doxygen_cpp_doc doc) list( APPEND ${PYTHON_LIB_NAME}_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh ) endif() set( ${PYTHON_LIB_NAME}_SOURCES version.cc math.cc collision-geometries.cc collision.cc contact_patch.cc distance.cc coal.cc gjk.cc broadphase/broadphase.cc ) if(COAL_HAS_OCTOMAP) list(APPEND ${PYTHON_LIB_NAME}_SOURCES octree.cc) endif(COAL_HAS_OCTOMAP) add_library( ${PYTHON_LIB_NAME} MODULE ${${PYTHON_LIB_NAME}_SOURCES} ${${PYTHON_LIB_NAME}_HEADERS} ) target_include_directories( ${PYTHON_LIB_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS} ) target_include_directories( ${PYTHON_LIB_NAME} PRIVATE "${PROJECT_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}" ) if(WIN32) target_link_libraries(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY}) endif(WIN32) add_dependencies(${PROJECT_NAME}_python ${PYTHON_LIB_NAME}) ADD_HEADER_GROUP(${PYTHON_LIB_NAME}_HEADERS) ADD_SOURCE_GROUP(${PYTHON_LIB_NAME}_SOURCES) if(ENABLE_DOXYGEN_AUTODOC) add_dependencies(${PYTHON_LIB_NAME} generate_doxygen_cpp_doc) target_compile_definitions( ${PYTHON_LIB_NAME} PRIVATE COAL_HAS_DOXYGEN_AUTODOC ) endif() target_link_libraries( ${PYTHON_LIB_NAME} PUBLIC ${PROJECT_NAME}::${PROJECT_NAME} eigenpy::eigenpy Boost::boost ) set_target_properties( ${PYTHON_LIB_NAME} PROPERTIES PREFIX "" SUFFIX "${PYTHON_EXT_SUFFIX}" OUTPUT_NAME "${PYTHON_LIB_NAME}" LIBRARY_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${PROJECT_NAME}" # On Windows, shared library are treat as binary RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${PROJECT_NAME}" ) if(IS_ABSOLUTE ${PYTHON_SITELIB}) set(ABSOLUTE_PYTHON_SITELIB ${PYTHON_SITELIB}) else() set(ABSOLUTE_PYTHON_SITELIB ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}) endif() set(${PYTHON_LIB_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${PROJECT_NAME}) if(UNIX) GET_RELATIVE_RPATH( ${${PYTHON_LIB_NAME}_INSTALL_DIR} ${PYTHON_LIB_NAME}_INSTALL_RPATH ) set_target_properties( ${PYTHON_LIB_NAME} PROPERTIES INSTALL_RPATH "${${PYTHON_LIB_NAME}_INSTALL_RPATH}" ) endif() install( TARGETS ${PYTHON_LIB_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR} ) # --- GENERATE STUBS if(GENERATE_PYTHON_STUBS) LOAD_STUBGEN() GENERATE_STUBS( ${CMAKE_CURRENT_BINARY_DIR} ${PROJECT_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${PYTHON_LIB_NAME} ${PROJECT_NAME}::${PROJECT_NAME} ) endif(GENERATE_PYTHON_STUBS) # --- INSTALL SCRIPTS set(PYTHON_FILES __init__.py viewer.py windows_dll_manager.py) foreach(python ${PYTHON_FILES}) PYTHON_BUILD(${PROJECT_NAME} ${python}) install( FILES "${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}/${python}" DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR} ) endforeach(python) if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) PYTHON_INSTALL_ON_SITE(hppfcl __init__.py COMPONENT hpp-fcl-compatibility) PYTHON_INSTALL_ON_SITE(hppfcl viewer.py COMPONENT hpp-fcl-compatibility) endif() ================================================ FILE: python/broadphase/broadphase.cc ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2022 INRIA // Author: Justin Carpentier // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS. nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include "coal/fwd.hh" #include "../coal.hh" #include "../utils/std-pair.hh" #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #include "coal/broadphase/broadphase_bruteforce.h" #include "coal/broadphase/broadphase_SaP.h" #include "coal/broadphase/broadphase_SSaP.h" #include "coal/broadphase/broadphase_interval_tree.h" #include "coal/broadphase/broadphase_spatialhash.h" COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" COAL_COMPILER_DIAGNOSTIC_POP #include "doxygen_autodoc/coal/broadphase/default_broadphase_callbacks.h" // #include "doxygen_autodoc/coal/broadphase/broadphase_dynamic_AABB_tree.h" // #include //"doxygen_autodoc/coal/broadphase/broadphase_dynamic_AABB_tree_array.h" // #include "doxygen_autodoc/coal/broadphase/broadphase_bruteforce.h" // #include "doxygen_autodoc/coal/broadphase/broadphase_SaP.h" // #include "doxygen_autodoc/coal/broadphase/broadphase_SSaP.h" // #include "doxygen_autodoc/coal/broadphase/broadphase_interval_tree.h" // #include "doxygen_autodoc/coal/broadphase/broadphase_spatialhash.h" #endif #include "broadphase_callbacks.hh" #include "broadphase_collision_manager.hh" using namespace coal; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS void exposeBroadPhase() { CollisionCallBackBaseWrapper::expose(); DistanceCallBackBaseWrapper::expose(); // CollisionCallBackDefault bp::class_>( "CollisionCallBackDefault", bp::no_init) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(CollisionCallBackDefault, data); // DistanceCallBackDefault bp::class_>( "DistanceCallBackDefault", bp::no_init) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(DistanceCallBackDefault, data); // CollisionCallBackCollect bp::class_>( "CollisionCallBackCollect", bp::no_init) .def(dv::init()) .DEF_CLASS_FUNC(CollisionCallBackCollect, numCollisionPairs) .DEF_CLASS_FUNC2(CollisionCallBackCollect, getCollisionPairs, bp::return_value_policy()) .def(dv::member_func( "exist", (bool (CollisionCallBackCollect::*)( const CollisionCallBackCollect::CollisionPair&) const) & CollisionCallBackCollect::exist)) .def(dv::member_func("exist", (bool (CollisionCallBackCollect::*)( CollisionObject*, CollisionObject*) const) & CollisionCallBackCollect::exist)); StdPairConverter::registration(); bp::class_("CollisionData", bp::no_init) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(CollisionData, request) .DEF_RW_CLASS_ATTRIB(CollisionData, result) .DEF_RW_CLASS_ATTRIB(CollisionData, done) .DEF_CLASS_FUNC(CollisionData, clear); bp::class_("DistanceData", bp::no_init) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(DistanceData, request) .DEF_RW_CLASS_ATTRIB(DistanceData, result) .DEF_RW_CLASS_ATTRIB(DistanceData, done) .DEF_CLASS_FUNC(DistanceData, clear); BroadPhaseCollisionManagerWrapper::expose(); BroadPhaseCollisionManagerWrapper::exposeDerived< DynamicAABBTreeCollisionManager>(); BroadPhaseCollisionManagerWrapper::exposeDerived< DynamicAABBTreeArrayCollisionManager>(); BroadPhaseCollisionManagerWrapper::exposeDerived< IntervalTreeCollisionManager>(); BroadPhaseCollisionManagerWrapper::exposeDerived(); BroadPhaseCollisionManagerWrapper::exposeDerived(); BroadPhaseCollisionManagerWrapper::exposeDerived(); // Specific case of SpatialHashingCollisionManager { typedef detail::SimpleHashTable HashTable; typedef SpatialHashingCollisionManager Derived; bp::class_>( "SpatialHashingCollisionManager", bp::no_init) .def(dv::init>()); } } COAL_COMPILER_DIAGNOSTIC_POP ================================================ FILE: python/broadphase/broadphase_callbacks.hh ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2022 INRIA // Author: Justin Carpentier // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of INRIA nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH #define COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH #include #include "coal/fwd.hh" #include "coal/broadphase/broadphase_callbacks.h" #include "../coal.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/coal/broadphase/broadphase_callbacks.h" #endif namespace coal { struct CollisionCallBackBaseWrapper : CollisionCallBackBase, bp::wrapper { typedef CollisionCallBackBase Base; void init() { this->get_override("init")(); } bool collide(CollisionObject* o1, CollisionObject* o2) { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" return this->get_override("collide")(o1, o2); #pragma GCC diagnostic pop } static void expose() { bp::class_( "CollisionCallBackBase", bp::no_init) .def("init", bp::pure_virtual(&Base::init), doxygen::member_func_doc(&Base::init)) .def("collide", bp::pure_virtual(&Base::collide), doxygen::member_func_doc(&Base::collide)) .def("__call__", &Base::operator(), doxygen::member_func_doc(&Base::operator())); } }; // CollisionCallBackBaseWrapper struct DistanceCallBackBaseWrapper : DistanceCallBackBase, bp::wrapper { typedef DistanceCallBackBase Base; typedef DistanceCallBackBaseWrapper Self; void init() { this->get_override("init")(); } bool distance(CollisionObject* o1, CollisionObject* o2, Eigen::Matrix& dist) { return distance(o1, o2, dist.coeffRef(0, 0)); } bool distance(CollisionObject* o1, CollisionObject* o2, Scalar& dist) { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" return this->get_override("distance")(o1, o2, dist); #pragma GCC diagnostic pop } static void expose() { bp::class_( "DistanceCallBackBase", bp::no_init) .def("init", bp::pure_virtual(&Base::init), doxygen::member_func_doc(&Base::init)) .def("distance", bp::pure_virtual( static_cast& dist)>(&Self::distance)), doxygen::member_func_doc(&Base::distance)) .def("__call__", &Base::operator(), doxygen::member_func_doc(&Base::operator())); } }; // DistanceCallBackBaseWrapper } // namespace coal #endif // ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH ================================================ FILE: python/broadphase/broadphase_collision_manager.hh ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2022 INRIA // Author: Justin Carpentier // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of INRIA nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH #define COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH #include #include "coal/fwd.hh" #include "coal/broadphase/broadphase_collision_manager.h" #include "coal/broadphase/default_broadphase_callbacks.h" #include "../coal.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/coal/broadphase/broadphase_collision_manager.h" #endif #include #include namespace coal { struct BroadPhaseCollisionManagerWrapper : BroadPhaseCollisionManager, bp::wrapper { typedef BroadPhaseCollisionManager Base; void registerObjects(const std::vector& other_objs) { this->get_override("registerObjects")(other_objs); } void registerObject(CollisionObject* obj) { this->get_override("registerObjects")(obj); } void unregisterObject(CollisionObject* obj) { this->get_override("unregisterObject")(obj); } void update(const std::vector& other_objs) { this->get_override("update")(other_objs); } void update(CollisionObject* obj) { this->get_override("update")(obj); } void update() { this->get_override("update")(); } void setup() { this->get_override("setup")(); } void clear() { this->get_override("clear")(); } std::vector getObjects() const { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" return this->get_override("getObjects")(); #pragma GCC diagnostic pop } void collide(CollisionCallBackBase* callback) const { this->get_override("collide")(callback); } void collide(CollisionObject* obj, CollisionCallBackBase* callback) const { this->get_override("collide")(obj, callback); } void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase* callback) const { this->get_override("collide")(other_manager, callback); } void distance(DistanceCallBackBase* callback) const { this->get_override("distance")(callback); } void distance(CollisionObject* obj, DistanceCallBackBase* callback) const { this->get_override("collide")(obj, callback); } void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase* callback) const { this->get_override("collide")(other_manager, callback); } bool empty() const { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" return this->get_override("empty")(); #pragma GCC diagnostic pop } size_t size() const { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" return this->get_override("size")(); #pragma GCC diagnostic pop } static void expose() { bp::class_( "BroadPhaseCollisionManager", bp::no_init) .def("registerObjects", bp::pure_virtual(&Base::registerObjects), doxygen::member_func_doc(&Base::registerObjects), bp::with_custodian_and_ward_postcall<1, 2>()) .def("registerObject", bp::pure_virtual(&Base::registerObject), doxygen::member_func_doc(&Base::registerObject), bp::with_custodian_and_ward_postcall<1, 2>()) .def("unregisterObject", bp::pure_virtual(&Base::unregisterObject), doxygen::member_func_doc(&Base::unregisterObject)) .def("update", bp::pure_virtual((void (Base::*)())&Base::update), doxygen::member_func_doc((void (Base::*)())(&Base::update))) .def("update", bp::pure_virtual((void (Base::*)( const std::vector&))&Base::update), doxygen::member_func_doc((void (Base::*)( const std::vector&))(&Base::update)), bp::with_custodian_and_ward_postcall<1, 2>()) .def("update", bp::pure_virtual( (void (Base::*)(CollisionObject* obj))&Base::update), doxygen::member_func_doc( (void (Base::*)(CollisionObject* obj))(&Base::update)), bp::with_custodian_and_ward_postcall<1, 2>()) .def("setup", bp::pure_virtual(&Base::setup), doxygen::member_func_doc(&Base::setup)) .def("clear", bp::pure_virtual(&Base::clear), doxygen::member_func_doc(&Base::clear)) .def("empty", bp::pure_virtual(&Base::empty), doxygen::member_func_doc(&Base::empty)) .def("size", bp::pure_virtual(&Base::size), doxygen::member_func_doc(&Base::size)) .def( "getObjects", bp::pure_virtual((std::vector (Base::*)() const) & Base::getObjects), doxygen::member_func_doc( (std::vector (Base::*)() const) & Base::getObjects), bp::with_custodian_and_ward_postcall<0, 1>()) .def( "collide", bp::pure_virtual((void (Base::*)(CollisionCallBackBase*) const) & Base::collide), doxygen::member_func_doc( (void (Base::*)(CollisionCallBackBase*) const) & Base::collide)) .def("collide", bp::pure_virtual((void (Base::*)(CollisionObject*, CollisionCallBackBase*) const) & Base::collide), doxygen::member_func_doc( (void (Base::*)(CollisionObject*, CollisionCallBackBase*) const) & Base::collide)) .def("collide", bp::pure_virtual((void (Base::*)(BroadPhaseCollisionManager*, CollisionCallBackBase*) const) & Base::collide), doxygen::member_func_doc( (void (Base::*)(BroadPhaseCollisionManager*, CollisionCallBackBase*) const) & Base::collide)) .def( "distance", bp::pure_virtual((void (Base::*)(DistanceCallBackBase*) const) & Base::distance), doxygen::member_func_doc( (void (Base::*)(DistanceCallBackBase*) const) & Base::distance)) .def("distance", bp::pure_virtual((void (Base::*)(CollisionObject*, DistanceCallBackBase*) const) & Base::distance), doxygen::member_func_doc( (void (Base::*)(CollisionObject*, DistanceCallBackBase*) const) & Base::distance)) .def("distance", bp::pure_virtual((void (Base::*)(BroadPhaseCollisionManager*, DistanceCallBackBase*) const) & Base::distance), doxygen::member_func_doc( (void (Base::*)(BroadPhaseCollisionManager*, DistanceCallBackBase*) const) & Base::distance)); } template static void exposeDerived() { std::string class_name = boost::typeindex::type_id().pretty_name(); boost::algorithm::replace_all(class_name, "coal::", ""); #if defined(_WIN32) boost::algorithm::replace_all(class_name, "class ", ""); #endif bp::class_ >( class_name.c_str(), bp::no_init) .def(dv::init()); } }; // BroadPhaseCollisionManagerWrapper } // namespace coal #endif // ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH ================================================ FILE: python/broadphase/fwd.hh ================================================ // // Copyright (c) 2022 INRIA // #ifndef COAL_PYTHON_BROADPHASE_FWD_HH #define COAL_PYTHON_BROADPHASE_FWD_HH #include "coal/fwd.hh" namespace coal { namespace python { void exposeBroadPhase(); } } // namespace coal #endif // ifndef COAL_PYTHON_BROADPHASE_FWD_HH ================================================ FILE: python/coal/__init__.py ================================================ # Software License Agreement (BSD License) # # Copyright (c) 2019 INRIA # Author: Justin Carpentier # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of CNRS-LAAS. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # ruff: noqa: F401, F403 # On Windows, if coal.dll is not in the same directory than # the .pyd, it will not be loaded. # We first try to load coal, then, if it fail and we are on Windows: # 1. We add all paths inside COAL_WINDOWS_DLL_PATH to DllDirectory # 2. If COAL_WINDOWS_DLL_PATH we add the relative path from the # package directory to the bin directory to DllDirectory # This solution is inspired from: # - https://github.com/PixarAnimationStudios/OpenUSD/pull/1511/files # - https://stackoverflow.com/questions/65334494/python-c-extension-packaging-dll-along-with-pyd # More resources on https://github.com/diffpy/pyobjcryst/issues/33 try: from .coal_pywrap import * # noqa from .coal_pywrap import __raw_version__, __version__ except ImportError: import platform if platform.system() == "Windows": from .windows_dll_manager import build_directory_manager, get_dll_paths with build_directory_manager() as dll_dir_manager: for p in get_dll_paths(): dll_dir_manager.add_dll_directory(p) from .coal_pywrap import * # noqa from .coal_pywrap import __raw_version__, __version__ # noqa else: raise ================================================ FILE: python/coal/viewer.py ================================================ # Software License Agreement (BSD License) # # Copyright (c) 2019 CNRS # Author: Joseph Mirabel import warnings import numpy as np from gepetto import Color import coal def applyConfiguration(gui, name, tf): gui.applyConfiguration( name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist() ) def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)): if isinstance(geom, coal.Capsule): return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color) elif isinstance(geom, coal.Cylinder): return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color) elif isinstance(geom, coal.Box): w, h, d = (2.0 * geom.halfSide).tolist() return gui.addBox(name, w, h, d, color) elif isinstance(geom, coal.Sphere): return gui.addSphere(name, geom.radius, color) elif isinstance(geom, coal.Cone): return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color) elif isinstance(geom, coal.Convex): pts = [ geom.points(geom.polygons(f)[i]).tolist() for f in range(geom.num_polygons) for i in range(3) ] gui.addCurve(name, pts, color) gui.setCurveMode(name, "TRIANGLES") gui.setLightingMode(name, "ON") gui.setBoolProperty(name, "BackfaceDrawing", True) return True elif isinstance(geom, coal.ConvexBase): pts = [geom.points(i).tolist() for i in range(geom.num_points)] gui.addCurve(name, pts, color) gui.setCurveMode(name, "POINTS") gui.setLightingMode(name, "OFF") return True else: msg = "Unsupported geometry type for %s (%s)" % (name, type(geom)) warnings.warn(msg, category=UserWarning, stacklevel=2) return False def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True): gui.createGroup(group_name) r = 0.01 if closest_points: p = [group_name + "/p1", group_name + "/p2"] gui.addSphere(p[0], r, Color.red) gui.addSphere(p[1], r, Color.blue) qid = [0, 0, 0, 1] gui.applyConfigurations( p, [ res.getNearestPoint1().tolist() + qid, res.getNearestPoint2().tolist() + qid, ], ) if normal: n = group_name + "/normal" gui.addArrow(n, r, 0.1, Color.green) gui.applyConfiguration( n, res.getNearestPoint1().tolist() + coal.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal) .coeffs() .tolist(), ) gui.refresh() def displayCollisionResult(gui, group_name, res, color=Color.green): if res.isCollision(): if gui.nodeExists(group_name): gui.setVisibility(group_name, "ON") else: gui.createGroup(group_name) for i in range(res.numContacts()): contact = res.getContact(i) n = group_name + "/contact" + str(i) depth = contact.penetration_depth if gui.nodeExists(n): gui.setFloatProperty(n, "Size", depth) gui.setFloatProperty(n, "Radius", 0.1 * depth) gui.setColor(n, color) else: gui.addArrow(n, depth * 0.1, depth, color) N = contact.normal P = contact.pos gui.applyConfiguration( n, (P - depth * N / 2).tolist() + coal.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N) .coeffs() .tolist(), ) gui.refresh() elif gui.nodeExists(group_name): gui.setVisibility(group_name, "OFF") ================================================ FILE: python/coal/windows_dll_manager.py ================================================ import contextlib import os import sys def get_dll_paths(): coal_paths = os.getenv("COAL_WINDOWS_DLL_PATH") if coal_paths is None: # From https://peps.python.org/pep-0250/#implementation # lib/python-version/site-packages/package RELATIVE_DLL_PATH1 = "..\\..\\..\\..\\bin" # lib/site-packages/package RELATIVE_DLL_PATH2 = "..\\..\\..\\bin" # For unit test RELATIVE_DLL_PATH3 = "..\\..\\bin" return [ os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH1), os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH2), os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH3), ] else: return coal_paths.split(os.pathsep) class PathManager(contextlib.AbstractContextManager): """Restore PATH state after importing Python module""" def add_dll_directory(self, dll_dir: str): os.environ["PATH"] += os.pathsep + dll_dir def __enter__(self): self.old_path = os.environ["PATH"] return self def __exit__(self, *exc_details): os.environ["PATH"] = self.old_path class DllDirectoryManager(contextlib.AbstractContextManager): """Restore DllDirectory state after importing Python module""" def add_dll_directory(self, dll_dir: str): # add_dll_directory can fail on relative path and non # existing path. # Since we don't know all the fail criterion we just ignore # thrown exception try: self.dll_dirs.append(os.add_dll_directory(dll_dir)) except OSError: pass def __enter__(self): self.dll_dirs = [] return self def __exit__(self, *exc_details): for d in self.dll_dirs: d.close() def build_directory_manager(): if sys.version_info >= (3, 8): return DllDirectoryManager() else: return PathManager() ================================================ FILE: python/coal.cc ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2019-2022 CNRS-LAAS INRIA // Author: Joseph Mirabel // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS. nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include "coal.hh" #include "coal/fwd.hh" #include "coal/shape/geometric_shapes.h" #include "coal/BVH/BVH_model.h" #include "coal/mesh_loader/loader.h" #include "coal/collision.h" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/coal/mesh_loader/loader.h" #endif #include "../doc/python/doxygen.hh" #include "../doc/python/doxygen-boost.hh" using namespace coal; namespace dv = doxygen::visitor; #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(load_overloads, MeshLoader::load, 1, 2) #pragma GCC diagnostic pop void exposeMeshLoader() { using namespace boost::python; if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "MeshLoader", doxygen::class_doc(), init >( (arg("self"), arg("node_type")), doxygen::constructor_doc())) .def("load", &MeshLoader::load, load_overloads((arg("self"), arg("filename"), arg("scale")), doxygen::member_func_doc(&MeshLoader::load))) .def(dv::member_func("loadOctree", &MeshLoader::loadOctree)); } if (!eigenpy::register_symbolic_link_to_registered_type()) { class_, shared_ptr >( "CachedMeshLoader", doxygen::class_doc(), init >( (arg("self"), arg("node_type")), doxygen::constructor_doc())); } } BOOST_PYTHON_MODULE(coal_pywrap) { namespace bp = boost::python; PyImport_ImportModule("warnings"); eigenpy::enableEigenPy(); exposeVersion(); exposeMaths(); exposeCollisionGeometries(); exposeCollisionObject(); exposeMeshLoader(); exposeCollisionAPI(); exposeContactPatchAPI(); exposeDistanceAPI(); exposeGJK(); #ifdef COAL_HAS_OCTOMAP exposeOctree(); #endif exposeBroadPhase(); } ================================================ FILE: python/coal.hh ================================================ #include "fwd.hh" void exposeVersion(); void exposeMaths(); void exposeCollisionGeometries(); void exposeCollisionObject(); void exposeMeshLoader(); void exposeCollisionAPI(); void exposeContactPatchAPI(); void exposeDistanceAPI(); void exposeGJK(); #ifdef COAL_HAS_OCTOMAP void exposeOctree(); #endif void exposeBroadPhase(); ================================================ FILE: python/collision-geometries.cc ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2019-2024 CNRS-LAAS INRIA // Author: Joseph Mirabel // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS. nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) #include #endif #include "coal.hh" #include "deprecation.hh" #include "coal/fwd.hh" #include "coal/shape/geometric_shapes.h" #include "coal/shape/convex.h" #include "coal/BVH/BVH_model.h" #include "coal/hfield.h" #include "coal/serialization/memory.h" #include "coal/serialization/AABB.h" #include "coal/serialization/BVH_model.h" #include "coal/serialization/hfield.h" #include "coal/serialization/geometric_shapes.h" #include "coal/serialization/convex.h" #include "pickle.hh" #include "serializable.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC // FIXME for a reason I do not understand, doxygen fails to understand that // BV_splitter is not defined in coal/BVH/BVH_model.h #include "coal/internal/BV_splitter.h" #include "coal/broadphase/detail/hierarchy_tree.h" #include "doxygen_autodoc/coal/BVH/BVH_model.h" #include "doxygen_autodoc/coal/BV/AABB.h" #include "doxygen_autodoc/coal/hfield.h" #include "doxygen_autodoc/coal/shape/geometric_shapes.h" #include "doxygen_autodoc/functions.h" #endif using namespace boost::python; using namespace coal; using namespace coal::python; namespace dv = doxygen::visitor; namespace bp = boost::python; using boost::noncopyable; typedef std::vector Vec3ss; struct BVHModelBaseWrapper { typedef Eigen::Matrix RowMatrixX3; typedef Eigen::Map MapRowMatrixX3; typedef Eigen::Ref RefRowMatrixX3; static Vec3s& vertex(BVHModelBase& bvh, unsigned int i) { if (i >= bvh.num_vertices) throw std::out_of_range("index is out of range"); return (*(bvh.vertices))[i]; } static RefRowMatrixX3 vertices(BVHModelBase& bvh) { if (bvh.num_vertices > 0) return MapRowMatrixX3(bvh.vertices->data()->data(), bvh.num_vertices, 3); else return MapRowMatrixX3(NULL, bvh.num_vertices, 3); } static Triangle32 tri_indices(const BVHModelBase& bvh, unsigned int i) { if (i >= bvh.num_tris) throw std::out_of_range("index is out of range"); return (*bvh.tri_indices)[i]; } }; template void exposeBVHModel(const std::string& bvname) { typedef BVHModel BVH; const std::string type_name = "BVHModel" + bvname; class_, shared_ptr>( type_name.c_str(), doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .DEF_CLASS_FUNC(BVH, getNumBVs) .DEF_CLASS_FUNC(BVH, makeParentRelative) .DEF_CLASS_FUNC(BVHModelBase, memUsage) .def("clone", &BVH::clone, doxygen::member_func_doc(&BVH::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; } template void exposeHeightField(const std::string& bvname) { typedef HeightField Geometry; typedef typename Geometry::Base Base; typedef typename Geometry::Node Node; const std::string type_name = "HeightField" + bvname; class_, shared_ptr>( type_name.c_str(), doxygen::class_doc(), no_init) .def(dv::init>()) .def(dv::init, const HeightField&>()) .def(dv::init, Scalar, Scalar, const MatrixXs&, bp::optional>()) .DEF_CLASS_FUNC(Geometry, getXDim) .DEF_CLASS_FUNC(Geometry, getYDim) .DEF_CLASS_FUNC(Geometry, getMinHeight) .DEF_CLASS_FUNC(Geometry, getMaxHeight) .DEF_CLASS_FUNC(Geometry, getNodeType) .DEF_CLASS_FUNC(Geometry, updateHeights) .def("clone", &Geometry::clone, doxygen::member_func_doc(&Geometry::clone), return_value_policy()) .def("getXGrid", &Geometry::getXGrid, doxygen::member_func_doc(&Geometry::getXGrid), bp::return_value_policy()) .def("getYGrid", &Geometry::getYGrid, doxygen::member_func_doc(&Geometry::getYGrid), bp::return_value_policy()) .def("getHeights", &Geometry::getHeights, doxygen::member_func_doc(&Geometry::getHeights), bp::return_value_policy()) .def("getBV", (Node & (Geometry::*)(unsigned int)) & Geometry::getBV, doxygen::member_func_doc((Node & (Geometry::*)(unsigned int)) & Geometry::getBV), bp::return_internal_reference<>()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; } template struct ConvexBaseWrapper { typedef ConvexBaseTpl ConvexBaseType; typedef Eigen::Matrix RowMatrixX3; typedef Eigen::Map MapRowMatrixX3; typedef Eigen::Ref RefRowMatrixX3; typedef Eigen::Map MapVecXs; typedef Eigen::Ref RefVecXs; static Vec3s& point(const ConvexBaseType& convex, unsigned int i) { if (i >= convex.num_points) throw std::out_of_range("index is out of range"); return (*(convex.points))[i]; } static RefRowMatrixX3 points(const ConvexBaseType& convex) { return MapRowMatrixX3((*(convex.points))[0].data(), convex.num_points, 3); } static Vec3s& normal(const ConvexBaseType& convex, unsigned int i) { if (i >= convex.num_normals_and_offsets) throw std::out_of_range("index is out of range"); return (*(convex.normals))[i]; } static RefRowMatrixX3 normals(const ConvexBaseType& convex) { return MapRowMatrixX3((*(convex.normals))[0].data(), convex.num_normals_and_offsets, 3); } static Scalar offset(const ConvexBaseType& convex, unsigned int i) { if (i >= convex.num_normals_and_offsets) throw std::out_of_range("index is out of range"); return (*(convex.offsets))[i]; } static RefVecXs offsets(const ConvexBaseType& convex) { return MapVecXs(convex.offsets->data(), convex.num_normals_and_offsets, 1); } static list neighbors(const ConvexBaseType& convex, unsigned int i) { if (i >= convex.num_points) throw std::out_of_range("index is out of range"); list n; const std::vector& neighbors_ = *(convex.neighbors); for (unsigned char j = 0; j < neighbors_[i].count; ++j) { n.append(convex.neighbor(IndexType(i), j)); } return n; } static ConvexBaseType* convexHull(const Vec3ss& points, bool keepTri, const char* qhullCommand) { return ConvexBaseType::convexHull( points.data(), (unsigned int)points.size(), keepTri, qhullCommand); } }; template struct ConvexWrapper { typedef ConvexTpl Convex_t; typedef typename PolygonT::IndexType IndexType; typedef TriangleTpl TriangleType; static PolygonT polygons(const Convex_t& convex, unsigned int i) { if (i >= convex.num_polygons) throw std::out_of_range("index is out of range"); return (*convex.polygons)[i]; } static shared_ptr constructor( const Vec3ss& _points, const std::vector& _tris) { std::shared_ptr> points( new std::vector(_points.size())); std::vector& points_ = *points; for (std::size_t i = 0; i < _points.size(); ++i) points_[i] = _points[i]; std::shared_ptr> tris( new std::vector(_tris.size())); std::vector& tris_ = *tris; for (std::size_t i = 0; i < _tris.size(); ++i) tris_[i] = _tris[i]; return shared_ptr(new Convex_t(points, (unsigned int)_points.size(), tris, (unsigned int)_tris.size())); } }; template void defComputeMemoryFootprint() { doxygen::def("computeMemoryFootprint", &computeMemoryFootprint); } void exposeComputeMemoryFootprint() { defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint>(); defComputeMemoryFootprint>(); defComputeMemoryFootprint>(); } template void exposeConvexBase(const std::string& classname) { typedef ConvexBaseTpl ConvexBaseType; typedef ConvexBaseWrapper ConvexBaseTypeWrapper; class_, shared_ptr, noncopyable>(classname.c_str(), doxygen::class_doc(), no_init) .DEF_RO_CLASS_ATTRIB(ConvexBaseType, center) .DEF_RO_CLASS_ATTRIB(ConvexBaseType, num_points) .DEF_RO_CLASS_ATTRIB(ConvexBaseType, num_normals_and_offsets) .def("point", &ConvexBaseTypeWrapper::point, bp::args("self", "index"), "Retrieve the point given by its index.", bp::return_internal_reference<>()) .def("points", &ConvexBaseTypeWrapper::point, bp::args("self", "index"), "Retrieve the point given by its index.", ::coal::python::deprecated_member>()) .def("points", &ConvexBaseTypeWrapper::points, bp::args("self"), "Retrieve all the points.", bp::with_custodian_and_ward_postcall<0, 1>()) .def("normal", &ConvexBaseTypeWrapper::normal, bp::args("self", "index"), "Retrieve the normal given by its index.", bp::return_internal_reference<>()) .def("normals", &ConvexBaseTypeWrapper::normals, bp::args("self"), "Retrieve all the normals.", bp::with_custodian_and_ward_postcall<0, 1>()) .def("offset", &ConvexBaseTypeWrapper::offset, bp::args("self", "index"), "Retrieve the offset given by its index.") .def("offsets", &ConvexBaseTypeWrapper::offsets, bp::args("self"), "Retrieve all the offsets.", bp::with_custodian_and_ward_postcall<0, 1>()) .def("neighbors", &ConvexBaseTypeWrapper::neighbors) .def("convexHull", &ConvexBaseTypeWrapper::convexHull, return_value_policy()) .staticmethod("convexHull") .def("clone", &ConvexBaseType::clone, doxygen::member_func_doc(&ConvexBaseType::clone), return_value_policy()); } template void exposeConvex(const std::string& classname) { typedef ConvexTpl ConvexType; typedef ConvexBaseTpl ConvexBaseType; typedef ConvexWrapper ConvexWrapperType; class_, shared_ptr, noncopyable>(classname.c_str(), doxygen::class_doc(), no_init) .def("__init__", make_constructor(&ConvexWrapper::constructor)) .def(dv::init()) .def(dv::init()) .DEF_RO_CLASS_ATTRIB(ConvexType, num_polygons) .def("polygons", &ConvexWrapperType::polygons) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; } void exposeShapes() { class_, shared_ptr, noncopyable>("ShapeBase", doxygen::class_doc(), no_init) //.def ("getObjectType", &CollisionGeometry::getObjectType) .def(dv::member_func("setSweptSphereRadius", &ShapeBase::setSweptSphereRadius)) .def(dv::member_func("getSweptSphereRadius", &ShapeBase::getSweptSphereRadius)); class_, shared_ptr>( "Box", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Box, halfSide) .def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; class_, shared_ptr>( "Capsule", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Capsule, radius) .DEF_RW_CLASS_ATTRIB(Capsule, halfLength) .def("clone", &Capsule::clone, doxygen::member_func_doc(&Capsule::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; class_, shared_ptr>( "Cone", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Cone, radius) .DEF_RW_CLASS_ATTRIB(Cone, halfLength) .def("clone", &Cone::clone, doxygen::member_func_doc(&Cone::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; exposeConvexBase("ConvexBase16"); exposeConvexBase("ConvexBase32"); bp::scope().attr("ConvexBase") = bp::scope().attr("ConvexBase32"); exposeConvex("ConvexTriangle16"); exposeConvex("ConvexTriangle32"); bp::scope().attr("Convex") = bp::scope().attr("ConvexTriangle32"); class_, shared_ptr>( "Cylinder", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Cylinder, radius) .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength) .def("clone", &Cylinder::clone, doxygen::member_func_doc(&Cylinder::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; class_, shared_ptr>( "Halfspace", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Halfspace, n) .DEF_RW_CLASS_ATTRIB(Halfspace, d) .def("clone", &Halfspace::clone, doxygen::member_func_doc(&Halfspace::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; class_, shared_ptr>( "Plane", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Plane, n) .DEF_RW_CLASS_ATTRIB(Plane, d) .def("clone", &Plane::clone, doxygen::member_func_doc(&Plane::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; class_, shared_ptr>( "Sphere", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Sphere, radius) .def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; class_, shared_ptr>( "Ellipsoid", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) .def("clone", &Ellipsoid::clone, doxygen::member_func_doc(&Ellipsoid::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; class_, shared_ptr>( "TriangleP", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(TriangleP, a) .DEF_RW_CLASS_ATTRIB(TriangleP, b) .DEF_RW_CLASS_ATTRIB(TriangleP, c) .def("clone", &TriangleP::clone, doxygen::member_func_doc(&TriangleP::clone), return_value_policy()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; } boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) { Vec3s P, Q; Scalar distance = self.distance(other, &P, &Q); return boost::python::make_tuple(distance, P, Q); } void exposeCollisionGeometries() { enum_("BVHModelType") .value("BVH_MODEL_UNKNOWN", BVH_MODEL_UNKNOWN) .value("BVH_MODEL_TRIANGLES", BVH_MODEL_TRIANGLES) .value("BVH_MODEL_POINTCLOUD", BVH_MODEL_POINTCLOUD) .export_values(); enum_("BVHBuildState") .value("BVH_BUILD_STATE_EMPTY", BVH_BUILD_STATE_EMPTY) .value("BVH_BUILD_STATE_BEGUN", BVH_BUILD_STATE_BEGUN) .value("BVH_BUILD_STATE_PROCESSED", BVH_BUILD_STATE_PROCESSED) .value("BVH_BUILD_STATE_UPDATE_BEGUN", BVH_BUILD_STATE_UPDATE_BEGUN) .value("BVH_BUILD_STATE_UPDATED", BVH_BUILD_STATE_UPDATED) .value("BVH_BUILD_STATE_REPLACE_BEGUN", BVH_BUILD_STATE_REPLACE_BEGUN) .export_values(); if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("OBJECT_TYPE") .value("OT_UNKNOWN", OT_UNKNOWN) .value("OT_BVH", OT_BVH) .value("OT_GEOM", OT_GEOM) .value("OT_OCTREE", OT_OCTREE) .value("OT_HFIELD", OT_HFIELD) .export_values(); } if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("NODE_TYPE") .value("BV_UNKNOWN", BV_UNKNOWN) .value("BV_AABB", BV_AABB) .value("BV_OBB", BV_OBB) .value("BV_RSS", BV_RSS) .value("BV_kIOS", BV_kIOS) .value("BV_OBBRSS", BV_OBBRSS) .value("BV_KDOP16", BV_KDOP16) .value("BV_KDOP18", BV_KDOP18) .value("BV_KDOP24", BV_KDOP24) .value("GEOM_BOX", GEOM_BOX) .value("GEOM_SPHERE", GEOM_SPHERE) .value("GEOM_ELLIPSOID", GEOM_ELLIPSOID) .value("GEOM_CAPSULE", GEOM_CAPSULE) .value("GEOM_CONE", GEOM_CONE) .value("GEOM_CYLINDER", GEOM_CYLINDER) .value("GEOM_CONVEX", GEOM_CONVEX) .value("GEOM_PLANE", GEOM_PLANE) .value("GEOM_HALFSPACE", GEOM_HALFSPACE) .value("GEOM_TRIANGLE", GEOM_TRIANGLE) .value("GEOM_OCTREE", GEOM_OCTREE) .value("HF_AABB", HF_AABB) .value("HF_OBBRSS", HF_OBBRSS) .export_values(); } class_("AABB", "A class describing the AABB collision structure, which is a " "box in 3D space determined by two diagonal points", no_init) .def(init<>(bp::arg("self"), "Default constructor")) .def(init(bp::args("self", "other"), "Copy constructor")) .def(init(bp::args("self", "v"), "Creating an AABB at position v with zero size.")) .def(init(bp::args("self", "a", "b"), "Creating an AABB with two endpoints a and b.")) .def(init( bp::args("self", "core", "delta"), "Creating an AABB centered as core and is of half-dimension delta.")) .def(init(bp::args("self", "a", "b", "c"), "Creating an AABB contains three points.")) .def("contain", (bool (AABB::*)(const Vec3s&) const) & AABB::contain, bp::args("self", "p"), "Check whether the AABB contains a point p.") .def("contain", (bool (AABB::*)(const AABB&) const) & AABB::contain, bp::args("self", "other"), "Check whether the AABB contains another AABB.") .def("overlap", (bool (AABB::*)(const AABB&) const) & AABB::overlap, bp::args("self", "other"), "Check whether two AABB are overlap.") .def("overlap", (bool (AABB::*)(const AABB&, AABB&) const) & AABB::overlap, bp::args("self", "other", "overlapping_part"), "Check whether two AABB are overlaping and return the overloaping " "part if true.") .def("distance", (Scalar (AABB::*)(const AABB&) const) & AABB::distance, bp::args("self", "other"), "Distance between two AABBs.") // .def("distance", // AABB_distance_proxy, // bp::args("self","other"), // "Distance between two AABBs.") .add_property( "min_", bp::make_function( +[](AABB& self) -> Vec3s& { return self.min_; }, bp::return_internal_reference<>()), bp::make_function( +[](AABB& self, const Vec3s& min_) -> void { self.min_ = min_; }), "The min point in the AABB.") .add_property( "max_", bp::make_function( +[](AABB& self) -> Vec3s& { return self.max_; }, bp::return_internal_reference<>()), bp::make_function( +[](AABB& self, const Vec3s& max_) -> void { self.max_ = max_; }), "The max point in the AABB.") .def(bp::self == bp::self) .def(bp::self != bp::self) .def(bp::self + bp::self) .def(bp::self += bp::self) .def(bp::self += Vec3s()) .def("size", &AABB::volume, bp::arg("self"), "Size of the AABB.") .def("center", &AABB::center, bp::arg("self"), "Center of the AABB.") .def("width", &AABB::width, bp::arg("self"), "Width of the AABB.") .def("height", &AABB::height, bp::arg("self"), "Height of the AABB.") .def("depth", &AABB::depth, bp::arg("self"), "Depth of the AABB.") .def("volume", &AABB::volume, bp::arg("self"), "Volume of the AABB.") .def("expand", static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), bp::return_internal_reference<>()) .def("expand", static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), bp::return_internal_reference<>()) .def("expand", static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), bp::return_internal_reference<>()) .def_pickle(PickleObject()) .def(SerializableVisitor()) #if EIGENPY_VERSION_AT_LEAST(3, 8, 0) .def(eigenpy::IdVisitor()) #endif ; def("translate", (AABB (*)(const AABB&, const Vec3s&))&translate, bp::args("aabb", "t"), "Translate the center of AABB by t"); def("rotate", (AABB (*)(const AABB&, const Matrix3s&))&rotate, bp::args("aabb", "R"), "Rotate the AABB object by R"); if (!eigenpy::register_symbolic_link_to_registered_type< CollisionGeometry>()) { class_( "CollisionGeometry", no_init) .def("getObjectType", &CollisionGeometry::getObjectType) .def("getNodeType", &CollisionGeometry::getNodeType) .def("computeLocalAABB", &CollisionGeometry::computeLocalAABB) .def("computeCOM", &CollisionGeometry::computeCOM) .def("computeMomentofInertia", &CollisionGeometry::computeMomentofInertia) .def("computeVolume", &CollisionGeometry::computeVolume) .def("computeMomentofInertiaRelatedToCOM", &CollisionGeometry::computeMomentofInertiaRelatedToCOM) .def_readwrite("aabb_radius", &CollisionGeometry::aabb_radius, "AABB radius.") .def_readwrite("aabb_center", &CollisionGeometry::aabb_center, "AABB center in local coordinate.") .def_readwrite("aabb_local", &CollisionGeometry::aabb_local, "AABB in local coordinate, used for tight AABB when " "only translation transform.") .def("isOccupied", &CollisionGeometry::isOccupied, bp::arg("self"), "Whether the object is completely occupied.") .def("isFree", &CollisionGeometry::isFree, bp::arg("self"), "Whether the object is completely free.") .def("isUncertain", &CollisionGeometry::isUncertain, bp::arg("self"), "Whether the object has some uncertainty.") .def_readwrite("cost_density", &CollisionGeometry::cost_density, "Collision cost for unit volume.") .def_readwrite("threshold_occupied", &CollisionGeometry::threshold_occupied, "Threshold for occupied ( >= is occupied).") .def_readwrite("threshold_free", &CollisionGeometry::threshold_free, "Threshold for free (<= is free).") .def(self == self) .def(self != self); } exposeShapes(); class_, BVHModelPtr_t, noncopyable>( "BVHModelBase", no_init) .def("vertex", &BVHModelBaseWrapper::vertex, bp::args("self", "index"), "Retrieve the vertex given by its index.", bp::return_internal_reference<>()) .def("vertices", &BVHModelBaseWrapper::vertex, bp::args("self", "index"), "Retrieve the vertex given by its index.", ::coal::python::deprecated_member>()) .def("vertices", &BVHModelBaseWrapper::vertices, bp::args("self"), "Retrieve all the vertices.", bp::with_custodian_and_ward_postcall<0, 1>()) // .add_property ("vertices", // bp::make_function(&BVHModelBaseWrapper::vertices,bp::with_custodian_and_ward_postcall<0,1>()), // "Vertices of the BVH.") .def("tri_indices", &BVHModelBaseWrapper::tri_indices, bp::args("self", "index"), "Retrieve the triangle given by its index.") .def_readonly("num_vertices", &BVHModelBase::num_vertices) .def_readonly("num_tris", &BVHModelBase::num_tris) .def_readonly("build_state", &BVHModelBase::build_state) .def_readonly("convex", &BVHModelBase::convex) .DEF_CLASS_FUNC(BVHModelBase, buildConvexRepresentation) .DEF_CLASS_FUNC(BVHModelBase, buildConvexHull) // Expose function to build a BVH .def(dv::member_func("beginModel", &BVHModelBase::beginModel)) .def(dv::member_func("addVertex", &BVHModelBase::addVertex)) .def(dv::member_func("addVertices", &BVHModelBase::addVertices)) .def(dv::member_func("addTriangle", &BVHModelBase::addTriangle)) .def(dv::member_func("addTriangles", &BVHModelBase::addTriangles)) .def(dv::member_func&)>( "addSubModel", &BVHModelBase::addSubModel)) .def(dv::member_func( "addSubModel", &BVHModelBase::addSubModel)) .def(dv::member_func("endModel", &BVHModelBase::endModel)) // Expose function to replace a BVH .def(dv::member_func("beginReplaceModel", &BVHModelBase::beginReplaceModel)) .def(dv::member_func("replaceVertex", &BVHModelBase::replaceVertex)) .def(dv::member_func("replaceTriangle", &BVHModelBase::replaceTriangle)) .def(dv::member_func("replaceSubModel", &BVHModelBase::replaceSubModel)) .def(dv::member_func("endReplaceModel", &BVHModelBase::endReplaceModel)) .def(dv::member_func("getModelType", &BVHModelBase::getModelType)); exposeBVHModel("OBB"); exposeBVHModel("OBBRSS"); exposeHeightField("OBBRSS"); exposeHeightField("AABB"); exposeComputeMemoryFootprint(); } void exposeCollisionObject() { namespace bp = boost::python; if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("CollisionObject", no_init) .def(dv::init>()) .def(dv::init>()) .def(dv::init>()) .DEF_CLASS_FUNC(CollisionObject, getObjectType) .DEF_CLASS_FUNC(CollisionObject, getNodeType) .DEF_CLASS_FUNC(CollisionObject, computeAABB) .def(dv::member_func("getAABB", static_cast( &CollisionObject::getAABB), bp::return_internal_reference<>())) .DEF_CLASS_FUNC2(CollisionObject, getTranslation, bp::return_value_policy()) .DEF_CLASS_FUNC(CollisionObject, setTranslation) .DEF_CLASS_FUNC2(CollisionObject, getRotation, bp::return_value_policy()) .DEF_CLASS_FUNC(CollisionObject, setRotation) .DEF_CLASS_FUNC2(CollisionObject, getTransform, bp::return_value_policy()) .def(dv::member_func( "setTransform", static_cast( &CollisionObject::setTransform))) .DEF_CLASS_FUNC(CollisionObject, isIdentityTransform) .DEF_CLASS_FUNC(CollisionObject, setIdentityTransform) .DEF_CLASS_FUNC2(CollisionObject, setCollisionGeometry, (bp::with_custodian_and_ward_postcall<1, 2>())) .def(dv::member_func( "collisionGeometry", static_cast( &CollisionObject::collisionGeometry), bp::return_value_policy())); } } ================================================ FILE: python/collision.cc ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2019-2021 CNRS-LAAS, INRIA // Author: Joseph Mirabel // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS. nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include "coal/fwd.hh" COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #include "coal/collision.h" #include "coal/serialization/collision_data.h" COAL_COMPILER_DIAGNOSTIC_POP #include "coal.hh" #include "deprecation.hh" #include "serializable.hh" #include "printable.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/coal/collision_data.h" #endif #include "../doc/python/doxygen.hh" #include "../doc/python/doxygen-boost.hh" using namespace boost::python; using namespace coal; using namespace coal::python; namespace dv = doxygen::visitor; template const CollisionGeometry* geto(const Contact& c) { return index == 1 ? c.o1 : c.o2; } struct ContactWrapper { static Vec3s getNearestPoint1(const Contact& contact) { return contact.nearest_points[0]; } static Vec3s getNearestPoint2(const Contact& contact) { return contact.nearest_points[1]; } }; void exposeCollisionAPI() { if (!eigenpy::register_symbolic_link_to_registered_type< CollisionRequestFlag>()) { enum_("CollisionRequestFlag") .value("CONTACT", CONTACT) .value("DISTANCE_LOWER_BOUND", DISTANCE_LOWER_BOUND) .value("NO_REQUEST", NO_REQUEST) .export_values(); } if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("CPUTimes", no_init) .def_readonly("wall", &CPUTimes::wall, "wall time in micro seconds (us)") .def_readonly("user", &CPUTimes::user, "user time in micro seconds (us)") .def_readonly("system", &CPUTimes::system, "system time in micro seconds (us)") .def("clear", &CPUTimes::clear, arg("self"), "Reset the time values."); } COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("QueryRequest", doxygen::class_doc(), no_init) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_tolerance) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_max_iterations) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_variant) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_convergence_criterion) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_convergence_criterion_type) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_initial_guess) .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_cached_gjk_guess) .add_property( "enable_cached_gjk_guess", bp::make_function( +[](QueryRequest& self) -> bool { return self.enable_cached_gjk_guess; }, deprecated_warning_policy<>( "enable_cached_gjk_guess has been marked as deprecated and " "will be removed in a future release.\n" "Please use gjk_initial_guess instead.")), bp::make_function( +[](QueryRequest& self, const bool value) -> void { self.enable_cached_gjk_guess = value; }, deprecated_warning_policy<>( "enable_cached_gjk_guess has been marked as deprecated and " "will be removed in a future release.\n" "Please use gjk_initial_guess instead.")), doxygen::class_attrib_doc("enable_cached_gjk_guess")) .DEF_RW_CLASS_ATTRIB(QueryRequest, cached_gjk_guess) .DEF_RW_CLASS_ATTRIB(QueryRequest, cached_support_func_guess) .DEF_RW_CLASS_ATTRIB(QueryRequest, epa_max_iterations) .DEF_RW_CLASS_ATTRIB(QueryRequest, epa_tolerance) .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_timings) .DEF_CLASS_FUNC(QueryRequest, updateGuess); } COAL_COMPILER_DIAGNOSTIC_POP COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "CollisionRequest", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(CollisionRequest, num_max_contacts) .DEF_RW_CLASS_ATTRIB(CollisionRequest, enable_contact) .add_property( "enable_distance_lower_bound", bp::make_function( +[](CollisionRequest& self) -> bool { return self.enable_distance_lower_bound; }, deprecated_warning_policy<>( "enable_distance_lower_bound has been marked as " "deprecated. " "A lower bound on distance is always computed.\n")), bp::make_function( +[](CollisionRequest& self, const bool value) -> void { self.enable_distance_lower_bound = value; }, deprecated_warning_policy<>( "enable_distance_lower_bound has been marked as " "deprecated. " "A lower bound on distance is always computed.\n")), doxygen::class_attrib_doc( "enable_distance_lower_bound")) .DEF_RW_CLASS_ATTRIB(CollisionRequest, security_margin) .DEF_RW_CLASS_ATTRIB(CollisionRequest, break_distance) .DEF_RW_CLASS_ATTRIB(CollisionRequest, distance_upper_bound) .def(SerializableVisitor()) .def(PrintableVisitor()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { class_ >("StdVec_CollisionRequest") .def(vector_indexing_suite >()); } COAL_COMPILER_DIAGNOSTIC_POP if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("Contact", doxygen::class_doc(), init<>(arg("self"), "Default constructor")) .def(dv::init()) .def(dv::init()) .add_property( "o1", make_function(&geto<1>, return_value_policy()), doxygen::class_attrib_doc("o1")) .add_property( "o2", make_function(&geto<2>, return_value_policy()), doxygen::class_attrib_doc("o2")) .def("getNearestPoint1", &ContactWrapper::getNearestPoint1, doxygen::class_attrib_doc("nearest_points")) .def("getNearestPoint2", &ContactWrapper::getNearestPoint2, doxygen::class_attrib_doc("nearest_points")) .DEF_RW_CLASS_ATTRIB(Contact, b1) .DEF_RW_CLASS_ATTRIB(Contact, b2) .DEF_RW_CLASS_ATTRIB(Contact, normal) .DEF_RW_CLASS_ATTRIB(Contact, nearest_points) .DEF_RW_CLASS_ATTRIB(Contact, pos) .DEF_RW_CLASS_ATTRIB(Contact, penetration_depth) .def(self == self) .def(self != self) .def(SerializableVisitor()) .def(PrintableVisitor()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { class_ >("StdVec_Contact") .def(vector_indexing_suite >()); } if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("QueryResult", doxygen::class_doc(), no_init) .DEF_RW_CLASS_ATTRIB(QueryResult, cached_gjk_guess) .DEF_RW_CLASS_ATTRIB(QueryResult, cached_support_func_guess) .DEF_RW_CLASS_ATTRIB(QueryResult, timings); } if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "CollisionResult", doxygen::class_doc(), no_init) .def(dv::init()) .DEF_CLASS_FUNC(CollisionResult, isCollision) .DEF_CLASS_FUNC(CollisionResult, numContacts) .DEF_CLASS_FUNC(CollisionResult, addContact) .DEF_CLASS_FUNC(CollisionResult, clear) .DEF_CLASS_FUNC2(CollisionResult, getContact, return_value_policy()) .def(dv::member_func( "getContacts", static_cast&) const>( &CollisionResult::getContacts))) .def("getContacts", static_cast& (CollisionResult::*)() const>(&CollisionResult::getContacts), doxygen::member_func_doc( static_cast& (CollisionResult::*)() const>(&CollisionResult::getContacts)), return_internal_reference<>()) .DEF_RW_CLASS_ATTRIB(CollisionResult, distance_lower_bound) .def(SerializableVisitor()) .def(PrintableVisitor()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { class_ >("StdVec_CollisionResult") .def(vector_indexing_suite >()); } doxygen::def("collide", static_cast(&collide)); doxygen::def( "collide", static_cast( &collide)); class_("ComputeCollision", doxygen::class_doc(), no_init) .def(dv::init()) .def("__call__", static_cast(&ComputeCollision::operator())); } ================================================ FILE: python/contact_patch.cc ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2024 INRIA // Author: Louis Montaut // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of INRIA nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include "coal/fwd.hh" #include "coal/contact_patch.h" #include "coal/serialization/contact_patch.h" #include "coal.hh" #include "deprecation.hh" #include "printable.hh" #include "serializable.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/coal/collision_data.h" #endif #include "../doc/python/doxygen.hh" #include "../doc/python/doxygen-boost.hh" using namespace boost::python; using namespace coal; using namespace coal::python; namespace dv = doxygen::visitor; void exposeContactPatchAPI() { if (!eigenpy::register_symbolic_link_to_registered_type< ContactPatch::PatchDirection>()) { enum_("ContactPatchDirection") .value("DEFAULT", ContactPatch::PatchDirection::DEFAULT) .value("INVERTED", ContactPatch::PatchDirection::INVERTED) .export_values(); } if (!eigenpy::register_symbolic_link_to_registered_type()) { class_( "ContactPatch", doxygen::class_doc(), init>((arg("self"), arg("preallocated_size")), "ContactPatch constructor.")) .DEF_RW_CLASS_ATTRIB(ContactPatch, tf) .DEF_RW_CLASS_ATTRIB(ContactPatch, direction) .DEF_RW_CLASS_ATTRIB(ContactPatch, penetration_depth) .DEF_CLASS_FUNC(ContactPatch, size) .DEF_CLASS_FUNC(ContactPatch, getNormal) .DEF_CLASS_FUNC(ContactPatch, addPoint) .DEF_CLASS_FUNC(ContactPatch, getPoint) .DEF_CLASS_FUNC(ContactPatch, getPointShape1) .DEF_CLASS_FUNC(ContactPatch, getPointShape2) .DEF_CLASS_FUNC(ContactPatch, clear) .DEF_CLASS_FUNC(ContactPatch, isSame) .def(SerializableVisitor()) .def(PrintableVisitor()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector>()) { class_>("StdVec_ContactPatch") .def(vector_indexing_suite>()); } if (!eigenpy::register_symbolic_link_to_registered_type< ContactPatchRequest>()) { class_( "ContactPatchRequest", doxygen::class_doc(), init>( (arg("self"), arg("max_num_patch"), arg("num_samples_curved_shapes"), arg("patch_tolerance")), "ContactPatchRequest constructor.")) .def(dv::init>()) .DEF_RW_CLASS_ATTRIB(ContactPatchRequest, max_num_patch) .DEF_CLASS_FUNC(ContactPatchRequest, getNumSamplesCurvedShapes) .DEF_CLASS_FUNC(ContactPatchRequest, setNumSamplesCurvedShapes) .DEF_CLASS_FUNC(ContactPatchRequest, getPatchTolerance) .DEF_CLASS_FUNC(ContactPatchRequest, setPatchTolerance) .def(SerializableVisitor()) .def(PrintableVisitor()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector>()) { class_>("StdVec_ContactPatchRequest") .def(vector_indexing_suite>()); } if (!eigenpy::register_symbolic_link_to_registered_type< ContactPatchResult>()) { class_("ContactPatchResult", doxygen::class_doc(), init<>(arg("self"), "Default constructor.")) .def(dv::init()) .DEF_CLASS_FUNC(ContactPatchResult, numContactPatches) .DEF_CLASS_FUNC2(ContactPatchResult, getUnusedContactPatch, return_value_policy()) .DEF_CLASS_FUNC2(ContactPatchResult, getContactPatch, return_value_policy()) .DEF_CLASS_FUNC(ContactPatchResult, clear) .DEF_CLASS_FUNC(ContactPatchResult, set) .DEF_CLASS_FUNC(ContactPatchResult, check) .def(SerializableVisitor()) .def(PrintableVisitor()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector>()) { class_>("StdVec_ContactPatchResult") .def(vector_indexing_suite>()); } doxygen::def( "computeContactPatch", static_cast(&computeContactPatch)); doxygen::def( "computeContactPatch", static_cast(&computeContactPatch)); if (!eigenpy::register_symbolic_link_to_registered_type< ComputeContactPatch>()) { class_("ComputeContactPatch", doxygen::class_doc(), no_init) .def(dv::init()) .def("__call__", static_cast( &ComputeContactPatch::operator())); } if (!eigenpy::register_symbolic_link_to_registered_type< ContactPatchSimplifierGreedy>()) { class_("ContactPatchSimplifierGreedy", "Greedy contact patch simplifier.", init<>()) .def("compute", &ContactPatchSimplifierGreedy::compute, (arg("self"), arg("patch_in"), arg("target_vertices"), arg("patch_out")), "Simplify `patch_in` into `patch_out` using the greedy heuristic.") .def("simplify", &ContactPatchSimplifierGreedy::simplify, (arg("self"), arg("patch"), arg("target_vertices"))); } if (!eigenpy::register_symbolic_link_to_registered_type< ContactPatchSimplifierMaxArea>()) { class_( "ContactPatchSimplifierMaxArea", "Max-area dynamic-programming contact patch simplifier.", init<>()) .def("compute", &ContactPatchSimplifierMaxArea::compute, (arg("self"), arg("patch_in"), arg("target_vertices"), arg("patch_out")), "Simplify `patch_in` into `patch_out` using the max-area DP.") .def("simplify", &ContactPatchSimplifierMaxArea::simplify, (arg("self"), arg("patch"), arg("target_vertices"))); } } ================================================ FILE: python/deprecation.hh ================================================ // // Copyright (c) 2020-2021 INRIA // #ifndef COAL_PYTHON_UTILS_DEPRECATION_H #define COAL_PYTHON_UTILS_DEPRECATION_H #include #include #include namespace coal { namespace python { template struct deprecated_warning_policy : Policy { deprecated_warning_policy(const std::string& warning_message = "") : Policy(), m_warning_message(warning_message) {} template bool precall(ArgumentPackage const& args) const { PyErr_WarnEx(PyExc_DeprecationWarning, m_warning_message.c_str(), 1); return static_cast(this)->precall(args); } typedef typename Policy::result_converter result_converter; typedef typename Policy::argument_package argument_package; protected: const std::string m_warning_message; }; template struct deprecated_member : deprecated_warning_policy { deprecated_member(const std::string& warning_message = "This class member has been marked as deprecated and " "will be removed in a future release.") : deprecated_warning_policy(warning_message) {} }; template struct deprecated_function : deprecated_warning_policy { deprecated_function(const std::string& warning_message = "This function has been marked as deprecated and " "will be removed in a future release.") : deprecated_warning_policy(warning_message) {} }; } // namespace python } // namespace coal #endif // ifndef COAL_PYTHON_UTILS_DEPRECATION_H ================================================ FILE: python/distance.cc ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2019 CNRS-LAAS INRIA // Author: Joseph Mirabel // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS. nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include "coal.hh" COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #include "coal/fwd.hh" #include "coal/distance.h" #include "coal/serialization/collision_data.h" #include "deprecation.hh" COAL_COMPILER_DIAGNOSTIC_POP #include "serializable.hh" #include "printable.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/coal/collision_data.h" #endif using namespace boost::python; using namespace coal; using namespace coal::python; namespace dv = doxygen::visitor; struct DistanceResultWrapper { static Vec3s getNearestPoint1(const DistanceResult& res) { return res.nearest_points[0]; } static Vec3s getNearestPoint2(const DistanceResult& res) { return res.nearest_points[1]; } }; void exposeDistanceAPI() { COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "DistanceRequest", doxygen::class_doc(), init >( (arg("self"), arg("enable_nearest_points"), arg("rel_err"), arg("abs_err")), "Constructor")) .add_property( "enable_nearest_points", bp::make_function( +[](DistanceRequest& self) -> bool { return self.enable_nearest_points; }, deprecated_warning_policy<>( "enable_nearest_points has been marked as deprecated. " "Nearest points are always computed when computing " "distance. They are the points of the shapes that achieve " "a distance of " "DistanceResult::min_distance.\n" "Use `enable_signed_distance` if you want to compute a " "signed minimum " "distance (and thus its corresponding nearest points).")), bp::make_function( +[](DistanceRequest& self, const bool value) -> void { self.enable_nearest_points = value; }, deprecated_warning_policy<>( "enable_nearest_points has been marked as deprecated. " "Nearest points are always computed when computing " "distance. They are the points of the shapes that achieve " "a distance of " "DistanceResult::min_distance.\n" "Use `enable_signed_distance` if you want to compute a " "signed minimum " "distance (and thus its corresponding nearest points).")), doxygen::class_attrib_doc("enable_nearest_points")) .DEF_RW_CLASS_ATTRIB(DistanceRequest, enable_signed_distance) .DEF_RW_CLASS_ATTRIB(DistanceRequest, rel_err) .DEF_RW_CLASS_ATTRIB(DistanceRequest, abs_err) .def(SerializableVisitor()) .def(PrintableVisitor()); } COAL_COMPILER_DIAGNOSTIC_POP if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { class_ >("StdVec_DistanceRequest") .def(vector_indexing_suite >()); } if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "DistanceResult", doxygen::class_doc(), no_init) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(DistanceResult, min_distance) .DEF_RW_CLASS_ATTRIB(DistanceResult, normal) //.def_readwrite ("nearest_points", &DistanceResult::nearest_points) .def("getNearestPoint1", &DistanceResultWrapper::getNearestPoint1, doxygen::class_attrib_doc("nearest_points")) .def("getNearestPoint2", &DistanceResultWrapper::getNearestPoint2, doxygen::class_attrib_doc("nearest_points")) .DEF_RO_CLASS_ATTRIB(DistanceResult, nearest_points) .DEF_RO_CLASS_ATTRIB(DistanceResult, o1) .DEF_RO_CLASS_ATTRIB(DistanceResult, o2) .DEF_RW_CLASS_ATTRIB(DistanceResult, b1) .DEF_RW_CLASS_ATTRIB(DistanceResult, b2) .def("clear", &DistanceResult::clear, doxygen::member_func_doc(&DistanceResult::clear)) .def(SerializableVisitor()) .def(PrintableVisitor()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { class_ >("StdVec_DistanceResult") .def(vector_indexing_suite >()); } doxygen::def( "distance", static_cast( &distance)); doxygen::def( "distance", static_cast( &distance)); class_("ComputeDistance", doxygen::class_doc(), no_init) .def(dv::init()) .def("__call__", static_cast(&ComputeDistance::operator())); } ================================================ FILE: python/fwd.hh ================================================ // // Copyright (c) 2022 CNRS INRIA // #ifndef COAL_PYTHON_FWD_HH #define COAL_PYTHON_FWD_HH #include "coal/fwd.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC namespace doxygen { using coal::shared_ptr; } #endif // Silence a warning about a deprecated use of boost bind by boost python // at least fo boost 1.73 to 1.75 // ref. https://github.com/stack-of-tasks/tsid/issues/128 #define BOOST_BIND_GLOBAL_PLACEHOLDERS #include #include #undef BOOST_BIND_GLOBAL_PLACEHOLDERS #include "../doc/python/doxygen.hh" #include "../doc/python/doxygen-boost.hh" namespace bp = boost::python; namespace dv = doxygen::visitor; #define DEF_RW_CLASS_ATTRIB(CLASS, ATTRIB) \ def_readwrite(#ATTRIB, &CLASS::ATTRIB, \ doxygen::class_attrib_doc(#ATTRIB)) #define DEF_RO_CLASS_ATTRIB(CLASS, ATTRIB) \ def_readonly(#ATTRIB, &CLASS::ATTRIB, \ doxygen::class_attrib_doc(#ATTRIB)) #define DEF_CLASS_FUNC(CLASS, ATTRIB) \ def(dv::member_func(#ATTRIB, &CLASS::ATTRIB)) #define DEF_CLASS_FUNC2(CLASS, ATTRIB, policy) \ def(#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB), policy) #endif // ifndef COAL_PYTHON_FWD_HH ================================================ FILE: python/gjk.cc ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2020 CNRS-LAAS // Author: Joseph Mirabel // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS. nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include "coal.hh" #include "coal/fwd.hh" #include "coal/narrowphase/gjk.h" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/coal/narrowphase/gjk.h" #endif using namespace boost::python; using namespace coal; using coal::details::EPA; using coal::details::GJK; using coal::details::MinkowskiDiff; using coal::details::SupportOptions; struct MinkowskiDiffWrapper { static void support0(MinkowskiDiff& self, const Vec3s& dir, int& hint, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.support0(dir, hint); } else { self.support0(dir, hint); } } static void support1(MinkowskiDiff& self, const Vec3s& dir, int& hint, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.support1(dir, hint); } else { self.support1(dir, hint); } } static void set(MinkowskiDiff& self, const ShapeBase* shape0, const ShapeBase* shape1, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.set(shape0, shape1); } else { self.set(shape0, shape1); } } static void set(MinkowskiDiff& self, const ShapeBase* shape0, const ShapeBase* shape1, const Transform3s& tf0, const Transform3s& tf1, bool compute_swept_sphere_supports = false) { if (compute_swept_sphere_supports) { self.set(shape0, shape1, tf0, tf1); } else { self.set(shape0, shape1, tf0, tf1); } } }; void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("GJKStatus") .value("Failed", GJK::Status::Failed) .value("DidNotRun", GJK::Status::DidNotRun) .value("NoCollision", GJK::Status::NoCollision) .value("NoCollisionEarlyStopped", GJK::Status::NoCollisionEarlyStopped) .value("CollisionWithPenetrationInformation", GJK::Status::CollisionWithPenetrationInformation) .value("Collision", GJK::Status::Collision) .export_values(); } if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("MinkowskiDiff", doxygen::class_doc(), no_init) .def(doxygen::visitor::init()) .def("set", static_cast( &MinkowskiDiffWrapper::set), doxygen::member_func_doc(static_cast( &MinkowskiDiff::set))) .def("set", static_cast( &MinkowskiDiffWrapper::set), doxygen::member_func_doc( static_cast( &MinkowskiDiff::set))) .def("support0", &MinkowskiDiffWrapper::support0, doxygen::member_func_doc( &MinkowskiDiff::support0)) .def("support1", &MinkowskiDiffWrapper::support1, doxygen::member_func_doc( &MinkowskiDiff::support1)) .def("support", &MinkowskiDiff::support, doxygen::member_func_doc(&MinkowskiDiff::support)) .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, swept_sphere_radius) .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, normalize_support_direction); } if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("GJKVariant") .value("DefaultGJK", GJKVariant::DefaultGJK) .value("PolyakAcceleration", GJKVariant::PolyakAcceleration) .value("NesterovAcceleration", GJKVariant::NesterovAcceleration) .export_values(); } if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("GJKInitialGuess") .value("DefaultGuess", GJKInitialGuess::DefaultGuess) .value("CachedGuess", GJKInitialGuess::CachedGuess) .value("BoundingVolumeGuess", GJKInitialGuess::BoundingVolumeGuess) .export_values(); } if (!eigenpy::register_symbolic_link_to_registered_type< GJKConvergenceCriterion>()) { enum_("GJKConvergenceCriterion") .value("Default", GJKConvergenceCriterion::Default) .value("DualityGap", GJKConvergenceCriterion::DualityGap) .value("Hybrid", GJKConvergenceCriterion::Hybrid) .export_values(); } if (!eigenpy::register_symbolic_link_to_registered_type< GJKConvergenceCriterionType>()) { enum_("GJKConvergenceCriterionType") .value("Absolute", GJKConvergenceCriterionType::Absolute) .value("Relative", GJKConvergenceCriterionType::Relative) .export_values(); } if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("GJK", doxygen::class_doc(), no_init) .def(doxygen::visitor::init()) .DEF_RW_CLASS_ATTRIB(GJK, distance) .DEF_RW_CLASS_ATTRIB(GJK, ray) .DEF_RW_CLASS_ATTRIB(GJK, support_hint) .DEF_RW_CLASS_ATTRIB(GJK, gjk_variant) .DEF_RW_CLASS_ATTRIB(GJK, convergence_criterion) .DEF_RW_CLASS_ATTRIB(GJK, convergence_criterion_type) .DEF_CLASS_FUNC(GJK, reset) .DEF_CLASS_FUNC(GJK, evaluate) .DEF_CLASS_FUNC(GJK, getTolerance) .DEF_CLASS_FUNC(GJK, getNumMaxIterations) .DEF_CLASS_FUNC(GJK, getNumIterations) .DEF_CLASS_FUNC(GJK, getNumIterationsMomentumStopped) .DEF_CLASS_FUNC(GJK, hasClosestPoints) .DEF_CLASS_FUNC(GJK, getWitnessPointsAndNormal) .DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak) .DEF_CLASS_FUNC(GJK, getGuessFromSimplex); } } ================================================ FILE: python/hppfcl/__init__.py ================================================ import warnings warnings.warn( "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning ) from coal import Transform3s as Transform3f # noqa from coal import * # noqa from coal import __raw_version__, __version__ # noqa ================================================ FILE: python/hppfcl/viewer.py ================================================ import warnings warnings.warn( "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning ) from coal.viewer import * # noqa ================================================ FILE: python/math.cc ================================================ // // Software License Agreement (BSD License) // // Copyright (c) 2019 CNRS-LAAS INRIA // Author: Joseph Mirabel // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above // copyright notice, this list of conditions and the following // disclaimer in the documentation and/or other materials provided // with the distribution. // * Neither the name of CNRS-LAAS. nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include #include "coal/fwd.hh" #include "coal/math/transform.h" #include "coal/serialization/transform.h" #include "coal.hh" #include "pickle.hh" #include "serializable.hh" #include "printable.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/coal/math/transform.h" #endif using namespace boost::python; using namespace coal; using namespace coal::python; namespace dv = doxygen::visitor; template struct TriangleWrapper { static typename TriangleTpl::IndexType getitem( const TriangleTpl& t, int i) { if (i >= 3 || i <= -3) PyErr_SetString(PyExc_IndexError, "Index out of range"); return t[static_cast::IndexType>(i % 3)]; } static void setitem(TriangleTpl& t, int i, typename TriangleTpl::IndexType v) { if (i >= 3 || i <= -3) PyErr_SetString(PyExc_IndexError, "Index out of range"); t[static_cast::IndexType>(i % 3)] = v; } }; template void exposeTriangle(const std::string& classname) { typedef TriangleTpl TriangleType; class_(classname.c_str(), no_init) .def(dv::init()) .def(dv::init()) .def("__getitem__", &TriangleWrapper::getitem) .def("__setitem__", &TriangleWrapper::setitem) .def(dv::member_func("set", &TriangleType::set)) .def(dv::member_func("size", &TriangleType::size)) .staticmethod("size") .def(self == self); } void exposeMaths() { eigenpy::enableEigenPy(); if (!eigenpy::register_symbolic_link_to_registered_type()) eigenpy::exposeQuaternion(); if (!eigenpy::register_symbolic_link_to_registered_type()) eigenpy::exposeAngleAxis(); eigenpy::enableEigenPySpecific(); eigenpy::enableEigenPySpecific(); class_("Transform3s", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) .add_property( "translation", bp::make_function( +[](Transform3s& self) -> Vec3s& { return self.translation(); }, bp::return_internal_reference<>()), bp::make_function(+[](Transform3s& self, const Vec3s& t) -> void { self.translation() = t; }), doxygen::class_attrib_doc("translation")) .add_property( "rotation", bp::make_function( +[](Transform3s& self) -> Matrix3s& { return self.rotation(); }, bp::return_internal_reference<>()), bp::make_function(+[](Transform3s& self, const Matrix3s& R) -> void { self.rotation() = R; }), doxygen::class_attrib_doc("rotation")) .def(dv::member_func("getQuatRotation", &Transform3s::getQuatRotation)) .def("getTranslation", &Transform3s::getTranslation, doxygen::member_func_doc(&Transform3s::getTranslation), return_value_policy()) .def("getRotation", &Transform3s::getRotation, return_value_policy()) .def("isIdentity", &Transform3s::isIdentity, (bp::arg("self"), bp::arg("prec") = Eigen::NumTraits::dummy_precision()), doxygen::member_func_doc(&Transform3s::isIdentity)) .def(dv::member_func("setQuatRotation", &Transform3s::setQuatRotation)) .def("setTranslation", &Transform3s::setTranslation) .def("setRotation", &Transform3s::setRotation) .def(dv::member_func("setTransform", &Transform3s::setTransform)) .def(dv::member_func( "setTransform", static_cast( &Transform3s::setTransform))) .def(dv::member_func("setIdentity", &Transform3s::setIdentity)) .def(dv::member_func("Identity", &Transform3s::Identity)) .staticmethod("Identity") .def(dv::member_func("setRandom", &Transform3s::setRandom)) .def(dv::member_func("Random", &Transform3s::Random)) .staticmethod("Random") .def(dv::member_func("transform", &Transform3s::transform)) .def("inverseInPlace", &Transform3s::inverseInPlace, return_internal_reference<>(), doxygen::member_func_doc(&Transform3s::inverseInPlace)) .def(dv::member_func("inverse", &Transform3s::inverse)) .def(dv::member_func("inverseTimes", &Transform3s::inverseTimes)) .def(self * self) .def(self *= self) .def(self == self) .def(self != self) .def_pickle(PickleObject()) .def(SerializableVisitor()) .def(PrintableVisitor()); exposeTriangle("Triangle32"); bp::scope().attr("Triangle") = bp::scope().attr("Triangle32"); exposeTriangle("Triangle16"); if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { class_ >("StdVec_Vec3s") .def(vector_indexing_suite >()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { class_ >("StdVec_Triangle32") .def(vector_indexing_suite >()); bp::scope().attr("StdVec_Triangle") = bp::scope().attr("StdVec_Triangle32"); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { class_ >("StdVec_Triangle16") .def(vector_indexing_suite >()); } } ================================================ FILE: python/octree.cc ================================================ #include "coal.hh" #include #include "coal/fwd.hh" #include "coal/octree.h" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/coal/octree.h" #endif bp::object toPyBytes(std::vector& bytes) { #if PY_MAJOR_VERSION == 2 PyObject* py_buf = PyBuffer_FromMemory((char*)bytes.data(), Py_ssize_t(bytes.size())); return bp::object(bp::handle<>(py_buf)); #else PyObject* py_buf = PyMemoryView_FromMemory( (char*)bytes.data(), Py_ssize_t(bytes.size()), PyBUF_READ); return bp::object(bp::handle<>(PyBytes_FromObject(py_buf))); #endif } bp::object tobytes(const coal::OcTree& self) { std::vector bytes = self.tobytes(); return toPyBytes(bytes); } void exposeOctree() { using namespace coal; namespace bp = boost::python; namespace dv = doxygen::visitor; bp::class_, shared_ptr >( "OcTree", doxygen::class_doc(), bp::no_init) .def(dv::init()) .def("clone", &OcTree::clone, doxygen::member_func_doc(&OcTree::clone), bp::return_value_policy()) .def(dv::member_func("getTreeDepth", &OcTree::getTreeDepth)) .def(dv::member_func("size", &OcTree::size)) .def(dv::member_func("getResolution", &OcTree::getResolution)) .def(dv::member_func("getOccupancyThres", &OcTree::getOccupancyThres)) .def(dv::member_func("getFreeThres", &OcTree::getFreeThres)) .def(dv::member_func("getDefaultOccupancy", &OcTree::getDefaultOccupancy)) .def(dv::member_func("setCellDefaultOccupancy", &OcTree::setCellDefaultOccupancy)) .def(dv::member_func("setOccupancyThres", &OcTree::setOccupancyThres)) .def(dv::member_func("setFreeThres", &OcTree::setFreeThres)) .def(dv::member_func("getRootBV", &OcTree::getRootBV)) .def(dv::member_func("toBoxes", &OcTree::toBoxes)) .def("tobytes", tobytes, doxygen::member_func_doc(&OcTree::tobytes)); doxygen::def("makeOctree", &makeOctree); eigenpy::enableEigenPySpecific(); eigenpy::StdVectorPythonVisitor, true>::expose( "StdVec_Vec6"); } ================================================ FILE: python/pickle.hh ================================================ // // Copyright (c) 2022 INRIA // #ifndef COAL_PYTHON_PICKLE_H #define COAL_PYTHON_PICKLE_H #include #include #include #include #include using namespace boost::python; using namespace coal; // template struct PickleObject : boost::python::pickle_suite { static boost::python::tuple getinitargs(const T&) { return boost::python::make_tuple(); } static boost::python::tuple getstate(const T& obj) { std::stringstream ss; boost::archive::text_oarchive oa(ss); oa & obj; return boost::python::make_tuple(boost::python::str(ss.str())); } static void setstate(T& obj, boost::python::tuple tup) { if (boost::python::len(tup) == 0 || boost::python::len(tup) > 1) { throw eigenpy::Exception( "Pickle was not able to reconstruct the object from the loaded " "data.\n" "The pickle data structure contains too many elements."); } boost::python::object py_obj = tup[0]; boost::python::extract obj_as_string(py_obj.ptr()); if (obj_as_string.check()) { const std::string str = obj_as_string; std::istringstream is(str); boost::archive::text_iarchive ia(is, boost::archive::no_codecvt); ia >> obj; } else { throw eigenpy::Exception( "Pickle was not able to reconstruct the model from the loaded data.\n" "The entry is not a string."); } } static bool getstate_manages_dict() { return false; } }; #endif // ifndef COAL_PYTHON_PICKLE_H ================================================ FILE: python/printable.hh ================================================ // // Copyright (c) 2026 INRIA // This file was borrowed from the Pinocchio library: // https://github.com/stack-of-tasks/pinocchio // #ifndef COAL_PYTHON_PRINTABLE_H #define COAL_PYTHON_PRINTABLE_H #include namespace coal { namespace python { namespace bp = boost::python; /// /// \brief Set the Python method __str__ and __repr__ to use the overloading /// operator<<. /// template struct PrintableVisitor : public bp::def_visitor> { template void visit(PyClass& cl) const { cl.def(bp::self_ns::str(bp::self_ns::self)) .def(bp::self_ns::repr(bp::self_ns::self)); } }; } // namespace python } // namespace coal #endif // ifndef COAL_PYTHON_PRINTABLE_H ================================================ FILE: python/serializable.hh ================================================ // // Copyright (c) 2017-2024 CNRS INRIA // This file was borrowed from the Pinocchio library: // https://github.com/stack-of-tasks/pinocchio // #ifndef COAL_PYTHON_SERIALIZABLE_H #define COAL_PYTHON_SERIALIZABLE_H #include #include "coal/serialization/archive.h" #include "coal/serialization/serializer.h" namespace coal { namespace python { using Serializer = ::coal::serialization::Serializer; namespace bp = boost::python; template struct SerializableVisitor : public bp::def_visitor> { template void visit(PyClass& cl) const { cl.def("saveToText", &Serializer::saveToText, bp::arg("filename"), "Saves *this inside a text file.") .def("loadFromText", &Serializer::loadFromText, bp::arg("filename"), "Loads *this from a text file.") .def("saveToString", &Serializer::saveToString, bp::arg("self"), "Parses the current object to a string.") .def("loadFromString", &Serializer::loadFromString, bp::args("self", "string"), "Parses from the input string the content of the current object.") .def("saveToXML", &Serializer::saveToXML, bp::args("filename", "tag_name"), "Saves *this inside a XML file.") .def("loadFromXML", &Serializer::loadFromXML, bp::args("self", "filename", "tag_name"), "Loads *this from a XML file.") .def("saveToBinary", &Serializer::saveToBinary, bp::args("self", "filename"), "Saves *this inside a binary file.") .def("loadFromBinary", &Serializer::loadFromBinary, bp::args("self", "filename"), "Loads *this from a binary file.") .def("saveToBuffer", &Serializer::saveToBuffer, bp::args("self", "buffer"), "Saves *this inside a binary buffer.") .def("loadFromBuffer", &Serializer::loadFromBuffer, bp::args("self", "buffer"), "Loads *this from a binary buffer."); } }; } // namespace python } // namespace coal #endif // ifndef COAL_PYTHON_SERIALIZABLE_H ================================================ FILE: python/utils/std-pair.hh ================================================ // // Copyright (c) 2023 INRIA // #ifndef COAL_PYTHON_UTILS_STD_PAIR_H #define COAL_PYTHON_UTILS_STD_PAIR_H #include #include template struct StdPairConverter { typedef typename pair_type::first_type T1; typedef typename pair_type::second_type T2; static PyObject* convert(const pair_type& pair) { return boost::python::incref( boost::python::make_tuple(pair.first, pair.second).ptr()); } static void* convertible(PyObject* obj) { if (!PyTuple_CheckExact(obj)) return 0; if (PyTuple_Size(obj) != 2) return 0; { boost::python::tuple tuple(boost::python::borrowed(obj)); boost::python::extract elt1(tuple[0]); if (!elt1.check()) return 0; boost::python::extract elt2(tuple[1]); if (!elt2.check()) return 0; } return obj; } static void construct( PyObject* obj, boost::python::converter::rvalue_from_python_stage1_data* memory) { boost::python::tuple tuple(boost::python::borrowed(obj)); void* storage = reinterpret_cast< boost::python::converter::rvalue_from_python_storage*>( reinterpret_cast(memory)) ->storage.bytes; new (storage) pair_type(boost::python::extract(tuple[0]), boost::python::extract(tuple[1])); memory->convertible = storage; } static PyTypeObject const* get_pytype() { PyTypeObject const* py_type = &PyTuple_Type; return py_type; } static void registration() { boost::python::converter::registry::push_back( reinterpret_cast(&convertible), &construct, boost::python::type_id() #ifndef BOOST_PYTHON_NO_PY_SIGNATURES , get_pytype #endif ); } }; #endif // ifndef COAL_PYTHON_UTILS_STD_PAIR_H ================================================ FILE: python/version.cc ================================================ // // Copyright (c) 2019-2023 CNRS INRIA // #include "coal/config.hh" #include "coal.hh" #include namespace bp = boost::python; inline bool checkVersionAtLeast(int major, int minor, int patch) { return COAL_VERSION_AT_LEAST(major, minor, patch); } inline bool checkVersionAtMost(int major, int minor, int patch) { return COAL_VERSION_AT_MOST(major, minor, patch); } void exposeVersion() { // Define release numbers of the current Coal version. bp::scope().attr("__version__") = BOOST_PP_STRINGIZE(COAL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_PATCH_VERSION); bp::scope().attr("__raw_version__") = COAL_VERSION; bp::scope().attr("COAL_MAJOR_VERSION") = COAL_MAJOR_VERSION; bp::scope().attr("COAL_MINOR_VERSION") = COAL_MINOR_VERSION; bp::scope().attr("COAL_PATCH_VERSION") = COAL_PATCH_VERSION; #if COAL_HAS_QHULL bp::scope().attr("WITH_QHULL") = true; #else bp::scope().attr("WITH_QHULL") = false; #endif #if COAL_HAS_OCTOMAP bp::scope().attr("WITH_OCTOMAP") = true; #else bp::scope().attr("WITH_OCTOMAP") = false; #endif bp::def("checkVersionAtLeast", &checkVersionAtLeast, bp::args("major", "minor", "patch"), "Checks if the current version of coal is at least" " the version provided by the input arguments."); bp::def("checkVersionAtMost", &checkVersionAtMost, bp::args("major", "minor", "patch"), "Checks if the current version of coal is at most" " the version provided by the input arguments."); } ================================================ FILE: python-nb/CMakeLists.txt ================================================ # Copyright 2025 INRIA # Detect the installed nanobind package and import it into CMake execute_process( COMMAND "${Python_EXECUTABLE}" -m nanobind --cmake_dir OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE nanobind_ROOT ) # Honor nanobind_ROOT cmake_policy(PUSH) cmake_policy(SET CMP0074 NEW) find_package(nanobind CONFIG REQUIRED) cmake_policy(POP) find_package(Boost REQUIRED) # Nanoeigenpy find_package(nanoeigenpy CONFIG REQUIRED) set(PYTHON_LIB_NAME_V2 ${PROJECT_NAME}_pywrap_nb) # Headers set( PYTHON_LIB_HEADERS broadphase/broadphase_callbacks_collision_manager.hh fwd.h pickle.hh serializable.hh ) # Sources set( PYTHON_LIB_SOURCES broadphase/broadphase.cc aabb.cc bvh.cc collision-geometries.cc collision.cc contact_patch.cc distance.cc math.cc gjk.cc height_field.cc memory-footprint.cc shapes.cc module.cc ) if(COAL_HAS_OCTOMAP) list(APPEND PYTHON_LIB_SOURCES octree.cc) endif(COAL_HAS_OCTOMAP) nanobind_add_module(${PYTHON_LIB_NAME_V2} NB_STATIC LTO NB_SUPPRESS_WARNINGS ${PYTHON_LIB_SOURCES}) target_link_libraries( ${PYTHON_LIB_NAME_V2} PRIVATE ${PROJECT_NAME}::${PROJECT_NAME} Boost::boost ) target_compile_definitions( ${PYTHON_LIB_NAME_V2} PRIVATE COAL_PYTHON_LIBNAME=${PYTHON_LIB_NAME_V2} ) set(MODULE_DIR ${PROJECT_NAME}) set_target_properties( ${PYTHON_LIB_NAME_V2} PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/${MODULE_DIR}" ) set(PYTHON_LIB_STUB_FILE ${MODULE_DIR}/${PYTHON_LIB_NAME_V2}.pyi) nanobind_add_stub( ${PYTHON_LIB_NAME_V2}_stub VERBOSE MODULE ${PROJECT_NAME}.${PYTHON_LIB_NAME_V2} OUTPUT ${PYTHON_LIB_STUB_FILE} DEPENDS ${PYTHON_LIB_NAME_V2} PYTHON_PATH ${CMAKE_CURRENT_BINARY_DIR}/${MODULE_DIR} ) add_dependencies(${PROJECT_NAME}_python ${PYTHON_LIB_NAME_V2}) ADD_SOURCE_GROUP(PYTHON_LIB_SOURCES) set(PYTHON_FILES __init__.py windows_dll_manager.py) foreach(pyfile ${PYTHON_FILES}) message(STATUS "Building python file ${pyfile}") PYTHON_BUILD(${MODULE_DIR} ${pyfile}) install( FILES ${MODULE_DIR}/${pyfile} DESTINATION ${Python_SITELIB}/${MODULE_DIR} ) endforeach(pyfile) install( TARGETS ${PYTHON_LIB_NAME_V2} EXPORT ${TARGETS_EXPORT_NAME} LIBRARY DESTINATION ${Python_SITELIB}/${MODULE_DIR} ) install( FILES ${CMAKE_CURRENT_BINARY_DIR}/${PYTHON_LIB_STUB_FILE} DESTINATION ${Python_SITELIB}/${MODULE_DIR} ) ================================================ FILE: python-nb/aabb.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" #include "coal/hfield.h" #include "coal/serialization/AABB.h" #include "pickle.hh" #include "serializable.hh" #include "fwd.h" #include using namespace coal; using namespace nb::literals; void exposeAABB(nb::module_& m) { nb::class_( m, "AABB", "A class describing the AABB collision structure, which is a " "box in 3D space determined by two diagonal points") .def(nb::init<>()) .def(nb::init(), "other"_a) .def(nb::init(), "v"_a) .def(nb::init(), "a"_a, "b"_a) .def(nb::init(), "core"_a, "delta"_a) .def(nb::init(), "a"_a, "b"_a, "c"_a) .def( "contain", [](const AABB& self, const Vec3s& p) { return self.contain(p); }, "p"_a, "Check whether the AABB contains a point p.") .def( "contain", [](const AABB& self, const AABB& other) { return self.contain(other); }, "other"_a, "Check whether the AABB contains another AABB.") .def( "overlap", [](const AABB& self, const AABB& other) { return self.overlap(other); }, "other"_a, "Check whether two AABB overlap.") .def( "overlap", [](const AABB& self, const AABB& other, AABB& overlapping_part) { return self.overlap(other, overlapping_part); }, "other"_a, "overlapping_part"_a, "Check whether two AABB overlap and return the overlapping part if " "true.") .def( "distance", [](const AABB& self, const AABB& other) { return self.distance(other); }, "other"_a, "Distance between two AABBs.") .def_prop_rw( "min_", [](AABB& self) -> Vec3s& { return self.min_; }, [](AABB& self, const Vec3s& min_) { self.min_ = min_; }, "The min point in the AABB.") .def_prop_rw( "max_", [](AABB& self) -> Vec3s& { return self.max_; }, [](AABB& self, const Vec3s& max_) { self.max_ = max_; }, "The max point in the AABB.") .def(nb::self == nb::self) .def(nb::self != nb::self) .def(nb::self + nb::self) .def(nb::self += nb::self) .def(nb::self += Vec3s()) .def("size", &AABB::volume) .def("center", &AABB::center) .def("width", &AABB::width) .def("height", &AABB::height) .def("depth", &AABB::depth) .def("volume", &AABB::volume) .def( "expand", [](AABB& self, const AABB& other, Scalar scalar) -> AABB& { return self.expand(other, scalar); }, nb::rv_policy::reference_internal) .def( "expand", [](AABB& self, Scalar scalar) -> AABB& { return self.expand(scalar); }, nb::rv_policy::reference_internal) .def( "expand", [](AABB& self, const Vec3s& vec) -> AABB& { return self.expand(vec); }, nb::rv_policy::reference_internal) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); } ================================================ FILE: python-nb/broadphase/broadphase.cc ================================================ /// Copyrigh 2025 INRIA #include "coal/fwd.hh" #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #include "coal/broadphase/broadphase_bruteforce.h" #include "coal/broadphase/broadphase_SaP.h" #include "coal/broadphase/broadphase_SSaP.h" #include "coal/broadphase/broadphase_interval_tree.h" #include "coal/broadphase/broadphase_spatialhash.h" #include "coal/broadphase/default_broadphase_callbacks.h" #include "broadphase_callbacks_collision_manager.hh" #include "../fwd.h" #include using namespace coal; using namespace nb::literals; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS void exposeBroadPhase(nb::module_& m) { CollisionCallBackBaseWrapper::expose(m); DistanceCallBackBaseWrapper::expose(m); // CollisionCallBackDefault nb::class_( m, "CollisionCallBackDefault") .def(nb::init<>()) .DEF_RW_CLASS_ATTRIB(CollisionCallBackDefault, data); // DistanceCallBackDefault nb::class_( m, "DistanceCallBackDefault") .def(nb::init<>()) .DEF_RW_CLASS_ATTRIB(DistanceCallBackDefault, data); // CollisionCallBackCollect nb::class_( m, "CollisionCallBackCollect") .def(nb::init(), "max_size"_a) .DEF_CLASS_FUNC(CollisionCallBackCollect, numCollisionPairs) .def("getCollisionPairs", &CollisionCallBackCollect::getCollisionPairs, nb::rv_policy::copy) .def("exist", [](const CollisionCallBackCollect& self, const CollisionCallBackCollect::CollisionPair& pair) { return self.exist(pair); }) .def("exist", [](const CollisionCallBackCollect& self, CollisionObject* obj1, CollisionObject* obj2) { return self.exist(obj1, obj2); }); nb::class_(m, "CollisionData") .def(nb::init<>()) .DEF_RW_CLASS_ATTRIB(CollisionData, request) .DEF_RW_CLASS_ATTRIB(CollisionData, result) .DEF_RW_CLASS_ATTRIB(CollisionData, done) .DEF_CLASS_FUNC(CollisionData, clear); nb::class_(m, "DistanceData") .def(nb::init<>()) .DEF_RW_CLASS_ATTRIB(DistanceData, request) .DEF_RW_CLASS_ATTRIB(DistanceData, result) .DEF_RW_CLASS_ATTRIB(DistanceData, done) .DEF_CLASS_FUNC(DistanceData, clear); BroadPhaseCollisionManagerWrapper::expose(m); BroadPhaseCollisionManagerWrapper::exposeDerived< DynamicAABBTreeCollisionManager>(m, "DynamicAABBTreeCollisionManager"); BroadPhaseCollisionManagerWrapper::exposeDerived< DynamicAABBTreeArrayCollisionManager>( m, "DynamicAABBTreeArrayCollisionManager"); BroadPhaseCollisionManagerWrapper::exposeDerived< IntervalTreeCollisionManager>(m, "IntervalTreeCollisionManager"); BroadPhaseCollisionManagerWrapper::exposeDerived( m, "SSaPCollisionManager"); BroadPhaseCollisionManagerWrapper::exposeDerived( m, "SaPCollisionManager"); BroadPhaseCollisionManagerWrapper::exposeDerived( m, "NaiveCollisionManager"); // Specific case of SpatialHashingCollisionManager using HashTable = detail::SimpleHashTable; using Derived = SpatialHashingCollisionManager; nb::class_( m, "SpatialHashingCollisionManager") .def(nb::init(), "cell_size"_a, "scene_min"_a, "scene_max"_a, "default_table_size"_a = 1000); } COAL_COMPILER_DIAGNOSTIC_POP ================================================ FILE: python-nb/broadphase/broadphase_callbacks_collision_manager.hh ================================================ /// Copyright 2025 INRIA #ifndef COAL_PYTHON_NB_BROADPHASE_BROADPHASE_CALLBACKS_HH #define COAL_PYTHON_NB_BROADPHASE_BROADPHASE_CALLBACKS_HH #include "coal/fwd.hh" #include "coal/broadphase/broadphase_callbacks.h" #include "coal/broadphase/broadphase_collision_manager.h" #include "../fwd.h" #include #include namespace coal { struct CollisionCallBackBaseWrapper : CollisionCallBackBase { NB_TRAMPOLINE(CollisionCallBackBase, 2); using Base = CollisionCallBackBase; void init() override { NB_OVERRIDE_PURE(init); } bool collide(CollisionObject* o1, CollisionObject* o2) override { NB_OVERRIDE_PURE(collide, o1, o2); } static void expose(nb::module_& m) { nb::class_( m, "CollisionCallBackBase") .def("init", &Base::init) .def("collide", &Base::collide) .def("__call__", &Base::operator()); } }; // CollisionCallBackBaseWrapper struct DistanceCallBackBaseWrapper : DistanceCallBackBase { NB_TRAMPOLINE(DistanceCallBackBase, 2); using Base = DistanceCallBackBase; using Self = DistanceCallBackBaseWrapper; void init() override { NB_OVERRIDE_PURE(init); } bool distance(CollisionObject* o1, CollisionObject* o2, Eigen::Matrix& dist) { return distance(o1, o2, dist.coeffRef(0, 0)); } bool distance(CollisionObject* o1, CollisionObject* o2, Scalar& dist) override { NB_OVERRIDE_PURE(distance, o1, o2, dist); } static void expose(nb::module_& m) { nb::class_( m, "DistanceCallBackBase") .def("init", &Base::init) .def("distance", [](Self& self, CollisionObject* o1, CollisionObject* o2, Eigen::Matrix& dist) { return self.distance(o1, o2, dist); }) .def("__call__", &Base::operator()); } }; // CollisionCallBackBaseWrapper struct BroadPhaseCollisionManagerWrapper : BroadPhaseCollisionManager { NB_TRAMPOLINE(BroadPhaseCollisionManager, 17); using Base = BroadPhaseCollisionManager; void registerObjects( const std::vector& other_objs) override { NB_OVERRIDE_PURE(registerObjects, other_objs); } void registerObject(CollisionObject* obj) override { NB_OVERRIDE_PURE(registerObject, obj); } void unregisterObject(CollisionObject* obj) override { NB_OVERRIDE_PURE(unregisterObject, obj); } void update(const std::vector& other_objs) override { NB_OVERRIDE_PURE(update, other_objs); } void update(CollisionObject* obj) override { NB_OVERRIDE_PURE(update, obj); } void update() override { NB_OVERRIDE_PURE(update); } void setup() override { NB_OVERRIDE_PURE(setup); } void clear() override { NB_OVERRIDE_PURE(clear); } std::vector getObjects() const override { NB_OVERRIDE_PURE(getObjects); } void collide(CollisionCallBackBase* callback) const override { NB_OVERRIDE_PURE(collide, callback); } void collide(CollisionObject* obj, CollisionCallBackBase* callback) const override { NB_OVERRIDE_PURE(collide, obj, callback); } void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase* callback) const override { NB_OVERRIDE_PURE(collide, other_manager, callback); } void distance(DistanceCallBackBase* callback) const override { NB_OVERRIDE_PURE(distance, callback); } void distance(CollisionObject* obj, DistanceCallBackBase* callback) const override { NB_OVERRIDE_PURE(distance, obj, callback); } void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase* callback) const override { NB_OVERRIDE_PURE(distance, other_manager, callback); } bool empty() const override { NB_OVERRIDE_PURE(empty); } size_t size() const override { NB_OVERRIDE_PURE(size); } static void expose(nb::module_& m) { nb::class_( m, "BroadPhaseCollisionManager") .def("registerObjects", &Base::registerObjects) .def("registerObject", &Base::registerObject) .def("unregisterObject", &Base::unregisterObject) .def("update", [](Base& self) { self.update(); }) .def("update", [](Base& self, const std::vector& objects) { self.update(objects); }) .def("update", [](Base& self, CollisionObject* obj) { self.update(obj); }) .def("setup", &Base::setup) .def("clear", &Base::clear) .def("empty", &Base::empty) .def("size", &Base::size) .def("getObjects", [](const Base& self) { return self.getObjects(); }) .def("collide", [](const Base& self, CollisionCallBackBase* callback) { self.collide(callback); }) .def("collide", [](const Base& self, CollisionObject* obj, CollisionCallBackBase* callback) { self.collide(obj, callback); }) .def("collide", [](const Base& self, BroadPhaseCollisionManager* manager, CollisionCallBackBase* callback) { self.collide(manager, callback); }) .def("collide", [](const Base& self, const CollisionCallBackFunctor& callback) { self.collide(callback); }) .def("collide", [](const Base& self, CollisionObject* obj, const CollisionCallBackFunctor& callback) { self.collide(obj, callback); }) .def("collide", [](const Base& self, BroadPhaseCollisionManager* manager, const CollisionCallBackFunctor& callback) { self.collide(manager, callback); }) .def("distance", [](const Base& self, DistanceCallBackBase* callback) { self.distance(callback); }) .def("distance", [](const Base& self, CollisionObject* obj, DistanceCallBackBase* callback) { self.distance(obj, callback); }) .def("distance", [](const Base& self, BroadPhaseCollisionManager* manager, DistanceCallBackBase* callback) { self.distance(manager, callback); }) .def("distance", [](const Base& self, const DistanceCallBackFunctor& callback) { self.distance(callback); }) .def("distance", [](const Base& self, CollisionObject* obj, const DistanceCallBackFunctor& callback) { self.distance(obj, callback); }) .def("distance", [](const Base& self, BroadPhaseCollisionManager* manager, const DistanceCallBackFunctor& callback) { self.distance(manager, callback); }); } template static void exposeDerived(nb::module_& m, const char* name) { nb::class_(m, name).def(nb::init<>()); } }; // BroadPhaseCollisionManagerWrapper } // namespace coal #endif // ifndef COAL_PYTHON_NB_BROADPHASE_BROADPHASE_CALLBACKS_HH ================================================ FILE: python-nb/bvh.cc ================================================ /// Copyright 2025-2026 INRIA #include "coal/BVH/BVH_model.h" #include "coal/serialization/BVH_model.h" #include "coal/shape/convex.h" #include "pickle.hh" #include "serializable.hh" #include "fwd.h" #include using namespace coal; using namespace nb::literals; typedef std::vector Vec3ss; typedef std::vector Triangles; template void exposeBVHModel(nb::module_& m, const char* name) { using BVHType = BVHModel; nb::class_(m, name) .def(nb::init<>()) .def(nb::init(), "other"_a) .DEF_CLASS_FUNC(BVHType, getNumBVs) .DEF_CLASS_FUNC(BVHType, makeParentRelative) .DEF_CLASS_FUNC(BVHType, memUsage) .def("clone", &BVHType::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()); } void exposeBVHModels(nb::module_& m) { using RowMatrixX3 = Eigen::Matrix; using MapRowMatrixX3 = Eigen::Map; nb::class_(m, "BVHModelBase") .def("vertex", [](BVHModelBase& bvh, size_t i) -> Vec3s& { if (i >= bvh.num_vertices) throw nb::index_error("Vertex index out of range."); return (*bvh.vertices)[i]; }) .def( "vertices", [](BVHModelBase& bvh) { if (bvh.num_vertices > 0) { return MapRowMatrixX3{bvh.vertices->data()->data(), bvh.num_vertices, 3}; } else { return MapRowMatrixX3{nullptr, 0, 3}; } }, "Retrieve all vertices", nb::rv_policy::reference_internal) .def( "tri_indices", [](const BVHModelBase& bvh, size_t i) { if (i >= bvh.num_tris) nb::index_error("Triangle index out of range."); return (*bvh.tri_indices)[i]; }, "index"_a, "Retrieve the triangle given by its index.") .def_ro("num_vertices", &BVHModelBase::num_vertices) .def_ro("num_tris", &BVHModelBase::num_tris) .def_ro("build_state", &BVHModelBase::build_state) .def_ro("convex", &BVHModelBase::convex) .def("buildConvexRepresentation", &BVHModelBase::buildConvexRepresentation, "share_memory"_a) .def("buildConvexHull", &BVHModelBase::buildConvexHull, "keepTriangle"_a, "qhullCommand"_a = NULL) .DEF_CLASS_FUNC(BVHModelBase, beginModel) .DEF_CLASS_FUNC(BVHModelBase, addVertex) .DEF_CLASS_FUNC(BVHModelBase, addVertices) .DEF_CLASS_FUNC(BVHModelBase, addTriangle) .DEF_CLASS_FUNC(BVHModelBase, addTriangles) .def("addSubModel", [](BVHModelBase& self, const Vec3ss& vec, const Triangles& tri) { return self.addSubModel(vec, tri); }) .def("addSubModel", [](BVHModelBase& self, const Vec3ss& vec) { return self.addSubModel(vec); }) .DEF_CLASS_FUNC(BVHModelBase, endModel) .DEF_CLASS_FUNC(BVHModelBase, beginReplaceModel) .DEF_CLASS_FUNC(BVHModelBase, replaceVertex) .DEF_CLASS_FUNC(BVHModelBase, replaceTriangle) .DEF_CLASS_FUNC(BVHModelBase, replaceSubModel) .DEF_CLASS_FUNC(BVHModelBase, endReplaceModel) .DEF_CLASS_FUNC(BVHModelBase, getModelType) ; exposeBVHModel(m, "BVHModelOBB"); exposeBVHModel(m, "BVHModelOBBRSS"); } ================================================ FILE: python-nb/coal/__init__.py ================================================ # ruff: noqa: F401, F403 # On Windows, if coal.dll is not in the same directory than # the .pyd, it will not be loaded. # We first try to load coal, then, if it fail and we are on Windows: # 1. We add all paths inside COAL_WINDOWS_DLL_PATH to DllDirectory # 2. If COAL_WINDOWS_DLL_PATH we add the relative path from the # package directory to the bin directory to DllDirectory # This solution is inspired from: # - https://github.com/PixarAnimationStudios/OpenUSD/pull/1511/files # - https://stackoverflow.com/questions/65334494/python-c-extension-packaging-dll-along-with-pyd # More resources on https://github.com/diffpy/pyobjcryst/issues/33 try: from .coal_pywrap_nb import * # noqa from .coal_pywrap_nb import __version__ except ImportError: import platform if platform.system() == "Windows": from .windows_dll_manager import build_directory_manager, get_dll_paths with build_directory_manager() as dll_dir_manager: for p in get_dll_paths(): dll_dir_manager.add_dll_directory(p) from .coal_pywrap_nb import * # noqa from .coal_pywrap_nb import __version__ # noqa else: raise ================================================ FILE: python-nb/coal/windows_dll_manager.py ================================================ import contextlib import os import sys def get_dll_paths(): coal_paths = os.getenv("COAL_WINDOWS_DLL_PATH") if coal_paths is None: # From https://peps.python.org/pep-0250/#implementation # lib/python-version/site-packages/package RELATIVE_DLL_PATH1 = "..\\..\\..\\..\\bin" # lib/site-packages/package RELATIVE_DLL_PATH2 = "..\\..\\..\\bin" # For unit test RELATIVE_DLL_PATH3 = "..\\..\\bin" return [ os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH1), os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH2), os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH3), ] else: return coal_paths.split(os.pathsep) class PathManager(contextlib.AbstractContextManager): """Restore PATH state after importing Python module""" def add_dll_directory(self, dll_dir: str): os.environ["PATH"] += os.pathsep + dll_dir def __enter__(self): self.old_path = os.environ["PATH"] return self def __exit__(self, *exc_details): os.environ["PATH"] = self.old_path class DllDirectoryManager(contextlib.AbstractContextManager): """Restore DllDirectory state after importing Python module""" def add_dll_directory(self, dll_dir: str): # add_dll_directory can fail on relative path and non # existing path. # Since we don't know all the fail criterion we just ignore # thrown exception try: self.dll_dirs.append(os.add_dll_directory(dll_dir)) except OSError: pass def __enter__(self): self.dll_dirs = [] return self def __exit__(self, *exc_details): for d in self.dll_dirs: d.close() def build_directory_manager(): if sys.version_info >= (3, 8): return DllDirectoryManager() else: return PathManager() ================================================ FILE: python-nb/collision-geometries.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" #include "coal/hfield.h" #include "coal/serialization/AABB.h" #include "pickle.hh" #include "serializable.hh" #include "fwd.h" #include #include using namespace coal; using namespace nb::literals; void exposeAABB(nb::module_& m); void exposeShapes(nb::module_& m); void exposeBVHModels(nb::module_& m); void exposeHeightFields(nb::module_& m); void exposeComputeMemoryFootprint(nb::module_& m); void exposeCollisionGeometries(nb::module_& m) { nb::enum_(m, "BVHModelType") .value("BVH_MODEL_UNKNOWN", BVH_MODEL_UNKNOWN) .value("BVH_MODEL_TRIANGLES", BVH_MODEL_TRIANGLES) .value("BVH_MODEL_POINTCLOUD", BVH_MODEL_POINTCLOUD) .export_values(); nb::enum_(m, "BVHBuildState") .value("BVH_BUILD_STATE_EMPTY", BVH_BUILD_STATE_EMPTY) .value("BVH_BUILD_STATE_BEGUN", BVH_BUILD_STATE_BEGUN) .value("BVH_BUILD_STATE_PROCESSED", BVH_BUILD_STATE_PROCESSED) .value("BVH_BUILD_STATE_UPDATE_BEGUN", BVH_BUILD_STATE_UPDATE_BEGUN) .value("BVH_BUILD_STATE_UPDATED", BVH_BUILD_STATE_UPDATED) .value("BVH_BUILD_STATE_REPLACE_BEGUN", BVH_BUILD_STATE_REPLACE_BEGUN) .export_values(); nb::enum_(m, "OBJECT_TYPE") .value("OT_UNKNOWN", OT_UNKNOWN) .value("OT_BVH", OT_BVH) .value("OT_GEOM", OT_GEOM) .value("OT_OCTREE", OT_OCTREE) .value("OT_HFIELD", OT_HFIELD) .export_values(); nb::enum_(m, "NODE_TYPE") .value("BV_UNKNOWN", BV_UNKNOWN) .value("BV_AABB", BV_AABB) .value("BV_OBB", BV_OBB) .value("BV_RSS", BV_RSS) .value("BV_kIOS", BV_kIOS) .value("BV_OBBRSS", BV_OBBRSS) .value("BV_KDOP16", BV_KDOP16) .value("BV_KDOP18", BV_KDOP18) .value("BV_KDOP24", BV_KDOP24) .value("GEOM_BOX", GEOM_BOX) .value("GEOM_SPHERE", GEOM_SPHERE) .value("GEOM_ELLIPSOID", GEOM_ELLIPSOID) .value("GEOM_CAPSULE", GEOM_CAPSULE) .value("GEOM_CONE", GEOM_CONE) .value("GEOM_CYLINDER", GEOM_CYLINDER) .value("GEOM_CONVEX", GEOM_CONVEX) .value("GEOM_PLANE", GEOM_PLANE) .value("GEOM_HALFSPACE", GEOM_HALFSPACE) .value("GEOM_TRIANGLE", GEOM_TRIANGLE) .value("GEOM_OCTREE", GEOM_OCTREE) .value("HF_AABB", HF_AABB) .value("HF_OBBRSS", HF_OBBRSS) .export_values(); m.def( "translate", [](const AABB& aabb, const Vec3s& t) { return translate(aabb, t); }, "aabb"_a, "t"_a, "Translate the center of AABB by t"); m.def( "rotate", [](const AABB& aabb, const Matrix3s& R) { return rotate(aabb, R); }, "aabb"_a, "R"_a, "Rotate the AABB object by R"); nb::class_(m, "CollisionGeometry") .DEF_CLASS_FUNC(CollisionGeometry, getObjectType) .DEF_CLASS_FUNC(CollisionGeometry, getNodeType) .DEF_CLASS_FUNC(CollisionGeometry, computeLocalAABB) .DEF_CLASS_FUNC(CollisionGeometry, computeCOM) .DEF_CLASS_FUNC(CollisionGeometry, computeMomentofInertia) .DEF_CLASS_FUNC(CollisionGeometry, computeVolume) .DEF_CLASS_FUNC(CollisionGeometry, computeMomentofInertiaRelatedToCOM) .def_rw("aabb_radius", &CollisionGeometry::aabb_radius) .def_rw("aabb_center", &CollisionGeometry::aabb_center) .def_rw("aabb_local", &CollisionGeometry::aabb_local) .def("isOccupied", &CollisionGeometry::isOccupied) .def("isFree", &CollisionGeometry::isFree) .def("isUncertain", &CollisionGeometry::isUncertain) .def_rw("cost_density", &CollisionGeometry::cost_density) .def_rw("threshold_occupied", &CollisionGeometry::threshold_occupied) .def_rw("threshold_free", &CollisionGeometry::threshold_free) .def(nb::self == nb::self) .def(nb::self != nb::self); exposeAABB(m); exposeShapes(m); exposeBVHModels(m); exposeHeightFields(m); exposeComputeMemoryFootprint(m); } void exposeCollisionObject(nb::module_& m) { nb::class_(m, "CollisionObject") .def(nb::init(), "cgeom"_a, "compute_local_aabb"_a = true) .def(nb::init(), "cgeom"_a, "tf"_a, "compute_local_aabb"_a = true) .def(nb::init(), "cgeom"_a, "R"_a, "t"_a, "compute_local_aabb"_a = true) .def(nb::self == nb::self) .def(nb::self != nb::self) .DEF_CLASS_FUNC(CollisionObject, getObjectType) .DEF_CLASS_FUNC(CollisionObject, getNodeType) // properties .def("getTranslation", &CollisionObject::getTranslation, nb::rv_policy::copy) .def("setTranslation", &CollisionObject::setTranslation) .def("getRotation", &CollisionObject::getRotation, nb::rv_policy::copy) .def("setRotation", &CollisionObject::setRotation) .def("getTransform", &CollisionObject::getTransform, nb::rv_policy::copy) .def("setTransform", [](CollisionObject& o, const Transform3s& tf) { o.setTransform(tf); }) .def("setTransform", [](CollisionObject& o, const Matrix3s& R, const Vec3s& t) { o.setTransform(R, t); }) .def("isIdentityTransform", &CollisionObject::isIdentityTransform) .def("setIdentityTransform", &CollisionObject::setIdentityTransform) .def( "getAABB", [](CollisionObject& o) -> AABB& { return o.getAABB(); }, nb::rv_policy::automatic_reference) .DEF_CLASS_FUNC(CollisionObject, computeAABB) .def("setCollisionGeometry", &CollisionObject::setCollisionGeometry, "cgeom"_a, "compute_local_aabb"_a = true, nb::rv_policy::reference_internal) .def( "collisionGeometry", [](CollisionObject& o) { return o.collisionGeometry(); }, nb::rv_policy::copy); } ================================================ FILE: python-nb/collision.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #include "coal/collision.h" #include "coal/serialization/collision_data.h" COAL_COMPILER_DIAGNOSTIC_POP #include "serializable.hh" #include "fwd.h" #include using namespace coal; using namespace nb::literals; void exposeCollisionAPI(nb::module_& m) { nb::enum_(m, "CollisionRequestFlag") .value("CONTACT", CONTACT) .value("DISTANCE_LOWER_BOUND", DISTANCE_LOWER_BOUND) .value("NO_REQUEST", NO_REQUEST) .export_values(); nb::class_(m, "CPUTimes") .def_ro("wall", &CPUTimes::wall, "wall time in micro seconds (us)") .def_ro("user", &CPUTimes::user, "user time in micro seconds (us)") .def_ro("system", &CPUTimes::system, "system time in micro seconds (us)") .def("clear", &CPUTimes::clear, "Reset the time values."); COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS nb::class_(m, "QueryRequest") .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_tolerance) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_max_iterations) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_variant) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_convergence_criterion) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_convergence_criterion_type) .DEF_RW_CLASS_ATTRIB(QueryRequest, gjk_initial_guess) .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_cached_gjk_guess) .def_prop_rw( "enable_cached_gjk_guess", [](QueryRequest& self) -> bool { return self.enable_cached_gjk_guess; }, [](QueryRequest& self, bool value) { self.enable_cached_gjk_guess = value; }) .DEF_RW_CLASS_ATTRIB(QueryRequest, cached_gjk_guess) .DEF_RW_CLASS_ATTRIB(QueryRequest, cached_support_func_guess) .DEF_RW_CLASS_ATTRIB(QueryRequest, epa_max_iterations) .DEF_RW_CLASS_ATTRIB(QueryRequest, epa_tolerance) .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_timings) .DEF_CLASS_FUNC(QueryRequest, updateGuess); COAL_COMPILER_DIAGNOSTIC_POP COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS nb::class_(m, "CollisionRequest") .def(nb::init<>()) .def(nb::init(), "flag"_a, "num_max_contacts_"_a) .DEF_RW_CLASS_ATTRIB(CollisionRequest, num_max_contacts) .DEF_RW_CLASS_ATTRIB(CollisionRequest, enable_contact) .def_prop_rw( "enable_distance_lower_bound", [](CollisionRequest& self) -> bool { return self.enable_distance_lower_bound; }, [](CollisionRequest& self, bool value) { self.enable_distance_lower_bound = value; }) .DEF_RW_CLASS_ATTRIB(CollisionRequest, security_margin) .DEF_RW_CLASS_ATTRIB(CollisionRequest, break_distance) .DEF_RW_CLASS_ATTRIB(CollisionRequest, distance_upper_bound) .def(python::v2::SerializableVisitor()); nb::bind_vector>(m, "StdVec_CollisionRequest"); COAL_COMPILER_DIAGNOSTIC_POP nb::class_(m, "Contact") .def(nb::init<>()) .def(nb::init(), "o1_"_a, "o2_"_a, "b1_"_a, "b2_"_a) .def(nb::init(), "o1_"_a, "o2_"_a, "b1_"_a, "b2_"_a, "pos_"_a, "normal_"_a, "depth_"_a) .def_prop_ro("o1", [](Contact& self) -> CollisionGeometry* { return const_cast(self.o1); }) .def_prop_ro("o2", [](Contact& self) -> CollisionGeometry* { return const_cast(self.o2); }) .def("getNearestPoint1", [](const Contact& self) -> Vec3s { return self.nearest_points[0]; }) .def("getNearestPoint2", [](const Contact& self) -> Vec3s { return self.nearest_points[1]; }) .DEF_RW_CLASS_ATTRIB(Contact, b1) .DEF_RW_CLASS_ATTRIB(Contact, b2) .DEF_RW_CLASS_ATTRIB(Contact, normal) .DEF_RW_CLASS_ATTRIB(Contact, nearest_points) .DEF_RW_CLASS_ATTRIB(Contact, pos) .DEF_RW_CLASS_ATTRIB(Contact, penetration_depth) .def(nb::self == nb::self) .def(nb::self != nb::self); nb::bind_vector>(m, "StdVec_Contact"); nb::class_(m, "QueryResult") .DEF_RW_CLASS_ATTRIB(QueryResult, cached_gjk_guess) .DEF_RW_CLASS_ATTRIB(QueryResult, cached_support_func_guess) .DEF_RW_CLASS_ATTRIB(QueryResult, timings); nb::class_(m, "CollisionResult") .def(nb::init<>()) .DEF_CLASS_FUNC(CollisionResult, isCollision) .DEF_CLASS_FUNC(CollisionResult, numContacts) .DEF_CLASS_FUNC(CollisionResult, addContact) .DEF_CLASS_FUNC(CollisionResult, clear) .def("getContact", &CollisionResult::getContact, nb::rv_policy::copy) .def("getContacts", [](const CollisionResult& self, std::vector& contacts) { self.getContacts(contacts); }) .def( "getContacts", [](const CollisionResult& self) -> const std::vector& { return self.getContacts(); }, nb::rv_policy::reference_internal) .DEF_RW_CLASS_ATTRIB(CollisionResult, distance_lower_bound) .def(python::v2::SerializableVisitor()); nb::bind_vector>(m, "StdVec_CollisionResult"); m.def( "collide", [](const CollisionObject* o1, const CollisionObject* o2, const CollisionRequest& request, CollisionResult& result) -> std::size_t { return collide(o1, o2, request, result); }, "o1"_a, "o2"_a, "request"_a, "result"_a); m.def( "collide", [](const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) -> std::size_t { return collide(o1, tf1, o2, tf2, request, result); }, "geom1"_a, "transform1"_a, "geom2"_a, "transform2"_a, "request"_a, "result"_a); nb::class_(m, "ComputeCollision") .def(nb::init(), "o1"_a, "o2"_a) .def("call", [](const ComputeCollision& self, const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) -> std::size_t { return self(tf1, tf2, request, result); }); } ================================================ FILE: python-nb/contact_patch.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" #include "coal/contact_patch.h" #include "coal/serialization/collision_data.h" #include "serializable.hh" #include "fwd.h" using namespace coal; using namespace nb::literals; void exposeContactPatchAPI(nb::module_& m) { nb::enum_(m, "ContactPatchDirection") .value("DEFAULT", ContactPatch::PatchDirection::DEFAULT) .value("INVERTED", ContactPatch::PatchDirection::INVERTED) .export_values(); nb::class_(m, "ContactPatch") .def(nb::init(), "preallocated_size"_a = 12) .DEF_RW_CLASS_ATTRIB(ContactPatch, tf) .DEF_RW_CLASS_ATTRIB(ContactPatch, direction) .DEF_RW_CLASS_ATTRIB(ContactPatch, penetration_depth) .DEF_CLASS_FUNC(ContactPatch, size) .DEF_CLASS_FUNC(ContactPatch, getNormal) .DEF_CLASS_FUNC(ContactPatch, addPoint) .DEF_CLASS_FUNC(ContactPatch, getPoint) .DEF_CLASS_FUNC(ContactPatch, getPointShape1) .DEF_CLASS_FUNC(ContactPatch, getPointShape2) .DEF_CLASS_FUNC(ContactPatch, clear) .DEF_CLASS_FUNC(ContactPatch, isSame); nb::bind_vector>(m, "StdVec_ContactPatch"); nb::class_(m, "ContactPatchRequest") .def(nb::init(), "max_num_patch"_a = 1, "num_samples_curved_shapes"_a = 12, "patch_tolerance"_a = Scalar(1e-3)) .def(nb::init(), "collision_request"_a, "num_samples_curved_shapes"_a = 12, "patch_tolerance"_a = Scalar(1e-3)) .DEF_RW_CLASS_ATTRIB(ContactPatchRequest, max_num_patch) .DEF_CLASS_FUNC(ContactPatchRequest, getNumSamplesCurvedShapes) .DEF_CLASS_FUNC(ContactPatchRequest, setNumSamplesCurvedShapes) .DEF_CLASS_FUNC(ContactPatchRequest, getPatchTolerance) .DEF_CLASS_FUNC(ContactPatchRequest, setPatchTolerance); nb::bind_vector>( m, "StdVec_ContactPatchRequest"); nb::class_(m, "ContactPatchResult") .def(nb::init(), "request"_a = 12) .DEF_CLASS_FUNC(ContactPatchResult, numContactPatches) .DEF_CLASS_FUNC(ContactPatchResult, getUnusedContactPatch) .def("getContactPatch", &ContactPatchResult::getContactPatch, nb::rv_policy::copy) .DEF_CLASS_FUNC(ContactPatchResult, clear) .DEF_CLASS_FUNC(ContactPatchResult, set) .DEF_CLASS_FUNC(ContactPatchResult, check); nb::bind_vector>(m, "StdVec_ContactPatchResult"); nb::class_(m, "ComputeContactPatch") .def(nb::init(), "o1"_a, "o2"_a) .def("__call__", [](const ComputeContactPatch& self, const Transform3s& t1, const Transform3s& t2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) { self.operator()(t1, t2, collision_result, request, result); }); } ================================================ FILE: python-nb/distance.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" #include "coal/distance.h" #include "coal/serialization/collision_data.h" #include "serializable.hh" #include "fwd.h" using namespace coal; using namespace nb::literals; void exposeDistanceAPI(nb::module_& m) { COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS nb::class_(m, "DistanceRequest") .def(nb::init(), "enable_nearest_points"_a = true, "enable_signed_distance_"_a = true, "rel_err"_a = Scalar(0.0), "abs_err"_a = Scalar(0.0)) .def_prop_rw( "enable_nearest_points", [](DistanceRequest& self) -> bool { return self.enable_nearest_points; }, [](DistanceRequest& self, const bool value) -> void { self.enable_nearest_points = value; }) .DEF_RW_CLASS_ATTRIB(DistanceRequest, enable_signed_distance) .DEF_RW_CLASS_ATTRIB(DistanceRequest, rel_err) .DEF_RW_CLASS_ATTRIB(DistanceRequest, abs_err) .def(python::v2::SerializableVisitor()); COAL_COMPILER_DIAGNOSTIC_POP nb::bind_vector>(m, "StdVec_DistanceRequest"); nb::class_(m, "DistanceResult") .def(nb::init<>()) .DEF_RW_CLASS_ATTRIB(DistanceResult, min_distance) .DEF_RW_CLASS_ATTRIB(DistanceResult, normal) .def("getNearestPoint1", [](const DistanceResult& res) -> Vec3s { return res.nearest_points[0]; }) .def("getNearestPoint2", [](const DistanceResult& res) -> Vec3s { return res.nearest_points[1]; }) .DEF_RO_CLASS_ATTRIB(DistanceResult, nearest_points) .DEF_RO_CLASS_ATTRIB(DistanceResult, o1) .DEF_RO_CLASS_ATTRIB(DistanceResult, o2) .DEF_RW_CLASS_ATTRIB(DistanceResult, b1) .DEF_RW_CLASS_ATTRIB(DistanceResult, b2) .def("clear", &DistanceResult::clear) .def(python::v2::SerializableVisitor()); nb::bind_vector>(m, "StdVec_DistanceResult"); m.def( "distance", [](const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result) -> Scalar { return distance(o1, o2, request, result); }, "o1"_a, "o2"_a, "request"_a, "result"_a); m.def( "distance", [](const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) -> Scalar { return distance(o1, tf1, o2, tf2, request, result); }, "geom1"_a, "transform1"_a, "geom2"_a, "transform2"_a, "request"_a, "result"_a); nb::class_(m, "ComputeDistance") .def(nb::init(), "o1"_a, "o2"_a) .def("__call__", [](const ComputeDistance& self, const Transform3s& t1, const Transform3s& t2, const DistanceRequest& request, DistanceResult& result) { return self.operator()(t1, t2, request, result); }); } ================================================ FILE: python-nb/fwd.h ================================================ /// Copyright 2025 INRIA #ifndef COAL_PYTHON_NB_FWD_H #define COAL_PYTHON_NB_FWD_H #include #include #include #include namespace nb = nanobind; #define DEF_RW_CLASS_ATTRIB(cls_name, name) def_rw(#name, &cls_name::name) #define DEF_RO_CLASS_ATTRIB(cls_name, name) def_ro(#name, &cls_name::name) #define DEF_CLASS_FUNC(cls_name, name) def(#name, &cls_name::name) #endif // ifndef COAL_PYTHON_NB_FWD_H ================================================ FILE: python-nb/gjk.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" #include "coal/narrowphase/gjk.h" #include "fwd.h" using namespace coal; using coal::details::EPA; using coal::details::GJK; using coal::details::MinkowskiDiff; using coal::details::SupportOptions; using namespace nb::literals; void exposeGJK(nb::module_& m) { nb::enum_(m, "GJKStatus") .value("Failed", GJK::Status::Failed) .value("DidNotRun", GJK::Status::DidNotRun) .value("NoCollision", GJK::Status::NoCollision) .value("NoCollisionEarlyStopped", GJK::Status::NoCollisionEarlyStopped) .value("CollisionWithPenetrationInformation", GJK::Status::CollisionWithPenetrationInformation) .value("Collision", GJK::Status::Collision) .export_values(); nb::class_(m, "MinkowskiDiff") .def(nb::init<>()) .def("set", [](MinkowskiDiff& diff, const ShapeBase* shape1, const ShapeBase* shape2, bool flag) { if (flag) { diff.set(shape1, shape2); } else { diff.set(shape1, shape2); } }) .def("set", [](MinkowskiDiff& diff, const ShapeBase* shape1, const ShapeBase* shape2, const Transform3s& transform1, const Transform3s& transform2, bool flag) { if (flag) { diff.set( shape1, shape2, transform1, transform2); } else { diff.set(shape1, shape2, transform1, transform2); } }) .def("support0", [](MinkowskiDiff& self, const Vec3s& dir, int& hint, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.support0(dir, hint); } else { self.support0(dir, hint); } }) .def("support1", [](MinkowskiDiff& self, const Vec3s& dir, int& hint, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.support1(dir, hint); } else { self.support1(dir, hint); } }) .def("support", &MinkowskiDiff::support) .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, swept_sphere_radius) .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, normalize_support_direction); nb::enum_(m, "GJKVariant") .value("DefaultGJK", GJKVariant::DefaultGJK) .value("PolyakAcceleration", GJKVariant::PolyakAcceleration) .value("NesterovAcceleration", GJKVariant::NesterovAcceleration) .export_values(); nb::enum_(m, "GJKInitialGuess") .value("DefaultGuess", GJKInitialGuess::DefaultGuess) .value("CachedGuess", GJKInitialGuess::CachedGuess) .value("BoundingVolumeGuess", GJKInitialGuess::BoundingVolumeGuess) .export_values(); nb::enum_(m, "GJKConvergenceCriterion") .value("Default", GJKConvergenceCriterion::Default) .value("DualityGap", GJKConvergenceCriterion::DualityGap) .value("Hybrid", GJKConvergenceCriterion::Hybrid) .export_values(); nb::enum_(m, "GJKConvergenceCriterionType") .value("Absolute", GJKConvergenceCriterionType::Absolute) .value("Relative", GJKConvergenceCriterionType::Relative) .export_values(); nb::class_(m, "GJK") .def(nb::init(), "max_iterations_"_a, "tolerance"_a) .DEF_RW_CLASS_ATTRIB(GJK, distance) .DEF_RW_CLASS_ATTRIB(GJK, ray) .DEF_RW_CLASS_ATTRIB(GJK, support_hint) .DEF_RW_CLASS_ATTRIB(GJK, gjk_variant) .DEF_RW_CLASS_ATTRIB(GJK, convergence_criterion) .DEF_RW_CLASS_ATTRIB(GJK, convergence_criterion_type) .DEF_CLASS_FUNC(GJK, reset) .DEF_CLASS_FUNC(GJK, evaluate) .DEF_CLASS_FUNC(GJK, getTolerance) .DEF_CLASS_FUNC(GJK, getNumMaxIterations) .DEF_CLASS_FUNC(GJK, getNumIterations) .DEF_CLASS_FUNC(GJK, getNumIterationsMomentumStopped) .DEF_CLASS_FUNC(GJK, hasClosestPoints) .DEF_CLASS_FUNC(GJK, getWitnessPointsAndNormal) .DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak) .DEF_CLASS_FUNC(GJK, getGuessFromSimplex); } ================================================ FILE: python-nb/height_field.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" #include "coal/hfield.h" #include "coal/serialization/hfield.h" #include "coal/serialization/geometric_shapes.h" #include "pickle.hh" #include "serializable.hh" #include "fwd.h" using namespace coal; using namespace nb::literals; template void exposeHeightField(nb::module_& m, const char* name) { using Geometry = HeightField; using Base = typename Geometry::Base; using Node = typename Geometry::Node; nb::class_(m, name) .def(nb::init<>()) .def(nb::init&>(), "other"_a) .def(nb::init(), "x_dim"_a, "y_dim"_a, "heights"_a, "min_height"_a = Scalar(0)) .DEF_CLASS_FUNC(Geometry, getXDim) .DEF_CLASS_FUNC(Geometry, getYDim) .DEF_CLASS_FUNC(Geometry, getMinHeight) .DEF_CLASS_FUNC(Geometry, getMaxHeight) .DEF_CLASS_FUNC(Geometry, getNodeType) .DEF_CLASS_FUNC(Geometry, updateHeights) .def("clone", &Geometry::clone, nb::rv_policy::take_ownership) .def("getXGrid", &Geometry::getXGrid, nb::rv_policy::copy) .def("getYGrid", &Geometry::getYGrid, nb::rv_policy::copy) .def("getHeights", &Geometry::getHeights, nb::rv_policy::copy) .def( "getBV", [](Geometry& self, unsigned int index) -> Node& { return self.getBV(index); }, nanobind::rv_policy::reference_internal) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); } void exposeHeightFields(nb::module_& m) { exposeHeightField(m, "HeightFieldOBBRSS"); exposeHeightField(m, "HeightFieldAABB"); } ================================================ FILE: python-nb/math.cc ================================================ /// Copyright 2025 INRIA #include "coal/data_types.h" #include "coal/math/transform.h" #include "coal/serialization/transform.h" #include "serializable.hh" #include "pickle.hh" #include "fwd.h" #include #include using namespace coal; using namespace nb::literals; template void exposeTriangle(nb::module_& m, const std::string& classname) { using TriangleType = TriangleTpl; nb::class_(m, classname.c_str()) .def(nb::init<>()) .def(nb::init(), "p1"_a, "p2"_a, "p3"_a) .def("__getitem__", [](TriangleType& m, Py_ssize_t i) { if (i >= 3 || i <= -3) { throw nb::index_error("Index out of range"); } return m[IndexType(i % 3)]; }) .def("__setitem__", [](TriangleType& m, Py_ssize_t i, IndexType v) { if (i >= 3 || i <= -3) { throw nb::index_error("Index out of range"); } m[IndexType(i % 3)] = v; }) .def("set", &TriangleType::set) .def_static("size", &TriangleType::size) .def(nb::self == nb::self); } void exposeMaths(nb::module_& m) { nanoeigenpy::exposeQuaternion(m, "Quaternion"); nanoeigenpy::exposeAngleAxis(m, "AngleAxis"); nb::class_(m, "Transform3s") .def(nb::init<>()) .def(nb::init(), "R"_a, "t"_a) .def(nb::init(), "q"_a, "t"_a) .def(nb::init(), "R"_a) .def(nb::init(), "q"_a) .def(nb::init(), "t"_a) .def(nb::init()) .def("getQuatRotation", &Transform3s::getQuatRotation, nb::rv_policy::automatic_reference) .def("getTranslation", &Transform3s::getTranslation, nb::rv_policy::copy) .def("setTranslation", [](Transform3s& t, Vec3s v) { t.setTranslation(v); }) .def("getRotation", &Transform3s::getRotation, nb::rv_policy::copy) .def("setRotation", [](Transform3s& t, Matrix3s R) { t.setRotation(R); }) .def("isIdentity", &Transform3s::isIdentity) .def("setQuatRotation", &Transform3s::setQuatRotation) .def("setTransform", [](Transform3s& self, const Matrix3s& R, const Vec3s& t) { self.setTransform(R, t); }) .def("setTransform", [](Transform3s& self, const Quats& quat, const Vec3s& vec) { self.setTransform(quat, vec); }) .def("setIdentity", &Transform3s::setIdentity) .def_static("Identity", &Transform3s::Identity) .def("setRandom", &Transform3s::setRandom) .def_static("Random", &Transform3s::Random) .def("transform", [](const Transform3s& self, const Vec3s& v) { return self.transform(v); }) .def("inverseInPlace", &Transform3s::inverseInPlace, nb::rv_policy::automatic_reference) .def("inverse", &Transform3s::inverse) .def("inverseTimes", &Transform3s::inverseTimes) .def(nb::self * nb::self) .def(nb::self *= nb::self) .def(nb::self == nb::self) .def(nb::self != nb::self) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()); exposeTriangle(m, "Triangle32"); m.attr("Triangle") = m.attr("Triangle32"); exposeTriangle(m, "Triangle16"); nb::bind_vector>(m, "StdVec_Triangle32"); m.attr("StdVec_Triangle") = m.attr("StdVec_Triangle32"); nb::bind_vector>(m, "StdVec_Triangle16"); nb::bind_vector>(m, "StdVec_Vec3s"); } ================================================ FILE: python-nb/memory-footprint.cc ================================================ /// Copyright 2025 INRIA #include "coal/shape/geometric_shapes.h" #include "coal/BVH/BVH_model.h" #include "coal/serialization/memory.h" #include "fwd.h" using namespace coal; template void defComputeMemoryFootprint(nb::module_& m) { m.def("computeMemoryFootprint", &computeMemoryFootprint); } void exposeComputeMemoryFootprint(nb::module_& m) { defComputeMemoryFootprint(m); defComputeMemoryFootprint(m); defComputeMemoryFootprint(m); defComputeMemoryFootprint(m); defComputeMemoryFootprint(m); defComputeMemoryFootprint(m); defComputeMemoryFootprint(m); defComputeMemoryFootprint(m); defComputeMemoryFootprint(m); defComputeMemoryFootprint>(m); defComputeMemoryFootprint>(m); defComputeMemoryFootprint>(m); } ================================================ FILE: python-nb/module.cc ================================================ /// Copyright 2025 INRIA #include "coal/config.hh" #include "coal/mesh_loader/loader.h" #include "fwd.h" using namespace nb::literals; inline constexpr bool checkVersionAtLeast(int major, int minor, int patch) { return COAL_VERSION_AT_LEAST(major, minor, patch); } inline constexpr bool checkVersionAtMost(int major, int minor, int patch) { return COAL_VERSION_AT_MOST(major, minor, patch); } void exposeVersion(nb::module_& m) { m.attr("__version__") = COAL_VERSION; m.attr("COAL_MAJOR_VERSION") = COAL_MAJOR_VERSION; m.attr("COAL_MINOR_VERSION") = COAL_MINOR_VERSION; m.attr("COAL_PATCH_VERSION") = COAL_PATCH_VERSION; m.attr("WITH_QHULL") = #if COAL_HAS_QHULL true; #else false; #endif m.attr("WITH_OCTOMAP") = #if COAL_HAS_OCTOMAP true; #else false; #endif m.def("checkVersionAtLeast", &checkVersionAtLeast, "major"_a, "minor"_a, "patch"_a, "Checks if the current version of coal is at least the version " "provided by the input arguments."); m.def("checkVersionAtMost", &checkVersionAtMost, "major"_a, "minor"_a, "patch"_a, "Checks if the current version of coal is at most the version provided " "by the input arguments."); } void exposeMeshLoader(nb::module_& m) { using namespace coal; nb::handle cl_cur = nb::type(); if (cl_cur.is_valid()) { return; } if (!nb::type().is_valid()) { nb::class_(m, "MeshLoader") .def(nb::init(), "node_type"_a = BV_OBBRSS) .def("load", &MeshLoader::load, "filename"_a, "scale"_a = Vec3s::Ones()) .def("loadOctree", &MeshLoader::loadOctree, "filename"_a); } if (!nb::type().is_valid()) { nb::class_(m, "CachedMeshLoader") .def(nb::init(), "node_type"_a = BV_OBBRSS); } } void exposeMaths(nb::module_& m); void exposeCollisionGeometries(nb::module_& m); void exposeCollisionObject(nb::module_& m); void exposeCollisionAPI(nb::module_& m); void exposeContactPatchAPI(nb::module_& m); void exposeDistanceAPI(nb::module_& m); void exposeGJK(nb::module_& m); #ifdef COAL_HAS_OCTOMAP void exposeOctree(nb::module_& m); #endif void exposeBroadPhase(nb::module_& m); NB_MODULE(COAL_PYTHON_LIBNAME, m) { exposeVersion(m); exposeMaths(m); exposeCollisionGeometries(m); exposeCollisionObject(m); exposeCollisionAPI(m); exposeContactPatchAPI(m); exposeDistanceAPI(m); exposeGJK(m); exposeMeshLoader(m); #ifdef COAL_HAS_OCTOMAP exposeOctree(m); #endif exposeBroadPhase(m); } ================================================ FILE: python-nb/octree.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" #include "coal/octree.h" #include "fwd.h" using namespace coal; using namespace nb::literals; nb::bytes tobytes(const OcTree& self) { std::vector bytes = self.tobytes(); return nb::bytes(reinterpret_cast(bytes.data()), bytes.size()); } void exposeOctree(nb::module_& m) { nb::class_(m, "OcTree") .def(nb::init(), "resolution"_a) .def("clone", &OcTree::clone, nb::rv_policy::take_ownership) .def("getTreeDepth", &OcTree::getTreeDepth) .def("size", &OcTree::size) .def("getResolution", &OcTree::getResolution) .def("getOccupancyThres", &OcTree::getOccupancyThres) .def("getFreeThres", &OcTree::getFreeThres) .def("getDefaultOccupancy", &OcTree::getDefaultOccupancy) .def("setCellDefaultOccupancy", &OcTree::setCellDefaultOccupancy) .def("setOccupancyThres", &OcTree::setOccupancyThres) .def("setFreeThres", &OcTree::setFreeThres) .def("getRootBV", &OcTree::getRootBV) .def("toBoxes", &OcTree::toBoxes) .def("tobytes", &tobytes); } ================================================ FILE: python-nb/pickle.hh ================================================ /// Copyright 2025 INRIA #ifndef COAL_PYTHON_NB_PICKLE_HH #define COAL_PYTHON_NB_PICKLE_HH #include #include #include #include "fwd.h" #include namespace coal::python { namespace v2 { /// See: https://nanobind.readthedocs.io/en/latest/classes.html#pickling template struct PickleVisitor : nb::def_visitor> { template void execute(nb::class_& cl) { using namespace nb::literals; cl.def("__getstate__", [](const T& obj) -> std::string { std::stringstream ss; boost::archive::text_oarchive oa(ss); oa & obj; return ss.str(); }).def("__setstate__", [](T& obj, const std::string& state) { std::istringstream is(state); boost::archive::text_iarchive ia(is, boost::archive::no_codecvt); new (&obj) T(); ia >> obj; }); } }; } // namespace v2 } // namespace coal::python #endif // ifndef COAL_PYTHON_NB_PICKLE_HH ================================================ FILE: python-nb/serializable.hh ================================================ /// Copyright 2025 INRIA #ifndef COAL_PYTHON_NB_SERIALIZABLE_HH #define COAL_PYTHON_NB_SERIALIZABLE_HH #include "fwd.h" #include "coal/serialization/archive.h" #include "coal/serialization/serializer.h" namespace coal::python { namespace v2 { using Serializer = ::coal::serialization::Serializer; template struct SerializableVisitor : nb::def_visitor> { template void execute(nb::class_& cl) { using namespace nb::literals; cl.def("saveToText", &Serializer::saveToText, nb::arg("filename"), "Saves *this inside a text file.") .def("loadFromText", &Serializer::loadFromText, nb::arg("filename"), "Loads *this from a text file.") .def("saveToString", &Serializer::saveToString, "Parses the current object to a string.") .def("loadFromString", &Serializer::loadFromString, "string"_a, "Parses from the input string the content of the current object.") .def("saveToXML", &Serializer::saveToXML, "filename"_a, "tag_name"_a, "Saves *this inside a XML file.") .def("loadFromXML", &Serializer::loadFromXML, "filename"_a, "tag_name"_a, "Loads *this from a XML file.") .def("saveToBinary", &Serializer::saveToBinary, "filename"_a, "Saves *this inside a binary file.") .def("loadFromBinary", &Serializer::loadFromBinary, "filename"_a, "Loads *this from a binary file.") .def("saveToBuffer", &Serializer::saveToBuffer, "buffer"_a, "Saves *this inside a binary buffer.") .def("loadFromBuffer", &Serializer::loadFromBuffer, "buffer"_a, "Loads *this from a binary buffer."); } }; } // namespace v2 } // namespace coal::python #endif // ifndef COAL_PYTHON_NB_SERIALIZABLE_HH ================================================ FILE: python-nb/shapes.cc ================================================ /// Copyright 2025 INRIA #include "coal/fwd.hh" #include "coal/shape/geometric_shapes.h" #include "coal/shape/convex.h" #include "coal/BVH/BVH_model.h" #include "coal/serialization/geometric_shapes.h" #include "coal/serialization/convex.h" #include "pickle.hh" #include "serializable.hh" #include "fwd.h" using namespace coal; using namespace nb::literals; typedef std::vector Vec3ss; typedef Eigen::Matrix RowMatrixX3; typedef Eigen::Map MapRowMatrixX3; typedef Eigen::Ref RefRowMatrixX3; typedef Eigen::Map MapVecXs; typedef Eigen::Ref RefVecXs; template void exposeConvexBase(nb::module_& m, const std::string& classname) { typedef ConvexBaseTpl ConvexBaseType; nb::class_(m, classname.c_str()) .DEF_RO_CLASS_ATTRIB(ConvexBaseType, center) .DEF_RO_CLASS_ATTRIB(ConvexBaseType, num_points) .DEF_RO_CLASS_ATTRIB(ConvexBaseType, num_normals_and_offsets) .def( "point", [](const ConvexBaseType& convex, unsigned int i) -> Vec3s& { if (i >= convex.num_points) { throw std::out_of_range("index is out of range"); } return (*(convex.points))[i]; }, "index"_a, "Retrieve the point given by its index.", nb::rv_policy::reference_internal) .def( "points", [](const ConvexBaseType& convex, unsigned int i) -> Vec3s& { if (i >= convex.num_points) { throw std::out_of_range("index is out of range"); } return (*(convex.points))[i]; }, "index"_a, "Retrieve the point given by its index.", nb::rv_policy::reference_internal) .def( "points", [](const ConvexBaseType& convex) -> RefRowMatrixX3 { return MapRowMatrixX3((*(convex.points))[0].data(), convex.num_points, 3); }, "Retrieve all the points.", nb::rv_policy::reference_internal) .def( "normal", [](const ConvexBaseType& convex, unsigned int i) -> Vec3s& { if (i >= convex.num_normals_and_offsets) { throw std::out_of_range("index is out of range"); } return (*(convex.normals))[i]; }, "index"_a, "Retrieve the normal given by its index.", nb::rv_policy::reference_internal) .def( "normals", [](const ConvexBaseType& convex) -> RefRowMatrixX3 { return MapRowMatrixX3((*(convex.normals))[0].data(), convex.num_normals_and_offsets, 3); }, "Retrieve all the normals.", nb::rv_policy::reference_internal) .def( "offset", [](const ConvexBaseType& convex, unsigned int i) -> Scalar { if (i >= convex.num_normals_and_offsets) { throw std::out_of_range("index is out of range"); } return (*(convex.offsets))[i]; }, "index"_a, "Retrieve the offset given by its index.") .def( "offsets", [](const ConvexBaseType& convex) -> RefVecXs { return MapVecXs(convex.offsets->data(), convex.num_normals_and_offsets, 1); }, "Retrieve all the offsets.", nb::rv_policy::reference_internal) .def("neighbors", [](const ConvexBaseType& convex, unsigned int i) -> nb::list { if (i >= convex.num_points) { throw std::out_of_range("index is out of range"); } nb::list n; const std::vector& neighbors_ = *(convex.neighbors); for (unsigned char j = 0; j < neighbors_[i].count; ++j) { n.append(convex.neighbor(IndexType(i), j)); } return n; }) .def_static( "convexHull", [](const Vec3ss& points, bool keepTri, nb::handle qhullCommand) -> ConvexBaseType* { const char* qhullCommand_a = nullptr; if (!qhullCommand.is_none()) { qhullCommand_a = nb::cast(qhullCommand).c_str(); } return ConvexBaseType::convexHull(points.data(), (unsigned int)points.size(), keepTri, qhullCommand_a); }, "points"_a, "keepTri"_a, "qhullCommand"_a = nb::none(), nb::rv_policy::take_ownership) .def("clone", &ConvexBaseType::clone, nb::rv_policy::take_ownership); } template void exposeConvex(nb::module_& m, const std::string& classname) { typedef ConvexTpl ConvexType; typedef ConvexBaseTpl ConvexBaseType; typedef typename PolygonT::IndexType IndexType; typedef TriangleTpl TriangleType; nb::class_(m, classname.c_str()) .def(nb::init<>()) .def("__init__", [](ConvexType* self, const Vec3ss& pts, const std::vector& tri) { std::shared_ptr points(new Vec3ss(pts.size())); Vec3ss& points_ = *points; for (std::size_t i = 0; i < pts.size(); ++i) { points_[i] = pts[i]; } std::shared_ptr> tris( new std::vector(tri.size())); std::vector& tris_ = *tris; for (std::size_t i = 0; i < tri.size(); ++i) { tris_[i] = tri[i]; } new (self) ConvexType(*(shared_ptr( new ConvexType(points, (unsigned int)pts.size(), tris, (unsigned int)tri.size())))); }) .def(nb::init(), "other_"_a) .DEF_RO_CLASS_ATTRIB(ConvexType, num_polygons) .def( "polygons", [](const ConvexType& convex, unsigned int i) -> TriangleType { if (i >= convex.num_polygons) { throw std::out_of_range("index is out of range"); } return (*convex.polygons)[i]; }, "index"_a, "Retrieve the polygon given by its index.") .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); } void exposeShapes(nb::module_& m) { nb::class_(m, "ShapeBase") .def("setSweptSphereRadius", &ShapeBase::setSweptSphereRadius) .def("getSweptSphereRadius", &ShapeBase::getSweptSphereRadius); nb::class_(m, "Box") .def(nb::init<>()) .def(nb::init(), "other"_a) .def(nb::init(), "x"_a, "y"_a, "z"_a) .def(nb::init(), "side_"_a) .DEF_RW_CLASS_ATTRIB(Box, halfSide) .def("clone", &Box::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); nb::class_(m, "Capsule") .def(nb::init<>()) .def(nb::init(), "radius"_a, "lz_"_a) .def(nb::init(), "other_"_a) .DEF_RW_CLASS_ATTRIB(Capsule, radius) .DEF_RW_CLASS_ATTRIB(Capsule, halfLength) .def("clone", &Capsule::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); nb::class_(m, "Cone") .def(nb::init<>()) .def(nb::init(), "radius"_a, "lz_"_a) .def(nb::init(), "other_"_a) .DEF_RW_CLASS_ATTRIB(Cone, radius) .DEF_RW_CLASS_ATTRIB(Cone, halfLength) .def("clone", &Cone::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); nb::class_(m, "Cylinder") .def(nb::init<>()) .def(nb::init(), "radius"_a, "lz_"_a) .def(nb::init(), "other_"_a) .DEF_RW_CLASS_ATTRIB(Cylinder, radius) .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength) .def("clone", &Cylinder::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); nb::class_(m, "Halfspace") .def(nb::init<>()) .def(nb::init(), "n_"_a, "d_"_a) .def(nb::init(), "other_"_a) .def(nb::init(), "a"_a, "b"_a, "c"_a, "d_"_a) .DEF_RW_CLASS_ATTRIB(Halfspace, n) .DEF_RW_CLASS_ATTRIB(Halfspace, d) .def("clone", &Halfspace::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); nb::class_(m, "Plane") .def(nb::init<>()) .def(nb::init(), "n_"_a, "d_"_a) .def(nb::init(), "other_"_a) .def(nb::init(), "a"_a, "b"_a, "c"_a, "d_"_a) .DEF_RW_CLASS_ATTRIB(Plane, n) .DEF_RW_CLASS_ATTRIB(Plane, d) .def("clone", &Plane::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); nb::class_(m, "Sphere") .def(nb::init<>()) .def(nb::init(), "radius_"_a) .def(nb::init(), "other_"_a) .DEF_RW_CLASS_ATTRIB(Sphere, radius) .def("clone", &Sphere::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); nb::class_(m, "Ellipsoid") .def(nb::init<>()) .def(nb::init(), "rx"_a, "ry"_a, "rz"_a) .def(nb::init(), "radii"_a) .def(nb::init(), "other_"_a) .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) .def("clone", &Ellipsoid::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); nb::class_(m, "TriangleP") .def(nb::init<>()) .def(nb::init(), "a_"_a, "b_"_a, "c_"_a) .def(nb::init(), "other_"_a) .DEF_RW_CLASS_ATTRIB(TriangleP, a) .DEF_RW_CLASS_ATTRIB(TriangleP, b) .DEF_RW_CLASS_ATTRIB(TriangleP, c) .def("clone", &TriangleP::clone, nb::rv_policy::take_ownership) .def(python::v2::PickleVisitor()) .def(python::v2::SerializableVisitor()) .def(nanoeigenpy::IdVisitor()); exposeConvexBase(m, "ConvexBase16"); exposeConvexBase(m, "ConvexBase32"); exposeConvex(m, "Convex16"); exposeConvex(m, "Convex32"); m.attr("Convex") = m.attr("Convex32"); } ================================================ FILE: src/BV/AABB.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/BV/AABB.h" #include "coal/shape/geometric_shapes.h" #include "coal/collision_data.h" #include namespace coal { AABB::AABB() : min_(Vec3s::Constant((std::numeric_limits::max)())), max_(Vec3s::Constant(-(std::numeric_limits::max)())) {} bool AABB::overlap(const AABB& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const { const Scalar break_distance_squared = request.break_distance * request.break_distance; sqrDistLowerBound = (min_ - other.max_ - Vec3s::Constant(request.security_margin)) .array() .max(Scalar(0)) .matrix() .squaredNorm(); if (sqrDistLowerBound > break_distance_squared) return false; sqrDistLowerBound = (other.min_ - max_ - Vec3s::Constant(request.security_margin)) .array() .max(Scalar(0)) .matrix() .squaredNorm(); if (sqrDistLowerBound > break_distance_squared) return false; return true; } Scalar AABB::distance(const AABB& other, Vec3s* P, Vec3s* Q) const { Scalar result = 0; for (Eigen::DenseIndex i = 0; i < 3; ++i) { const Scalar& amin = min_[i]; const Scalar& amax = max_[i]; const Scalar& bmin = other.min_[i]; const Scalar& bmax = other.max_[i]; if (amin > bmax) { Scalar delta = bmax - amin; result += delta * delta; if (P && Q) { (*P)[i] = amin; (*Q)[i] = bmax; } } else if (bmin > amax) { Scalar delta = amax - bmin; result += delta * delta; if (P && Q) { (*P)[i] = amax; (*Q)[i] = bmin; } } else { if (P && Q) { if (bmin >= amin) { Scalar t = Scalar(0.5) * (amax + bmin); (*P)[i] = t; (*Q)[i] = t; } else { Scalar t = Scalar(0.5) * (amin + bmax); (*P)[i] = t; (*Q)[i] = t; } } } } return std::sqrt(result); } Scalar AABB::distance(const AABB& other) const { Scalar result = 0; for (Eigen::DenseIndex i = 0; i < 3; ++i) { const Scalar& amin = min_[i]; const Scalar& amax = max_[i]; const Scalar& bmin = other.min_[i]; const Scalar& bmax = other.max_[i]; if (amin > bmax) { Scalar delta = bmax - amin; result += delta * delta; } else if (bmin > amax) { Scalar delta = amax - bmin; result += delta * delta; } } return std::sqrt(result); } bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2) { AABB bb1(translate(rotate(b1, R0), T0)); return bb1.overlap(b2); } bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound) { AABB bb1(translate(rotate(b1, R0), T0)); return bb1.overlap(b2, request, sqrDistLowerBound); } bool AABB::overlap(const Plane& p) const { // Convert AABB to a (box, transform) representation and compute the support // points in the directions normal and -normal. // If both points lie on different sides of the plane, there is an overlap // between the AABB and the plane. Otherwise, there is no overlap. const Vec3s halfside = (this->max_ - this->min_) / 2; const Vec3s center = (this->max_ + this->min_) / 2; const Vec3s support1 = (p.n.array() > 0).select(halfside, -halfside) + center; const Vec3s support2 = ((-p.n).array() > 0).select(halfside, -halfside) + center; const Scalar dist1 = p.n.dot(support1) - p.d; const Scalar dist2 = p.n.dot(support2) - p.d; const int sign1 = (dist1 > 0) ? 1 : -1; const int sign2 = (dist2 > 0) ? 1 : -1; if (p.getSweptSphereRadius() > 0) { if (sign1 != sign2) { // Supports are on different sides of the plane. There is an overlap. return true; } // Both supports are on the same side of the plane. // We now need to check if they are on the same side of the plane inflated // by the swept-sphere radius. const Scalar ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius(); const Scalar ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius(); const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1; const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1; return ssr_sign1 != ssr_sign2; } return (sign1 != sign2); } bool AABB::overlap(const Halfspace& hs) const { // Convert AABB to a (box, transform) representation and compute the support // points in the direction -normal. // If the support is below the plane defined by the halfspace, there is an // overlap between the AABB and the halfspace. Otherwise, there is no // overlap. Vec3s halfside = (this->max_ - this->min_) / 2; Vec3s center = (this->max_ + this->min_) / 2; Vec3s support = ((-hs.n).array() > 0).select(halfside, -halfside) + center; return (hs.signedDistance(support) < 0); } } // namespace coal ================================================ FILE: src/BV/OBB.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan, Florent Lamiraux */ #include "coal/BV/OBB.h" #include "coal/BVH/BVH_utility.h" #include "coal/math/transform.h" #include "coal/collision_data.h" #include "coal/internal/tools.h" #include #include namespace coal { /// @brief Compute the 8 vertices of a OBB inline void computeVertices(const OBB& b, Vec3s vertices[8]) { Matrix3s extAxes(b.axes * b.extent.asDiagonal()); vertices[0].noalias() = b.To + extAxes * Vec3s(-1, -1, -1); vertices[1].noalias() = b.To + extAxes * Vec3s(1, -1, -1); vertices[2].noalias() = b.To + extAxes * Vec3s(1, 1, -1); vertices[3].noalias() = b.To + extAxes * Vec3s(-1, 1, -1); vertices[4].noalias() = b.To + extAxes * Vec3s(-1, -1, 1); vertices[5].noalias() = b.To + extAxes * Vec3s(1, -1, 1); vertices[6].noalias() = b.To + extAxes * Vec3s(1, 1, 1); vertices[7].noalias() = b.To + extAxes * Vec3s(-1, 1, 1); } /// @brief OBB merge method when the centers of two smaller OBB are far away inline OBB merge_largedist(const OBB& b1, const OBB& b2) { OBB b; Vec3s vertex[16]; computeVertices(b1, vertex); computeVertices(b2, vertex + 8); Matrix3s M; Vec3s E[3]; Scalar s[3] = {0, 0, 0}; // obb axes b.axes.col(0).noalias() = (b1.To - b2.To).normalized(); Vec3s vertex_proj[16]; for (int i = 0; i < 16; ++i) vertex_proj[i].noalias() = vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0)); getCovariance(vertex_proj, NULL, NULL, NULL, 16, M); eigen(M, s, E); int min, mid, max; if (s[0] > s[1]) { max = 0; min = 1; } else { min = 0; max = 1; } if (s[2] < s[min]) { mid = min; min = 2; } else if (s[2] > s[max]) { mid = max; max = 2; } else { mid = 2; } b.axes.col(1) << E[0][max], E[1][max], E[2][max]; b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid]; // set obb centers and extensions Vec3s center, extent; getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent); b.To.noalias() = center; b.extent.noalias() = extent; return b; } /// @brief OBB merge method when the centers of two smaller OBB are close inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { OBB b; b.To = (b1.To + b2.To) * 0.5; Quats q0(b1.axes), q1(b2.axes); if (q0.dot(q1) < 0) q1.coeffs() *= -1; Quats q((q0.coeffs() + q1.coeffs()).normalized()); b.axes = q.toRotationMatrix(); Vec3s vertex[8], diff; Scalar real_max = (std::numeric_limits::max)(); Vec3s pmin(real_max, real_max, real_max); Vec3s pmax(-real_max, -real_max, -real_max); computeVertices(b1, vertex); for (int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; for (int j = 0; j < 3; ++j) { Scalar dot = diff.dot(b.axes.col(j)); if (dot > pmax[j]) pmax[j] = dot; else if (dot < pmin[j]) pmin[j] = dot; } } computeVertices(b2, vertex); for (int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; for (int j = 0; j < 3; ++j) { Scalar dot = diff.dot(b.axes.col(j)); if (dot > pmax[j]) pmax[j] = dot; else if (dot < pmin[j]) pmin[j] = dot; } } for (int j = 0; j < 3; ++j) { b.To.noalias() += (b.axes.col(j) * (0.5 * (pmax[j] + pmin[j]))); b.extent[j] = Scalar(0.5) * (pmax[j] - pmin[j]); } return b; } bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b) { Scalar t, s; const Scalar reps = Scalar(1e-6); Matrix3s Bf(B.array().abs() + reps); // Bf += reps; // if any of these tests are one-sided, then the polyhedra are disjoint // A1 x A2 = A0 t = ((T[0] < 0.0) ? -T[0] : T[0]); // if(t > (a[0] + Bf.dotX(b))) if (t > (a[0] + Bf.row(0).dot(b))) return true; // B1 x B2 = B0 // s = B.transposeDotX(T); s = B.col(0).dot(T); t = ((s < 0.0) ? -s : s); // if(t > (b[0] + Bf.transposeDotX(a))) if (t > (b[0] + Bf.col(0).dot(a))) return true; // A2 x A0 = A1 t = ((T[1] < 0.0) ? -T[1] : T[1]); // if(t > (a[1] + Bf.dotY(b))) if (t > (a[1] + Bf.row(1).dot(b))) return true; // A0 x A1 = A2 t = ((T[2] < 0.0) ? -T[2] : T[2]); // if(t > (a[2] + Bf.dotZ(b))) if (t > (a[2] + Bf.row(2).dot(b))) return true; // B2 x B0 = B1 // s = B.transposeDotY(T); s = B.col(1).dot(T); t = ((s < 0.0) ? -s : s); // if(t > (b[1] + Bf.transposeDotY(a))) if (t > (b[1] + Bf.col(1).dot(a))) return true; // B0 x B1 = B2 // s = B.transposeDotZ(T); s = B.col(2).dot(T); t = ((s < 0.0) ? -s : s); // if(t > (b[2] + Bf.transposeDotZ(a))) if (t > (b[2] + Bf.col(2).dot(a))) return true; // A0 x B0 s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); if (t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) return true; // A0 x B1 s = T[2] * B(1, 1) - T[1] * B(2, 1); t = ((s < 0.0) ? -s : s); if (t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) return true; // A0 x B2 s = T[2] * B(1, 2) - T[1] * B(2, 2); t = ((s < 0.0) ? -s : s); if (t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) return true; // A1 x B0 s = T[0] * B(2, 0) - T[2] * B(0, 0); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) return true; // A1 x B1 s = T[0] * B(2, 1) - T[2] * B(0, 1); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) return true; // A1 x B2 s = T[0] * B(2, 2) - T[2] * B(0, 2); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) return true; // A2 x B0 s = T[1] * B(0, 0) - T[0] * B(1, 0); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) return true; // A2 x B1 s = T[1] * B(0, 1) - T[0] * B(1, 1); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) return true; // A2 x B2 s = T[1] * B(0, 2) - T[0] * B(1, 2); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) return true; return false; } namespace internal { inline Scalar obbDisjoint_check_A_axis(const Vec3s& T, const Vec3s& a, const Vec3s& b, const Matrix3s& Bf) { // |T| - |B| * b - a Vec3s AABB_corner(T.cwiseAbs() - a); AABB_corner.noalias() -= Bf * b; return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm(); } inline Scalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Matrix3s& Bf) { // Bf = |B| // | B^T T| - Bf^T * a - b Scalar s, t = 0; s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; if (s > 0) t += s * s; s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; if (s > 0) t += s * s; s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2]; if (s > 0) t += s * s; return t; } template struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi { static inline bool run(int ia, int ja, int ka, const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Matrix3s& Bf, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 < 1e-6) return false; const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } return false; } // namespace internal }; // namespace coal } // namespace internal // B, T orientation and position of 2nd OBB in frame of 1st OBB, // a extent of 1st OBB, // b extent of 2nd OBB. // // This function tests whether bounding boxes should be broken down. // bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T, const Vec3s& a_, const Vec3s& b_, const CollisionRequest& request, Scalar& squaredLowerBoundDistance) { assert(request.security_margin > -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) - 10 * Eigen::NumTraits::epsilon() && "A negative security margin could not be lower than the OBB extent."); // const Scalar breakDistance(request.break_distance + // request.security_margin); const Scalar breakDistance2 = request.break_distance * request.break_distance; Matrix3s Bf(B.cwiseAbs()); const Vec3s a((a_ + Vec3s::Constant(request.security_margin / 2)) .array() .max(Scalar(0))); const Vec3s b((b_ + Vec3s::Constant(request.security_margin / 2)) .array() .max(Scalar(0))); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; squaredLowerBoundDistance = internal::obbDisjoint_check_B_axis(B, T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; // Ai x Bj int ja = 1, ka = 2; for (int ia = 0; ia < 3; ++ia) { if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run( ia, ja, ka, B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run( ia, ja, ka, B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run( ia, ja, ka, B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; ja = ka; ka = ia; } return false; } bool OBB::overlap(const OBB& other) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part Vec3s T(axes.transpose() * (other.To - To)); Matrix3s R(axes.transpose() * other.axes); return !obbDisjoint(R, T, extent, other.extent); } bool OBB::overlap(const OBB& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part // Vec3s t = other.To - To; // T2 - T1 // Vec3s T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) // Matrix3s R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), // axis[0].dot(other.axis[2]), // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), // axis[1].dot(other.axis[2]), // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), // axis[2].dot(other.axis[2])); Vec3s T(axes.transpose() * (other.To - To)); Matrix3s R(axes.transpose() * other.axes); return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request, sqrDistLowerBound); } bool OBB::contain(const Vec3s& p) const { Vec3s local_p(p - To); Scalar proj = local_p.dot(axes.col(0)); if ((proj > extent[0]) || (proj < -extent[0])) return false; proj = local_p.dot(axes.col(1)); if ((proj > extent[1]) || (proj < -extent[1])) return false; proj = local_p.dot(axes.col(2)); if ((proj > extent[2]) || (proj < -extent[2])) return false; return true; } OBB& OBB::operator+=(const Vec3s& p) { OBB bvp; bvp.To = p; bvp.axes.noalias() = axes; bvp.extent.setZero(); *this += bvp; return *this; } OBB OBB::operator+(const OBB& other) const { Vec3s center_diff = To - other.To; Scalar max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); Scalar max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); if (center_diff.norm() > 2 * (max_extent + max_extent2)) { return merge_largedist(*this, other); } else { return merge_smalldist(*this, other); } } Scalar OBB::distance(const OBB& /*other*/, Vec3s* /*P*/, Vec3s* /*Q*/) const { std::cerr << "OBB distance not implemented!" << std::endl; return 0.0; } bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2) { Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To); Vec3s T(b1.axes.transpose() * Ttemp); Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); return !obbDisjoint(R, T, b1.extent, b2.extent); } bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound) { Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To); Vec3s T(b1.axes.transpose() * Ttemp); Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request, sqrDistLowerBound); } OBB translate(const OBB& bv, const Vec3s& t) { OBB res(bv); res.To += t; return res; } } // namespace coal ================================================ FILE: src/BV/OBB.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2014, CNRS * Author: Florent Lamiraux * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of CNRS nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef COAL_SRC_OBB_H #define COAL_SRC_OBB_H namespace coal { bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const CollisionRequest& request, Scalar& squaredLowerBoundDistance); bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b); } // namespace coal #endif // COAL_SRC_OBB_H ================================================ FILE: src/BV/OBBRSS.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/BV/OBBRSS.h" namespace coal { OBBRSS translate(const OBBRSS& bv, const Vec3s& t) { OBBRSS res(bv); res.obb.To += t; res.rss.Tr += t; return res; } } // namespace coal ================================================ FILE: src/BV/RSS.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/BV/RSS.h" #include "coal/BVH/BVH_utility.h" #include "coal/internal/tools.h" #include "coal/collision_data.h" #include namespace coal { /// @brief Clip value between a and b void clipToRange(Scalar& val, Scalar a, Scalar b) { if (val < a) val = a; else if (val > b) val = b; } /// @brief Finds the parameters t & u corresponding to the two closest points on /// a pair of line segments. The first segment is defined as Pa + A*t, 0 <= t <= /// a, where "Pa" is one endpoint of the segment, "A" is a unit vector pointing /// to the other endpoint, and t is a scalar that produces all the points /// between the two endpoints. Since "A" is a unit vector, "a" is the segment's /// length. The second segment is defined as Pb + B*u, 0 <= u <= b. Many of the /// terms needed by the algorithm are already computed for other purposes,so we /// just pass these terms into the function instead of complete specifications /// of each segment. "T" in the dot products is the vector betweeen Pa and Pb. /// Reference: "On fast computation of distance between line segments." Vladimir /// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. void segCoords(Scalar& t, Scalar& u, Scalar a, Scalar b, Scalar A_dot_B, Scalar A_dot_T, Scalar B_dot_T) { Scalar denom = 1 - A_dot_B * A_dot_B; if (denom == 0) t = 0; else { t = (A_dot_T - B_dot_T * A_dot_B) / denom; clipToRange(t, 0, a); } u = t * A_dot_B - B_dot_T; if (u < 0) { u = 0; t = A_dot_T; clipToRange(t, 0, a); } else if (u > b) { u = b; t = u * A_dot_B + A_dot_T; clipToRange(t, 0, a); } } /// @brief Returns whether the nearest point on rectangle edge /// Pb + B*u, 0 <= u <= b, to the rectangle edge, /// Pa + A*t, 0 <= t <= a, is within the half space /// determined by the point Pa and the direction Anorm. /// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. bool inVoronoi(Scalar a, Scalar b, Scalar Anorm_dot_B, Scalar Anorm_dot_T, Scalar A_dot_B, Scalar A_dot_T, Scalar B_dot_T) { if (fabs(Anorm_dot_B) < 1e-7) return false; Scalar t, u, v; u = -Anorm_dot_T / Anorm_dot_B; clipToRange(u, 0, b); t = u * A_dot_B + A_dot_T; clipToRange(t, 0, a); v = t * A_dot_B - B_dot_T; if (Anorm_dot_B > 0) { if (v > (u + 1e-7)) return true; } else { if (v < (u - 1e-7)) return true; } return false; } /// @brief Distance between two oriented rectangles; P and Q (optional return /// values) are the closest points in the rectangles, both are in the local /// frame of the first rectangle. Scalar rectDistance(const Matrix3s& Rab, Vec3s const& Tab, const Scalar a[2], const Scalar b[2], Vec3s* P = NULL, Vec3s* Q = NULL) { Scalar A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; A0_dot_B0 = Rab(0, 0); A0_dot_B1 = Rab(0, 1); A1_dot_B0 = Rab(1, 0); A1_dot_B1 = Rab(1, 1); Scalar aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; Scalar bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; aA0_dot_B0 = a[0] * A0_dot_B0; aA0_dot_B1 = a[0] * A0_dot_B1; aA1_dot_B0 = a[1] * A1_dot_B0; aA1_dot_B1 = a[1] * A1_dot_B1; bA0_dot_B0 = b[0] * A0_dot_B0; bA1_dot_B0 = b[0] * A1_dot_B0; bA0_dot_B1 = b[1] * A0_dot_B1; bA1_dot_B1 = b[1] * A1_dot_B1; Vec3s Tba(Rab.transpose() * Tab); Vec3s S; Scalar t, u; // determine if any edge pair contains the closest points Scalar ALL_x, ALU_x, AUL_x, AUU_x; Scalar BLL_x, BLU_x, BUL_x, BUU_x; Scalar LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; ALL_x = -Tba[0]; ALU_x = ALL_x + aA1_dot_B0; AUL_x = ALL_x + aA0_dot_B0; AUU_x = ALU_x + aA0_dot_B0; if (ALL_x < ALU_x) { LA1_lx = ALL_x; LA1_ux = ALU_x; UA1_lx = AUL_x; UA1_ux = AUU_x; } else { LA1_lx = ALU_x; LA1_ux = ALL_x; UA1_lx = AUU_x; UA1_ux = AUL_x; } BLL_x = Tab[0]; BLU_x = BLL_x + bA0_dot_B1; BUL_x = BLL_x + bA0_dot_B0; BUU_x = BLU_x + bA0_dot_B0; if (BLL_x < BLU_x) { LB1_lx = BLL_x; LB1_ux = BLU_x; UB1_lx = BUL_x; UB1_ux = BUU_x; } else { LB1_lx = BLU_x; LB1_ux = BLL_x; UB1_lx = BUU_x; UB1_ux = BUL_x; } // UA1, UB1 if ((UA1_ux > b[0]) && (UB1_ux > a[0])) { if (((UA1_lx > b[0]) || inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1] - bA1_dot_B0)) && ((UB1_lx > a[0]) || inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0]; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; if (P && Q) { *P << a[0], t, 0; *Q = S + (*P); } return S.norm(); } } // UA1, LB1 if ((UA1_lx < 0) && (LB1_ux > a[0])) { if (((UA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0, A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) && ((LB1_lx > a[0]) || inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1); S[0] = Tab[0] + Rab(0, 1) * u - a[0]; S[1] = Tab[1] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 1) * u; if (P && Q) { *P << a[0], t, 0; *Q = S + (*P); } return S.norm(); } } // LA1, UB1 if ((LA1_ux > b[0]) && (UB1_lx < 0)) { if (((LA1_lx > b[0]) || inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0)) && ((UB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0, A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; if (P && Q) { *P << 0, t, 0; *Q = S + (*P); } return S.norm(); } } // LA1, LB1 if ((LA1_lx < 0) && (LB1_lx < 0)) { if (((LA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1, -Tba[1], -Tab[1])) && ((LB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1, Tab[1], Tba[1]))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]); S[0] = Tab[0] + Rab(0, 1) * u; S[1] = Tab[1] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 1) * u; if (P && Q) { *P << 0, t, 0; *Q = S + (*P); } return S.norm(); } } Scalar ALL_y, ALU_y, AUL_y, AUU_y; ALL_y = -Tba[1]; ALU_y = ALL_y + aA1_dot_B1; AUL_y = ALL_y + aA0_dot_B1; AUU_y = ALU_y + aA0_dot_B1; Scalar LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; if (ALL_y < ALU_y) { LA1_ly = ALL_y; LA1_uy = ALU_y; UA1_ly = AUL_y; UA1_uy = AUU_y; } else { LA1_ly = ALU_y; LA1_uy = ALL_y; UA1_ly = AUU_y; UA1_uy = AUL_y; } if (BLL_x < BUL_x) { LB0_lx = BLL_x; LB0_ux = BUL_x; UB0_lx = BLU_x; UB0_ux = BUU_x; } else { LB0_lx = BUL_x; LB0_ux = BLL_x; UB0_lx = BUU_x; UB0_ux = BLU_x; } // UA1, UB0 if ((UA1_uy > b[1]) && (UB0_ux > a[0])) { if (((UA1_ly > b[1]) || inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) && ((UB0_lx > a[0]) || inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0]; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; if (P && Q) { *P << a[0], t, 0; *Q = S + (*P); } return S.norm(); } } // UA1, LB0 if ((UA1_ly < 0) && (LB0_ux > a[0])) { if (((UA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1])) && ((LB0_lx > a[0]) || inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0); S[0] = Tab[0] + Rab(0, 0) * u - a[0]; S[1] = Tab[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 0) * u; if (P && Q) { *P << a[0], t, 0; *Q = S + (*P); } return S.norm(); } } // LA1, UB0 if ((LA1_uy > b[1]) && (UB0_lx < 0)) { if (((LA1_ly > b[1]) || inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1)) && ((UB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; if (P && Q) { *P << 0, t, 0; *Q = S + (*P); } return S.norm(); } } // LA1, LB0 if ((LA1_ly < 0) && (LB0_lx < 0)) { if (((LA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0, -Tba[0], -Tab[1])) && ((LB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0, Tab[1], Tba[0]))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]); S[0] = Tab[0] + Rab(0, 0) * u; S[1] = Tab[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 0) * u; if (P && Q) { *P << 0, t, 0; *Q = S + (*P); } return S.norm(); } } Scalar BLL_y, BLU_y, BUL_y, BUU_y; BLL_y = Tab[1]; BLU_y = BLL_y + bA1_dot_B1; BUL_y = BLL_y + bA1_dot_B0; BUU_y = BLU_y + bA1_dot_B0; Scalar LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; if (ALL_x < AUL_x) { LA0_lx = ALL_x; LA0_ux = AUL_x; UA0_lx = ALU_x; UA0_ux = AUU_x; } else { LA0_lx = AUL_x; LA0_ux = ALL_x; UA0_lx = AUU_x; UA0_ux = ALU_x; } if (BLL_y < BLU_y) { LB1_ly = BLL_y; LB1_uy = BLU_y; UB1_ly = BUL_y; UB1_uy = BUU_y; } else { LB1_ly = BLU_y; LB1_uy = BLL_y; UB1_ly = BUU_y; UB1_uy = BUL_y; } // UA0, UB1 if ((UA0_ux > b[0]) && (UB1_uy > a[1])) { if (((UA0_lx > b[0]) || inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) && ((UB1_ly > a[1]) || inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1]; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; if (P && Q) { *P << t, a[1], 0; *Q = S + (*P); } return S.norm(); } } // UA0, LB1 if ((UA0_lx < 0) && (LB1_uy > a[1])) { if (((UA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0])) && ((LB1_ly > a[1]) || inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1); S[0] = Tab[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 1) * u - a[1]; S[2] = Tab[2] + Rab(2, 1) * u; if (P && Q) { *P << t, a[1], 0; *Q = S + (*P); } return S.norm(); } } // LA0, UB1 if ((LA0_ux > b[0]) && (UB1_ly < 0)) { if (((LA0_lx > b[0]) || inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1], -bA0_dot_B0 - Tab[0])) && ((UB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; if (P && Q) { *P << t, 0, 0; *Q = S + (*P); } return S.norm(); } } // LA0, LB1 if ((LA0_lx < 0) && (LB1_ly < 0)) { if (((LA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1], -Tab[0])) && ((LB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1, Tab[0], Tba[1]))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]); S[0] = Tab[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 1) * u; S[2] = Tab[2] + Rab(2, 1) * u; if (P && Q) { *P << t, 0, 0; *Q = S + (*P); } return S.norm(); } } Scalar LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; if (ALL_y < AUL_y) { LA0_ly = ALL_y; LA0_uy = AUL_y; UA0_ly = ALU_y; UA0_uy = AUU_y; } else { LA0_ly = AUL_y; LA0_uy = ALL_y; UA0_ly = AUU_y; UA0_uy = ALU_y; } if (BLL_y < BUL_y) { LB0_ly = BLL_y; LB0_uy = BUL_y; UB0_ly = BLU_y; UB0_uy = BUU_y; } else { LB0_ly = BUL_y; LB0_uy = BLL_y; UB0_ly = BUU_y; UB0_uy = BLU_y; } // UA0, UB0 if ((UA0_uy > b[1]) && (UB0_uy > a[1])) { if (((UA0_ly > b[1]) || inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) && ((UB0_ly > a[1]) || inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1]; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; if (P && Q) { *P << t, a[1], 0; *Q = S + (*P); } return S.norm(); } } // UA0, LB0 if ((UA0_ly < 0) && (LB0_uy > a[1])) { if (((UA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0])) && ((LB0_ly > a[1]) || inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0); S[0] = Tab[0] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 0) * u - a[1]; S[2] = Tab[2] + Rab(2, 0) * u; if (P && Q) { *P << t, a[1], 0; *Q = S + (*P); } return S.norm(); } } // LA0, UB0 if ((LA0_uy > b[1]) && (UB0_ly < 0)) { if (((LA0_ly > b[1]) || inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0], -Tab[0] - bA0_dot_B1)) && ((UB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; if (P && Q) { *P << t, 0, 0; *Q = S + (*P); } return S.norm(); } } // LA0, LB0 if ((LA0_ly < 0) && (LB0_ly < 0)) { if (((LA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0, -Tba[0], -Tab[0])) && ((LB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0, Tab[0], Tba[0]))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]); S[0] = Tab[0] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 0) * u; S[2] = Tab[2] + Rab(2, 0) * u; if (P && Q) { *P << t, 0, 0; *Q = S + (*P); } return S.norm(); } } // no edges passed, take max separation along face normals Scalar sep1, sep2; if (Tab[2] > 0.0) { sep1 = Tab[2]; if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0); if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1); } else { sep1 = -Tab[2]; if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0); if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1); } if (Tba[2] < 0) { sep2 = -Tba[2]; if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2); if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2); } else { sep2 = Tba[2]; if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2); if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2); } if (sep1 >= sep2 && sep1 >= 0) { if (Tab[2] > 0) S << 0, 0, sep1; else S << 0, 0, -sep1; if (P && Q) { *Q = S; P->setZero(); } } if (sep2 >= sep1 && sep2 >= 0) { Vec3s Q_(Tab[0], Tab[1], Tab[2]); Vec3s P_; if (Tba[2] < 0) { P_[0] = Rab(0, 2) * sep2 + Tab[0]; P_[1] = Rab(1, 2) * sep2 + Tab[1]; P_[2] = Rab(2, 2) * sep2 + Tab[2]; } else { P_[0] = -Rab(0, 2) * sep2 + Tab[0]; P_[1] = -Rab(1, 2) * sep2 + Tab[1]; P_[2] = -Rab(2, 2) * sep2 + Tab[2]; } S = Q_ - P_; if (P && Q) { *P = P_; *Q = Q_; } } Scalar sep = (sep1 > sep2 ? sep1 : sep2); return (sep > 0 ? sep : 0); } bool RSS::overlap(const RSS& other) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part /// Then compute R1'(T2 - T1) Vec3s T(axes.transpose() * (other.Tr - Tr)); /// Now compute R1'R2 Matrix3s R(axes.transpose() * other.axes); Scalar dist = rectDistance(R, T, length, other.length); return (dist <= (radius + other.radius)); } bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] // R = b2^T RO^T b1 Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); Vec3s T(b1.axes.transpose() * Ttemp); Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); Scalar dist = rectDistance(R, T, b1.length, b2.length); return (dist <= (b1.radius + b2.radius)); } bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] // R = b2^T RO^T b1 Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); Vec3s T(b1.axes.transpose() * Ttemp); Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); Scalar dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius - request.security_margin; if (dist <= 0) return true; sqrDistLowerBound = dist * dist; return false; } bool RSS::contain(const Vec3s& p) const { Vec3s local_p = p - Tr; // FIXME: Vec3s proj (axes.transpose() * local_p); Scalar proj0 = local_p.dot(axes.col(0)); Scalar proj1 = local_p.dot(axes.col(1)); Scalar proj2 = local_p.dot(axes.col(2)); Scalar abs_proj2 = fabs(proj2); Vec3s proj(proj0, proj1, proj2); /// projection is within the rectangle if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0)) { return (abs_proj2 < radius); } else if ((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) { Scalar y = (proj1 > 0) ? length[1] : 0; Vec3s v(proj0, y, 0); return ((proj - v).squaredNorm() < radius * radius); } else if ((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) { Scalar x = (proj0 > 0) ? length[0] : 0; Vec3s v(x, proj1, 0); return ((proj - v).squaredNorm() < radius * radius); } else { Scalar x = (proj0 > 0) ? length[0] : 0; Scalar y = (proj1 > 0) ? length[1] : 0; Vec3s v(x, y, 0); return ((proj - v).squaredNorm() < radius * radius); } } RSS& RSS::operator+=(const Vec3s& p) { Vec3s local_p = p - Tr; Scalar proj0 = local_p.dot(axes.col(0)); Scalar proj1 = local_p.dot(axes.col(1)); Scalar proj2 = local_p.dot(axes.col(2)); Scalar abs_proj2 = fabs(proj2); Vec3s proj(proj0, proj1, proj2); // projection is within the rectangle if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0)) { if (abs_proj2 < radius) ; // do nothing else { const Scalar half = Scalar(0.5); radius = half * (radius + abs_proj2); // enlarge the r // change RSS origin position if (proj2 > 0) Tr[2] += half * (abs_proj2 - radius); else Tr[2] -= half * (abs_proj2 - radius); } } else if ((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) { Scalar y = (proj1 > 0) ? length[1] : 0; Vec3s v(proj0, y, 0); Scalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { Scalar delta_y = -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y); length[1] += delta_y; if (proj1 < 0) Tr[1] -= delta_y; } else { Scalar delta_y = fabs(proj1 - y); length[1] += delta_y; if (proj1 < 0) Tr[1] -= delta_y; const Scalar half = Scalar(0.5); if (proj2 > 0) Tr[2] += half * (abs_proj2 - radius); else Tr[2] -= half * (abs_proj2 - radius); } } } else if ((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) { Scalar x = (proj0 > 0) ? length[0] : 0; Vec3s v(x, proj1, 0); Scalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { Scalar delta_x = -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x); length[0] += delta_x; if (proj0 < 0) Tr[0] -= delta_x; } else { Scalar delta_x = fabs(proj0 - x); length[0] += delta_x; if (proj0 < 0) Tr[0] -= delta_x; const Scalar half = Scalar(0.5); if (proj2 > 0) Tr[2] += half * (abs_proj2 - radius); else Tr[2] -= half * (abs_proj2 - radius); } } } else { Scalar x = (proj0 > 0) ? length[0] : 0; Scalar y = (proj1 > 0) ? length[1] : 0; Vec3s v(x, y, 0); Scalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { Scalar diag = std::sqrt(new_r_sqr - proj2 * proj2); Scalar delta_diag = -std::sqrt(radius * radius - proj2 * proj2) + diag; Scalar delta_x = delta_diag / diag * fabs(proj0 - x); Scalar delta_y = delta_diag / diag * fabs(proj1 - y); length[0] += delta_x; length[1] += delta_y; if (proj0 < 0 && proj1 < 0) { Tr[0] -= delta_x; Tr[1] -= delta_y; } } else { Scalar delta_x = fabs(proj0 - x); Scalar delta_y = fabs(proj1 - y); length[0] += delta_x; length[1] += delta_y; if (proj0 < 0 && proj1 < 0) { Tr[0] -= delta_x; Tr[1] -= delta_y; } const Scalar half = Scalar(0.5); if (proj2 > 0) Tr[2] += half * (abs_proj2 - radius); else Tr[2] -= half * (abs_proj2 - radius); } } } return *this; } RSS RSS::operator+(const RSS& other) const { RSS bv; Vec3s v[16]; Vec3s d0_pos(other.axes.col(0) * (other.length[0] + other.radius)); Vec3s d1_pos(other.axes.col(1) * (other.length[1] + other.radius)); Vec3s d0_neg(other.axes.col(0) * (-other.radius)); Vec3s d1_neg(other.axes.col(1) * (-other.radius)); Vec3s d2_pos(other.axes.col(2) * other.radius); Vec3s d2_neg(other.axes.col(2) * (-other.radius)); v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos; v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg; v[2].noalias() = other.Tr + d0_pos + d1_neg + d2_pos; v[3].noalias() = other.Tr + d0_pos + d1_neg + d2_neg; v[4].noalias() = other.Tr + d0_neg + d1_pos + d2_pos; v[5].noalias() = other.Tr + d0_neg + d1_pos + d2_neg; v[6].noalias() = other.Tr + d0_neg + d1_neg + d2_pos; v[7].noalias() = other.Tr + d0_neg + d1_neg + d2_neg; d0_pos.noalias() = axes.col(0) * (length[0] + radius); d1_pos.noalias() = axes.col(1) * (length[1] + radius); d0_neg.noalias() = axes.col(0) * (-radius); d1_neg.noalias() = axes.col(1) * (-radius); d2_pos.noalias() = axes.col(2) * radius; d2_neg.noalias() = axes.col(2) * (-radius); v[8].noalias() = Tr + d0_pos + d1_pos + d2_pos; v[9].noalias() = Tr + d0_pos + d1_pos + d2_neg; v[10].noalias() = Tr + d0_pos + d1_neg + d2_pos; v[11].noalias() = Tr + d0_pos + d1_neg + d2_neg; v[12].noalias() = Tr + d0_neg + d1_pos + d2_pos; v[13].noalias() = Tr + d0_neg + d1_pos + d2_neg; v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos; v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg; Matrix3s M; // row first matrix Vec3s E[3]; // row first eigen-vectors Scalar s[3] = {0, 0, 0}; getCovariance(v, NULL, NULL, NULL, 16, M); eigen(M, s, E); int min, mid, max; if (s[0] > s[1]) { max = 0; min = 1; } else { min = 0; max = 1; } if (s[2] < s[min]) { mid = min; min = 2; } else if (s[2] > s[max]) { mid = max; max = 2; } else { mid = 2; } // column first matrix, as the axis in RSS bv.axes.col(0) << E[0][max], E[1][max], E[2][max]; bv.axes.col(1) << E[0][mid], E[1][mid], E[2][mid]; bv.axes.col(2) << E[1][max] * E[2][mid] - E[1][mid] * E[2][max], E[0][mid] * E[2][max] - E[0][max] * E[2][mid], E[0][max] * E[1][mid] - E[0][mid] * E[1][max]; // set rss origin, rectangle size and radius getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.length, bv.radius); return bv; } Scalar RSS::distance(const RSS& other, Vec3s* P, Vec3s* Q) const { // compute what transform [R,T] that takes us from cs1 to cs2. // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] // First compute the rotation part, then translation part Matrix3s R(axes.transpose() * other.axes); Vec3s T(axes.transpose() * (other.Tr - Tr)); Scalar dist = rectDistance(R, T, length, other.length, P, Q); dist -= (radius + other.radius); return (dist < (Scalar)0.0) ? (Scalar)0.0 : dist; } Scalar distance(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2, Vec3s* P, Vec3s* Q) { Matrix3s R(b1.axes.transpose() * R0 * b2.axes); Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr); Vec3s T(b1.axes.transpose() * Ttemp); Scalar dist = rectDistance(R, T, b1.length, b2.length, P, Q); dist -= (b1.radius + b2.radius); return (dist < (Scalar)0.0) ? (Scalar)0.0 : dist; } RSS translate(const RSS& bv, const Vec3s& t) { RSS res(bv); res.Tr += t; return res; } } // namespace coal ================================================ FILE: src/BV/kDOP.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/collision_data.h" #include "coal/BV/kDOP.h" #include #include namespace coal { /// @brief Find the smaller and larger one of two values inline void minmax(Scalar a, Scalar b, Scalar& minv, Scalar& maxv) { if (a > b) { minv = b; maxv = a; } else { minv = a; maxv = b; } } /// @brief Merge the interval [minv, maxv] and value p/ inline void minmax(Scalar p, Scalar& minv, Scalar& maxv) { if (p > maxv) maxv = p; if (p < minv) minv = p; } /// @brief Compute the distances to planes with normals from KDOP vectors except /// those of AABB face planes template void getDistances(const Vec3s& /*p*/, Scalar* /*d*/) {} /// @brief Specification of getDistances template <> inline void getDistances<5>(const Vec3s& p, Scalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; d[3] = p[0] - p[1]; d[4] = p[0] - p[2]; } template <> inline void getDistances<6>(const Vec3s& p, Scalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; d[3] = p[0] - p[1]; d[4] = p[0] - p[2]; d[5] = p[1] - p[2]; } template <> inline void getDistances<9>(const Vec3s& p, Scalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; d[3] = p[0] - p[1]; d[4] = p[0] - p[2]; d[5] = p[1] - p[2]; d[6] = p[0] + p[1] - p[2]; d[7] = p[0] + p[2] - p[1]; d[8] = p[1] + p[2] - p[0]; } template KDOP::KDOP() { Scalar real_max = (std::numeric_limits::max)(); dist_.template head().setConstant(real_max); dist_.template tail().setConstant(-real_max); } template KDOP::KDOP(const Vec3s& v) { for (short i = 0; i < 3; ++i) { dist_[i] = dist_[N / 2 + i] = v[i]; } Scalar d[(N - 6) / 2]; getDistances<(N - 6) / 2>(v, d); for (short i = 0; i < (N - 6) / 2; ++i) { dist_[3 + i] = dist_[3 + i + N / 2] = d[i]; } } template KDOP::KDOP(const Vec3s& a, const Vec3s& b) { for (short i = 0; i < 3; ++i) { minmax(a[i], b[i], dist_[i], dist_[i + N / 2]); } Scalar ad[(N - 6) / 2], bd[(N - 6) / 2]; getDistances<(N - 6) / 2>(a, ad); getDistances<(N - 6) / 2>(b, bd); for (short i = 0; i < (N - 6) / 2; ++i) { minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]); } } template bool KDOP::overlap(const KDOP& other) const { if ((dist_.template head() > other.dist_.template tail()).any()) return false; if ((dist_.template tail() < other.dist_.template head()).any()) return false; return true; } template bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const { const Scalar breakDistance(request.break_distance + request.security_margin); Scalar a = (dist_.template head() - other.dist_.template tail()) .minCoeff(); if (a > breakDistance) { sqrDistLowerBound = a * a; return false; } Scalar b = (other.dist_.template head() - dist_.template tail()) .minCoeff(); if (b > breakDistance) { sqrDistLowerBound = b * b; return false; } sqrDistLowerBound = std::min(a, b); return true; } template bool KDOP::inside(const Vec3s& p) const { if ((p.array() < dist_.template head<3>()).any()) return false; if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false; enum { P = ((N - 6) / 2) }; Eigen::Array d; getDistances

(p, d.data()); if ((d < dist_.template segment

(3)).any()) return false; if ((d > dist_.template segment

(3 + N / 2)).any()) return false; return true; } template KDOP& KDOP::operator+=(const Vec3s& p) { for (short i = 0; i < 3; ++i) { minmax(p[i], dist_[i], dist_[N / 2 + i]); } Scalar pd[(N - 6) / 2]; getDistances<(N - 6) / 2>(p, pd); for (short i = 0; i < (N - 6) / 2; ++i) { minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]); } return *this; } template KDOP& KDOP::operator+=(const KDOP& other) { for (short i = 0; i < N / 2; ++i) { dist_[i] = std::min(other.dist_[i], dist_[i]); dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]); } return *this; } template KDOP KDOP::operator+(const KDOP& other) const { KDOP res(*this); return res += other; } template Scalar KDOP::distance(const KDOP& /*other*/, Vec3s* /*P*/, Vec3s* /*Q*/) const { std::cerr << "KDOP distance not implemented!" << std::endl; return 0.0; } template KDOP translate(const KDOP& bv, const Vec3s& t) { KDOP res(bv); for (short i = 0; i < 3; ++i) { res.dist(i) += t[i]; res.dist(short(N / 2 + i)) += t[i]; } Scalar d[(N - 6) / 2]; getDistances<(N - 6) / 2>(t, d); for (short i = 0; i < (N - 6) / 2; ++i) { res.dist(short(3 + i)) += d[i]; res.dist(short(3 + i + N / 2)) += d[i]; } return res; } template class KDOP<16>; template class KDOP<18>; template class KDOP<24>; template KDOP<16> translate<16>(const KDOP<16>&, const Vec3s&); template KDOP<18> translate<18>(const KDOP<18>&, const Vec3s&); template KDOP<24> translate<24>(const KDOP<24>&, const Vec3s&); } // namespace coal ================================================ FILE: src/BV/kIOS.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/BV/kIOS.h" #include "coal/BVH/BVH_utility.h" #include "coal/math/transform.h" #include #include namespace coal { bool kIOS::overlap(const kIOS& other) const { for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { Scalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); Scalar sum_r = spheres[i].r + other.spheres[j].r; if (o_dist > sum_r * sum_r) return false; } } return obb.overlap(other.obb); } bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, Scalar& sqrDistLowerBound) const { for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { Scalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); Scalar sum_r = spheres[i].r + other.spheres[j].r; if (o_dist > sum_r * sum_r) { o_dist = sqrt(o_dist) - sum_r; sqrDistLowerBound = o_dist * o_dist; return false; } } } return obb.overlap(other.obb, request, sqrDistLowerBound); } bool kIOS::contain(const Vec3s& p) const { for (unsigned int i = 0; i < num_spheres; ++i) { Scalar r = spheres[i].r; if ((spheres[i].o - p).squaredNorm() > r * r) return false; } return true; } kIOS& kIOS::operator+=(const Vec3s& p) { for (unsigned int i = 0; i < num_spheres; ++i) { Scalar r = spheres[i].r; Scalar new_r_sqr = (p - spheres[i].o).squaredNorm(); if (new_r_sqr > r * r) { spheres[i].r = sqrt(new_r_sqr); } } obb += p; return *this; } kIOS kIOS::operator+(const kIOS& other) const { kIOS result; unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres); for (unsigned int i = 0; i < new_num_spheres; ++i) { result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]); } result.num_spheres = new_num_spheres; result.obb = obb + other.obb; return result; } Scalar kIOS::width() const { return obb.width(); } Scalar kIOS::height() const { return obb.height(); } Scalar kIOS::depth() const { return obb.depth(); } Scalar kIOS::volume() const { return obb.volume(); } Scalar kIOS::size() const { return volume(); } Scalar kIOS::distance(const kIOS& other, Vec3s* P, Vec3s* Q) const { Scalar d_max = 0; long id_a = -1, id_b = -1; for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { Scalar d = (spheres[i].o - other.spheres[j].o).norm() - (spheres[i].r + other.spheres[j].r); if (d_max < d) { d_max = d; if (P && Q) { id_a = (long)i; id_b = (long)j; } } } } if (P && Q) { if (id_a != -1 && id_b != -1) { const Vec3s v = spheres[id_a].o - spheres[id_b].o; Scalar len_v = v.norm(); *P = spheres[id_a].o - v * (spheres[id_a].r / len_v); *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v); } } return d_max; } bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o.noalias() = R0.transpose() * (b2_temp.spheres[i].o - T0); } b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0); b2_temp.obb.axes.applyOnTheLeft(R0.transpose()); return b1.overlap(b2_temp); } bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2, const CollisionRequest& request, Scalar& sqrDistLowerBound) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o.noalias() = R0.transpose() * (b2_temp.spheres[i].o - T0); } b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0); b2_temp.obb.axes.applyOnTheLeft(R0.transpose()); return b1.overlap(b2_temp, request, sqrDistLowerBound); } Scalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2, Vec3s* P, Vec3s* Q) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; } return b1.distance(b2_temp, P, Q); } kIOS translate(const kIOS& bv, const Vec3s& t) { kIOS res(bv); for (size_t i = 0; i < res.num_spheres; ++i) { res.spheres[i].o += t; } translate(res.obb, t); return res; } } // namespace coal ================================================ FILE: src/BVH/BVH_model.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2020, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/BV/BV_node.h" #include "coal/BVH/BVH_model.h" #include "coal/BV/BV.h" #include "coal/shape/convex.h" #include "coal/internal/BV_splitter.h" #include "coal/internal/BV_fitter.h" #include #include namespace coal { BVHModelBase::BVHModelBase() : num_tris(0), num_vertices(0), build_state(BVH_BUILD_STATE_EMPTY), num_tris_allocated(0), num_vertices_allocated(0), num_vertex_updated(0) {} BVHModelBase::BVHModelBase(const BVHModelBase& other) : CollisionGeometry(other), num_tris(other.num_tris), num_vertices(other.num_vertices), build_state(other.build_state), num_tris_allocated(other.num_tris), num_vertices_allocated(other.num_vertices) { if (other.vertices.get() && other.vertices->size() > 0) { vertices.reset(new std::vector(*(other.vertices))); } else vertices.reset(); if (other.tri_indices.get() && other.tri_indices->size() > 0) { tri_indices.reset(new std::vector(*(other.tri_indices))); } else tri_indices.reset(); if (other.prev_vertices.get() && other.prev_vertices->size() > 0) { prev_vertices.reset(new std::vector(*(other.prev_vertices))); } else prev_vertices.reset(); } bool BVHModelBase::isEqual(const CollisionGeometry& _other) const { const BVHModelBase* other_ptr = dynamic_cast(&_other); if (other_ptr == nullptr) return false; const BVHModelBase& other = *other_ptr; COAL_EQUAL_OPERATOR_CHECK( shared_ptrs_are_equal(tri_indices, other.tri_indices)); COAL_EQUAL_OPERATOR_CHECK(shared_ptrs_are_equal(vertices, other.vertices)); COAL_EQUAL_OPERATOR_CHECK( shared_ptrs_are_equal(prev_vertices, other.prev_vertices)); return true; } void BVHModelBase::buildConvexRepresentation(bool share_memory) { if (!(vertices.get())) { std::cerr << "BVH Error in `buildConvexRepresentation`! The BVHModel has " "no vertices." << std::endl; return; } if (!(tri_indices.get())) { std::cerr << "BVH Error in `buildConvexRepresentation`! The BVHModel has " "no triangles." << std::endl; return; } if (!convex) { std::shared_ptr> points = vertices; std::shared_ptr> polygons = tri_indices; if (!share_memory) { points.reset(new std::vector(*(vertices))); polygons.reset(new std::vector(*(tri_indices))); } convex.reset( new ConvexTpl(points, num_vertices, polygons, num_tris)); } } bool BVHModelBase::buildConvexHull(bool keepTriangle, const char* qhullCommand) { convex.reset(BVHModelBase::ConvexType::convexHull( vertices, num_vertices, keepTriangle, qhullCommand)); return num_vertices == convex->num_points; } template BVHModel::BVHModel(const BVHModel& other) : BVHModelBase(other), bv_splitter(other.bv_splitter), bv_fitter(other.bv_fitter) { if (other.primitive_indices.get()) { primitive_indices.reset( new std::vector(*(other.primitive_indices))); } else primitive_indices.reset(); num_bvs = num_bvs_allocated = other.num_bvs; if (other.bvs.get()) { bvs.reset(new bv_node_vector_t(*(other.bvs))); } else bvs.reset(); } int BVHModelBase::beginModel(unsigned int num_tris_, unsigned int num_vertices_) { if (build_state != BVH_BUILD_STATE_EMPTY) { vertices.reset(); tri_indices.reset(); tri_indices.reset(); prev_vertices.reset(); num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = 0; deleteBVs(); } if (num_tris_ <= 0) num_tris_ = 8; if (num_vertices_ <= 0) num_vertices_ = 8; num_vertices_allocated = num_vertices_; num_tris_allocated = num_tris_; if (num_tris_allocated > 0) { tri_indices.reset(new std::vector(num_tris_allocated)); if (!(tri_indices.get())) { std::cerr << "BVH Error! Out of memory for tri_indices array on " "BeginModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } } else tri_indices.reset(); if (num_vertices_allocated > 0) { vertices.reset(new std::vector(num_vertices_allocated)); if (!(vertices.get())) { std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } } else { vertices.reset(); prev_vertices.reset(); }; if (build_state != BVH_BUILD_STATE_EMPTY) { std::cerr << "BVH Warning! Calling beginModel() on a BVHModel that is not empty. " "This model was cleared and previous triangles/vertices were lost." << std::endl; build_state = BVH_BUILD_STATE_EMPTY; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } build_state = BVH_BUILD_STATE_BEGUN; return BVH_OK; } int BVHModelBase::addVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() " "was ignored. Must do a beginModel() to clear the model for " "addition of new vertices." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if (num_vertices >= num_vertices_allocated) { std::shared_ptr> temp( new std::vector(num_vertices_allocated * 2)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_vertices; ++i) { (*temp)[i] = (*vertices)[i]; } vertices = temp; num_vertices_allocated *= 2; } (*vertices)[num_vertices] = p; num_vertices += 1; return BVH_OK; } int BVHModelBase::addTriangles(const Matrixx3i& triangles) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Call addSubModel() in a wrong order. " "addSubModel() was ignored. Must do a beginModel() to clear " "the model for addition of new vertices." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } const unsigned int num_tris_to_add = (unsigned int)triangles.rows(); if (num_tris + num_tris_to_add > num_tris_allocated) { std::shared_ptr> temp( new std::vector(num_tris_allocated * 2 + num_tris_to_add)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for tri_indices array on " "addSubModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_tris; ++i) { (*temp)[i] = (*tri_indices)[i]; } tri_indices = temp; num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add; } std::vector& tri_indices_ = *tri_indices; for (Eigen::DenseIndex i = 0; i < triangles.rows(); ++i) { const Matrixx3i::ConstRowXpr triangle = triangles.row(i); tri_indices_[num_tris++].set( static_cast(triangle[0]), static_cast(triangle[1]), static_cast(triangle[2])); } return BVH_OK; } int BVHModelBase::addVertices(const MatrixX3s& points) { if (build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call addVertex() in a wrong order. " "addVertices() was ignored. Must do a beginModel() to clear " "the model for addition of new vertices." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if (num_vertices + points.rows() > num_vertices_allocated) { num_vertices_allocated = num_vertices + (unsigned int)points.rows(); std::shared_ptr> temp( new std::vector(num_vertices_allocated)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_vertices; ++i) { (*temp)[i] = (*vertices)[i]; } vertices = temp; } std::vector& vertices_ = *vertices; for (Eigen::DenseIndex id = 0; id < points.rows(); ++id) vertices_[num_vertices++] = points.row(id).transpose(); return BVH_OK; } int BVHModelBase::addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Call addTriangle() in a wrong order. " "addTriangle() was ignored. Must do a beginModel() to clear " "the model for addition of new triangles." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if (num_vertices + 2 >= num_vertices_allocated) { std::shared_ptr> temp( new std::vector(num_vertices_allocated * 2 + 2)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " "addTriangle() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_vertices; ++i) { (*temp)[i] = (*vertices)[i]; } vertices = temp; num_vertices_allocated = num_vertices_allocated * 2 + 2; } const unsigned int offset = num_vertices; (*vertices)[num_vertices] = p1; num_vertices++; (*vertices)[num_vertices] = p2; num_vertices++; (*vertices)[num_vertices] = p3; num_vertices++; if (num_tris >= num_tris_allocated) { std::shared_ptr> temp( new std::vector(num_tris_allocated * 2)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for tri_indices array on " "addTriangle() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_tris; ++i) { (*temp)[i] = (*tri_indices)[i]; } tri_indices = temp; num_tris_allocated *= 2; } (*tri_indices)[num_tris].set(Triangle32::IndexType(offset), Triangle32::IndexType((offset + 1)), Triangle32::IndexType((offset + 2))); num_tris++; return BVH_OK; } int BVHModelBase::addSubModel(const std::vector& ps) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. " "addSubModel() was ignored. Must do a beginModel() to clear " "the model for addition of new vertices." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } const unsigned int num_vertices_to_add = (unsigned int)ps.size(); if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { std::shared_ptr> temp(new std::vector( num_vertices_allocated * 2 + num_vertices_to_add - 1)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " "addSubModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_vertices; ++i) { (*temp)[i] = (*vertices)[i]; } vertices = temp; num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; } std::vector& vertices_ = *vertices; for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { vertices_[num_vertices] = ps[i]; num_vertices++; } return BVH_OK; } int BVHModelBase::addSubModel(const std::vector& ps, const std::vector& ts) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. " "addSubModel() was ignored. Must do a beginModel() to clear " "the model for addition of new vertices." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } const unsigned int num_vertices_to_add = (unsigned int)ps.size(); if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { std::shared_ptr> temp(new std::vector( num_vertices_allocated * 2 + num_vertices_to_add - 1)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " "addSubModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_vertices; ++i) { (*temp)[i] = (*vertices)[i]; } vertices = temp; num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; } const unsigned int offset = num_vertices; std::vector& vertices_ = *vertices; for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { vertices_[num_vertices] = ps[i]; num_vertices++; } const unsigned int num_tris_to_add = (unsigned int)ts.size(); if (num_tris + num_tris_to_add - 1 >= num_tris_allocated) { std::shared_ptr> temp(new std::vector( num_tris_allocated * 2 + num_tris_to_add - 1)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for tri_indices array on " "addSubModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_tris; ++i) { (*temp)[i] = (*tri_indices)[i]; } tri_indices = temp; num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1; } std::vector& tri_indices_ = *tri_indices; for (size_t i = 0; i < (size_t)num_tris_to_add; ++i) { const Triangle32& t = ts[i]; tri_indices_[num_tris].set(t[0] + Triangle32::IndexType(offset), t[1] + Triangle32::IndexType(offset), t[2] + Triangle32::IndexType(offset)); num_tris++; } return BVH_OK; } int BVHModelBase::endModel() { if (build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was " "ignored." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if (num_tris == 0 && num_vertices == 0) { std::cerr << "BVH Error! endModel() called on model with no triangles and " "vertices." << std::endl; return BVH_ERR_BUILD_EMPTY_MODEL; } if (num_tris_allocated > num_tris) { if (num_tris > 0) { std::shared_ptr> new_tris( new std::vector(num_tris)); if (!(new_tris.get())) { std::cerr << "BVH Error! Out of memory for tri_indices array in " "endModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_tris; ++i) { (*new_tris)[i] = (*tri_indices)[i]; } tri_indices = new_tris; num_tris_allocated = num_tris; } else { tri_indices.reset(); num_tris = num_tris_allocated = 0; } } if (num_vertices_allocated > num_vertices) { if (num_vertices > 0) { std::shared_ptr> new_vertices( new std::vector(num_vertices)); if (!(new_vertices.get())) { std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } for (size_t i = 0; i < num_vertices; ++i) { (*new_vertices)[i] = (*vertices)[i]; } vertices = new_vertices; num_vertices_allocated = num_vertices; } else { vertices.reset(); num_vertices = num_vertices_allocated = 0; } } // construct BVH tree if (!allocateBVs()) return BVH_ERR_MODEL_OUT_OF_MEMORY; buildTree(); // finish constructing build_state = BVH_BUILD_STATE_PROCESSED; return BVH_OK; } int BVHModelBase::beginReplaceModel() { if (build_state != BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has " "no previous frame." << std::endl; return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; } if (prev_vertices.get()) prev_vertices.reset(); num_vertex_updated = 0; build_state = BVH_BUILD_STATE_REPLACE_BEGUN; return BVH_OK; } int BVHModelBase::replaceVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. " "replaceVertex() was ignored. Must do a beginReplaceModel() " "for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } (*vertices)[num_vertex_updated] = p; num_vertex_updated++; return BVH_OK; } int BVHModelBase::replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. " "replaceTriangle() was ignored. Must do a beginReplaceModel() " "for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } (*vertices)[num_vertex_updated] = p1; num_vertex_updated++; (*vertices)[num_vertex_updated] = p2; num_vertex_updated++; (*vertices)[num_vertex_updated] = p3; num_vertex_updated++; return BVH_OK; } int BVHModelBase::replaceSubModel(const std::vector& ps) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. " "replaceSubModel() was ignored. Must do a beginReplaceModel() " "for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < ps.size(); ++i) { vertices_[num_vertex_updated] = ps[i]; num_vertex_updated++; } return BVH_OK; } int BVHModelBase::endReplaceModel(bool refit, bool bottomup) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. " "endReplaceModel() was ignored. " << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if (num_vertex_updated != num_vertices) { std::cerr << "BVH Error! The replaced model should have the same number of " "vertices as the old model." << std::endl; return BVH_ERR_INCORRECT_DATA; } if (refit) // refit, do not change BVH structure { refitTree(bottomup); } else // reconstruct bvh tree based on current frame data { buildTree(); } build_state = BVH_BUILD_STATE_PROCESSED; return BVH_OK; } int BVHModelBase::beginUpdateModel() { if (build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED) { std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no " "previous frame." << std::endl; return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; } if (prev_vertices.get()) { std::shared_ptr> temp = prev_vertices; prev_vertices = vertices; vertices = temp; } else { prev_vertices = vertices; vertices.reset(new std::vector(num_vertices)); } num_vertex_updated = 0; build_state = BVH_BUILD_STATE_UPDATE_BEGUN; return BVH_OK; } int BVHModelBase::updateVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() " "was ignored. Must do a beginUpdateModel() for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } (*vertices)[num_vertex_updated] = p; num_vertex_updated++; return BVH_OK; } int BVHModelBase::updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. " "updateTriangle() was ignored. Must do a beginUpdateModel() " "for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } (*vertices)[num_vertex_updated] = p1; num_vertex_updated++; (*vertices)[num_vertex_updated] = p2; num_vertex_updated++; (*vertices)[num_vertex_updated] = p3; num_vertex_updated++; return BVH_OK; } int BVHModelBase::updateSubModel(const std::vector& ps) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. " "updateSubModel() was ignored. Must do a beginUpdateModel() " "for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < ps.size(); ++i) { vertices_[num_vertex_updated] = ps[i]; num_vertex_updated++; } return BVH_OK; } int BVHModelBase::endUpdateModel(bool refit, bool bottomup) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. " "endUpdateModel() was ignored. " << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if (num_vertex_updated != num_vertices) { std::cerr << "BVH Error! The updated model should have the same number of " "vertices as the old model." << std::endl; return BVH_ERR_INCORRECT_DATA; } if (refit) // refit, do not change BVH structure { refitTree(bottomup); } else // reconstruct bvh tree based on current frame data { buildTree(); // then refit refitTree(bottomup); } build_state = BVH_BUILD_STATE_UPDATED; return BVH_OK; } void BVHModelBase::computeLocalAABB() { AABB aabb_; const std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < num_vertices; ++i) { aabb_ += vertices_[i]; } aabb_center = aabb_.center(); aabb_radius = 0; for (unsigned int i = 0; i < num_vertices; ++i) { Scalar r = (aabb_center - vertices_[i]).squaredNorm(); if (r > aabb_radius) aabb_radius = r; } aabb_radius = sqrt(aabb_radius); aabb_local = aabb_; } /// @brief Constructing an empty BVH template BVHModel::BVHModel() : BVHModelBase(), bv_splitter(new BVSplitter(SPLIT_METHOD_MEAN)), bv_fitter(new BVFitter()), num_bvs_allocated(0), num_bvs(0) {} template void BVHModel::deleteBVs() { bvs.reset(); primitive_indices.reset(); num_bvs_allocated = num_bvs = 0; } template bool BVHModel::allocateBVs() { // construct BVH tree unsigned int num_bvs_to_be_allocated = 0; if (num_tris == 0) num_bvs_to_be_allocated = 2 * num_vertices - 1; else num_bvs_to_be_allocated = 2 * num_tris - 1; bvs.reset(new bv_node_vector_t(num_bvs_to_be_allocated)); primitive_indices.reset( new std::vector(num_bvs_to_be_allocated)); if (!(bvs.get()) || !(primitive_indices.get())) { std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl; return false; } num_bvs_allocated = num_bvs_to_be_allocated; num_bvs = 0; return true; } template int BVHModel::memUsage(const bool msg) const { unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs; unsigned int mem_tri_list = (unsigned int)sizeof(Triangle32) * num_tris; unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3s) * num_vertices; unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + (unsigned int)sizeof(BVHModel); if (msg) { std::cerr << "Total for model " << total_mem << " bytes." << std::endl; std::cerr << "BVs: " << num_bvs << " allocated." << std::endl; std::cerr << "Tris: " << num_tris << " allocated." << std::endl; std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl; } return static_cast(total_mem); } template int BVHModel::buildTree() { // set BVFitter Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL; Triangle32* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL; bv_fitter->set(vertices_, tri_indices_, getModelType()); // set SplitRule bv_splitter->set(vertices_, tri_indices_, getModelType()); num_bvs = 1; unsigned int num_primitives = 0; switch (getModelType()) { case BVH_MODEL_TRIANGLES: num_primitives = (unsigned int)num_tris; break; case BVH_MODEL_POINTCLOUD: num_primitives = (unsigned int)num_vertices; break; default: std::cerr << "BVH Error: Model type not supported!" << std::endl; return BVH_ERR_UNSUPPORTED_FUNCTION; } std::vector& primitive_indices_ = *primitive_indices; for (unsigned int i = 0; i < num_primitives; ++i) primitive_indices_[i] = i; recursiveBuildTree(0, 0, num_primitives); bv_fitter->clear(); bv_splitter->clear(); return BVH_OK; } template int BVHModel::recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives) { BVHModelType type = getModelType(); BVNode* bvnode = bvs->data() + bv_id; unsigned int* cur_primitive_indices = primitive_indices->data() + first_primitive; // constructing BV BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives); bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives); bvnode->bv = bv; bvnode->first_primitive = first_primitive; bvnode->num_primitives = num_primitives; if (num_primitives == 1) { bvnode->first_child = -((int)(*cur_primitive_indices) + 1); } else { bvnode->first_child = (int)num_bvs; num_bvs += 2; unsigned int c1 = 0; const std::vector& vertices_ = *vertices; const std::vector& tri_indices_ = *tri_indices; for (unsigned int i = 0; i < num_primitives; ++i) { Vec3s p; if (type == BVH_MODEL_POINTCLOUD) p = vertices_[cur_primitive_indices[i]]; else if (type == BVH_MODEL_TRIANGLES) { const Triangle32& t = tri_indices_[cur_primitive_indices[i]]; const Vec3s& p1 = vertices_[t[0]]; const Vec3s& p2 = vertices_[t[1]]; const Vec3s& p3 = vertices_[t[2]]; p = (p1 + p2 + p3) / 3.; } else { std::cerr << "BVH Error: Model type not supported!" << std::endl; return BVH_ERR_UNSUPPORTED_FUNCTION; } // loop invariant: up to (but not including) index c1 in group 1, // then up to (but not including) index i in group 2 // // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x] // c1 i // if (bv_splitter->apply(p)) // in the right side { // do nothing } else { unsigned int temp = cur_primitive_indices[i]; cur_primitive_indices[i] = cur_primitive_indices[c1]; cur_primitive_indices[c1] = temp; c1++; } } if ((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2; const unsigned int num_first_half = c1; recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half); recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half); } return BVH_OK; } template int BVHModel::refitTree(bool bottomup) { if (bottomup) return refitTree_bottomup(); else return refitTree_topdown(); } template int BVHModel::refitTree_bottomup() { // TODO the recomputation of the BV is done manually, without using // bv_fitter. The manual BV recomputation seems bugged. Using bv_fitter // seems to correct the bug. // bv_fitter->set(vertices, tri_indices, getModelType()); int res = recursiveRefitTree_bottomup(0); // bv_fitter->clear(); return res; } template int BVHModel::recursiveRefitTree_bottomup(int bv_id) { BVNode* bvnode = bvs->data() + bv_id; if (bvnode->isLeaf()) { BVHModelType type = getModelType(); int primitive_id = -(bvnode->first_child + 1); if (type == BVH_MODEL_POINTCLOUD) { BV bv; if (prev_vertices.get()) { Vec3s v[2]; v[0] = (*prev_vertices)[static_cast(primitive_id)]; v[1] = (*vertices)[static_cast(primitive_id)]; fit(v, 2, bv); } else fit(vertices->data() + primitive_id, 1, bv); bvnode->bv = bv; } else if (type == BVH_MODEL_TRIANGLES) { BV bv; const Triangle32& triangle = (*tri_indices)[static_cast(primitive_id)]; if (prev_vertices.get()) { Vec3s v[6]; for (Triangle32::IndexType i = 0; i < 3; ++i) { v[i] = (*prev_vertices)[triangle[i]]; v[i + 3] = (*vertices)[triangle[i]]; } fit(v, 6, bv); } else { // TODO use bv_fitter to build BV. See comment in refitTree_bottomup // unsigned int* cur_primitive_indices = primitive_indices + // bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices, // bvnode->num_primitives); Vec3s v[3]; for (int i = 0; i < 3; ++i) { v[i] = (*vertices)[triangle[(Triangle32::IndexType)i]]; } fit(v, 3, bv); } bvnode->bv = bv; } else { std::cerr << "BVH Error: Model type not supported!" << std::endl; return BVH_ERR_UNSUPPORTED_FUNCTION; } } else { recursiveRefitTree_bottomup(bvnode->leftChild()); recursiveRefitTree_bottomup(bvnode->rightChild()); bvnode->bv = (*bvs)[static_cast(bvnode->leftChild())].bv + (*bvs)[static_cast(bvnode->rightChild())].bv; // TODO use bv_fitter to build BV. See comment in refitTree_bottomup // unsigned int* cur_primitive_indices = primitive_indices + // bvnode->first_primitive; bvnode->bv = // bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives); } return BVH_OK; } template int BVHModel::refitTree_topdown() { Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL; Vec3s* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL; Triangle32* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL; bv_fitter->set(vertices_, prev_vertices_, tri_indices_, getModelType()); BVNode* bvs_ = bvs->data(); unsigned int* primitive_indices_ = primitive_indices->data(); for (unsigned int i = 0; i < num_bvs; ++i) { BV bv = bv_fitter->fit(primitive_indices_ + bvs_[i].first_primitive, bvs_[i].num_primitives); bvs_[i].bv = bv; } bv_fitter->clear(); return BVH_OK; } template <> void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; OBB& obb = bvs_[static_cast(bv_id)].bv; if (!bvs_[static_cast(bv_id)].isLeaf()) { makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child, obb.axes, obb.To); makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child + 1, obb.axes, obb.To); } // make self parent relative // obb.axes = parent_axes.transpose() * obb.axes; obb.axes.applyOnTheLeft(parent_axes.transpose()); Vec3s t(obb.To - parent_c); obb.To.noalias() = parent_axes.transpose() * t; } template <> void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; RSS& rss = bvs_[static_cast(bv_id)].bv; if (!bvs_[static_cast(bv_id)].isLeaf()) { makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child, rss.axes, rss.Tr); makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child + 1, rss.axes, rss.Tr); } // make self parent relative // rss.axes = parent_axes.transpose() * rss.axes; rss.axes.applyOnTheLeft(parent_axes.transpose()); Vec3s t(rss.Tr - parent_c); rss.Tr.noalias() = parent_axes.transpose() * t; } template <> void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; OBB& obb = bvs_[static_cast(bv_id)].bv.obb; RSS& rss = bvs_[static_cast(bv_id)].bv.rss; if (!bvs_[static_cast(bv_id)].isLeaf()) { makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child, obb.axes, obb.To); makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child + 1, obb.axes, obb.To); } // make self parent relative rss.axes.noalias() = parent_axes.transpose() * obb.axes; obb.axes = rss.axes; Vec3s t(obb.To - parent_c); obb.To.noalias() = parent_axes.transpose() * t; rss.Tr = obb.To; } template <> NODE_TYPE BVHModel::getNodeType() const { return BV_AABB; } template <> NODE_TYPE BVHModel::getNodeType() const { return BV_OBB; } template <> NODE_TYPE BVHModel::getNodeType() const { return BV_RSS; } template <> NODE_TYPE BVHModel::getNodeType() const { return BV_kIOS; } template <> NODE_TYPE BVHModel::getNodeType() const { return BV_OBBRSS; } template <> NODE_TYPE BVHModel>::getNodeType() const { return BV_KDOP16; } template <> NODE_TYPE BVHModel>::getNodeType() const { return BV_KDOP18; } template <> NODE_TYPE BVHModel>::getNodeType() const { return BV_KDOP24; } template class BVHModel>; template class BVHModel>; template class BVHModel>; template class BVHModel; template class BVHModel; template class BVHModel; template class BVHModel; template class BVHModel; } // namespace coal ================================================ FILE: src/BVH/BVH_utility.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/BVH/BVH_utility.h" #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes_utility.h" #include "coal/internal/shape_shape_func.h" namespace coal { namespace details { template BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& _aabb) { assert(model.getModelType() == BVH_MODEL_TRIANGLES); const Matrix3s& q = pose.getRotation(); AABB aabb = translate(_aabb, -pose.getTranslation()); Transform3s box_pose; Box box; constructBox(_aabb, box, box_pose); box_pose = pose.inverseTimes(box_pose); GJKSolver gjk; // Early-stop GJK as soon as a separating plane is found gjk.gjk.setDistanceEarlyBreak(Scalar(1e-3)); if (gjk.gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); } // Check what triangles should be kept. // TODO use the BV hierarchy std::vector keep_vertex(model.num_vertices, false); std::vector keep_tri(model.num_tris, false); unsigned int ntri = 0; const std::vector& model_vertices_ = *(model.vertices); const std::vector& model_tri_indices_ = *(model.tri_indices); for (unsigned int i = 0; i < model.num_tris; ++i) { const Triangle32& t = model_tri_indices_[i]; bool keep_this_tri = keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]]; if (!keep_this_tri) { for (unsigned int j = 0; j < 3; ++j) { if (aabb.contain(q * model_vertices_[t[j]])) { keep_this_tri = true; break; } } TriangleP tri(model_vertices_[t[0]], model_vertices_[t[1]], model_vertices_[t[2]]); if (gjk.gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { tri.computeLocalAABB(); } const bool enable_nearest_points = false; const bool compute_penetration = false; // we only need to know if there // is a collision or not const DistanceRequest distanceRequest(enable_nearest_points, compute_penetration); DistanceResult distanceResult; const Scalar distance = ShapeShapeDistance( &box, box_pose, &tri, Transform3s(), &gjk, distanceRequest, distanceResult); bool is_collision = (distance <= gjk.getDistancePrecision(compute_penetration)); if (!keep_this_tri && is_collision) { keep_this_tri = true; } } if (keep_this_tri) { keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true; keep_tri[i] = true; ntri++; } } if (ntri == 0) return NULL; BVHModel* new_model(new BVHModel()); new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices)); std::vector idxConversion(model.num_vertices); assert(new_model->num_vertices == 0); std::vector& new_model_vertices_ = *(new_model->vertices); for (unsigned int i = 0; i < keep_vertex.size(); ++i) { if (keep_vertex[i]) { idxConversion[i] = new_model->num_vertices; new_model_vertices_[new_model->num_vertices] = model_vertices_[i]; new_model->num_vertices++; } } assert(new_model->num_tris == 0); std::vector& new_model_tri_indices_ = *(new_model->tri_indices); for (unsigned int i = 0; i < keep_tri.size(); ++i) { if (keep_tri[i]) { new_model_tri_indices_[new_model->num_tris].set( idxConversion[model_tri_indices_[i][0]], idxConversion[model_tri_indices_[i][1]], idxConversion[model_tri_indices_[i][2]]); new_model->num_tris++; } } if (new_model->endModel() != BVH_OK) { delete new_model; return NULL; } return new_model; } } // namespace details template <> BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, Matrix3s& M) { Vec3s S1(Vec3s::Zero()); Vec3s S2[3] = {Vec3s::Zero(), Vec3s::Zero(), Vec3s::Zero()}; if (ts) { for (unsigned int i = 0; i < n; ++i) { const Triangle32& t = (indices) ? ts[indices[i]] : ts[i]; const Vec3s& p1 = ps[t[0]]; const Vec3s& p2 = ps[t[1]]; const Vec3s& p3 = ps[t[2]]; S1[0] += (p1[0] + p2[0] + p3[0]); S1[1] += (p1[1] + p2[1] + p3[1]); S1[2] += (p1[2] + p2[2] + p3[2]); S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); if (ps2) { const Vec3s& p1 = ps2[t[0]]; const Vec3s& p2 = ps2[t[1]]; const Vec3s& p3 = ps2[t[2]]; S1[0] += (p1[0] + p2[0] + p3[0]); S1[1] += (p1[1] + p2[1] + p3[1]); S1[2] += (p1[2] + p2[2] + p3[2]); S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); } } } else { for (unsigned int i = 0; i < n; ++i) { const Vec3s& p = (indices) ? ps[indices[i]] : ps[i]; S1 += p; S2[0][0] += (p[0] * p[0]); S2[1][1] += (p[1] * p[1]); S2[2][2] += (p[2] * p[2]); S2[0][1] += (p[0] * p[1]); S2[0][2] += (p[0] * p[2]); S2[1][2] += (p[1] * p[2]); if (ps2) // another frame { const Vec3s& p = (indices) ? ps2[indices[i]] : ps2[i]; S1 += p; S2[0][0] += (p[0] * p[0]); S2[1][1] += (p[1] * p[1]); S2[2][2] += (p[2] * p[2]); S2[0][1] += (p[0] * p[1]); S2[0][2] += (p[0] * p[2]); S2[1][2] += (p[1] * p[2]); } } } unsigned int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; M(0, 0) = S2[0][0] - S1[0] * S1[0] / Scalar(n_points); M(1, 1) = S2[1][1] - S1[1] * S1[1] / Scalar(n_points); M(2, 2) = S2[2][2] - S1[2] * S1[2] / Scalar(n_points); M(0, 1) = S2[0][1] - S1[0] * S1[1] / Scalar(n_points); M(1, 2) = S2[1][2] - S1[1] * S1[2] / Scalar(n_points); M(0, 2) = S2[0][2] - S1[0] * S1[2] / Scalar(n_points); M(1, 0) = M(0, 1); M(2, 0) = M(0, 2); M(2, 1) = M(1, 2); } /** @brief Compute the RSS bounding volume parameters: radius, rectangle size * and the origin. The bounding volume axes are known. */ void getRadiusAndOriginAndRectangleSize(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, const Matrix3s& axes, Vec3s& origin, Scalar l[2], Scalar& r) { bool indirect_index = true; if (!indices) indirect_index = false; unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; Scalar(*P)[3] = new Scalar[size_P][3]; int P_id = 0; if (ts) { for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Triangle32& t = ts[index]; for (Triangle32::IndexType j = 0; j < 3; ++j) { Triangle32::IndexType point_id = t[j]; const Vec3s& p = ps[point_id]; Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); P_id++; } if (ps2) { for (Triangle32::IndexType j = 0; j < 3; ++j) { Triangle32::IndexType point_id = t[j]; const Vec3s& p = ps2[point_id]; // FIXME Is this right ????? Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(0).dot(v); P[P_id][2] = axes.col(1).dot(v); P_id++; } } } } else { for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Vec3s& p = ps[index]; Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); P_id++; if (ps2) { const Vec3s& v = ps2[index]; P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); P_id++; } } } Scalar minx, maxx, miny, maxy, minz, maxz; Scalar cz, radsqr; minz = maxz = P[0][2]; for (unsigned int i = 1; i < size_P; ++i) { Scalar z_value = P[i][2]; if (z_value < minz) minz = z_value; else if (z_value > maxz) maxz = z_value; } r = (Scalar)0.5 * (maxz - minz); radsqr = r * r; cz = (Scalar)0.5 * (maxz + minz); // compute an initial norm of rectangle along x direction // find minx and maxx as starting points unsigned int minindex = 0, maxindex = 0; Scalar mintmp, maxtmp; mintmp = maxtmp = P[0][0]; for (unsigned int i = 1; i < size_P; ++i) { Scalar x_value = P[i][0]; if (x_value < mintmp) { minindex = i; mintmp = x_value; } else if (x_value > maxtmp) { maxindex = i; maxtmp = x_value; } } Scalar x, dz; dz = P[minindex][2] - cz; minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); dz = P[maxindex][2] - cz; maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); // grow minx/maxx for (unsigned int i = 0; i < size_P; ++i) { if (P[i][0] < minx) { dz = P[i][2] - cz; x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); if (x < minx) minx = x; } else if (P[i][0] > maxx) { dz = P[i][2] - cz; x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); if (x > maxx) maxx = x; } } // compute an initial norm of rectangle along y direction // find miny and maxy as starting points minindex = maxindex = 0; mintmp = maxtmp = P[0][1]; for (unsigned int i = 1; i < size_P; ++i) { Scalar y_value = P[i][1]; if (y_value < mintmp) { minindex = i; mintmp = y_value; } else if (y_value > maxtmp) { maxindex = i; maxtmp = y_value; } } Scalar y; dz = P[minindex][2] - cz; miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); dz = P[maxindex][2] - cz; maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); // grow miny/maxy for (unsigned int i = 0; i < size_P; ++i) { if (P[i][1] < miny) { dz = P[i][2] - cz; y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); if (y < miny) miny = y; } else if (P[i][1] > maxy) { dz = P[i][2] - cz; y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); if (y > maxy) maxy = y; } } // corners may have some points which are not covered - grow lengths if // necessary quite conservative (can be improved) Scalar dx, dy, u, t; Scalar a = std::sqrt((Scalar)0.5); for (unsigned int i = 0; i < size_P; ++i) { if (P[i][0] > maxx) { if (P[i][1] > maxy) { dx = P[i][0] - maxx; dy = P[i][1] - maxy; u = dx * a + dy * a; t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { maxx += u * a; maxy += u * a; } } else if (P[i][1] < miny) { dx = P[i][0] - maxx; dy = P[i][1] - miny; u = dx * a - dy * a; t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { maxx += u * a; miny -= u * a; } } } else if (P[i][0] < minx) { if (P[i][1] > maxy) { dx = P[i][0] - minx; dy = P[i][1] - maxy; u = dy * a - dx * a; t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { minx -= u * a; maxy += u * a; } } else if (P[i][1] < miny) { dx = P[i][0] - minx; dy = P[i][1] - miny; u = -dx * a - dy * a; t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { minx -= u * a; miny -= u * a; } } } } origin.noalias() = axes * Vec3s(minx, miny, cz); l[0] = std::max(maxx - minx, 0); l[1] = std::max(maxy - miny, 0); delete[] P; } /** @brief Compute the bounding volume extent and center for a set or subset of * points. The bounding volume axes are known. */ static inline void getExtentAndCenter_pointcloud(Vec3s* ps, Vec3s* ps2, unsigned int* indices, unsigned int n, Matrix3s& axes, Vec3s& center, Vec3s& extent) { bool indirect_index = true; if (!indices) indirect_index = false; Scalar real_max = (std::numeric_limits::max)(); Vec3s min_coord(real_max, real_max, real_max); Vec3s max_coord(-real_max, -real_max, -real_max); for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Vec3s& p = ps[index]; Vec3s proj(axes.transpose() * p); for (int j = 0; j < 3; ++j) { if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; if (proj[j] < min_coord[j]) min_coord[j] = proj[j]; } if (ps2) { const Vec3s& v = ps2[index]; proj.noalias() = axes.transpose() * v; for (int j = 0; j < 3; ++j) { if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; if (proj[j] < min_coord[j]) min_coord[j] = proj[j]; } } } center.noalias() = axes * (max_coord + min_coord) / 2; extent.noalias() = (max_coord - min_coord) / 2; } /** @brief Compute the bounding volume extent and center for a set or subset of * points. The bounding volume axes are known. */ static inline void getExtentAndCenter_mesh(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, Matrix3s& axes, Vec3s& center, Vec3s& extent) { bool indirect_index = true; if (!indices) indirect_index = false; Scalar real_max = (std::numeric_limits::max)(); Vec3s min_coord(real_max, real_max, real_max); Vec3s max_coord(-real_max, -real_max, -real_max); for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Triangle32& t = ts[index]; for (Triangle32::IndexType j = 0; j < 3; ++j) { Triangle32::IndexType point_id = t[j]; const Vec3s& p = ps[point_id]; Vec3s proj(axes.transpose() * p); for (int k = 0; k < 3; ++k) { if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; if (proj[k] < min_coord[k]) min_coord[k] = proj[k]; } } if (ps2) { for (Triangle32::IndexType j = 0; j < 3; ++j) { Triangle32::IndexType point_id = t[j]; const Vec3s& p = ps2[point_id]; Vec3s proj(axes.transpose() * p); for (int k = 0; k < 3; ++k) { if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; if (proj[k] < min_coord[k]) min_coord[k] = proj[k]; } } } } Vec3s o((max_coord + min_coord) / 2); center.noalias() = axes * o; extent.noalias() = (max_coord - min_coord) / 2; } void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, Matrix3s& axes, Vec3s& center, Vec3s& extent) { if (ts) getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent); else getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent); } void circumCircleComputation(const Vec3s& a, const Vec3s& b, const Vec3s& c, Vec3s& center, Scalar& radius) { Vec3s e1 = a - c; Vec3s e2 = b - c; Scalar e1_len2 = e1.squaredNorm(); Scalar e2_len2 = e2.squaredNorm(); Vec3s e3 = e1.cross(e2); Scalar e3_len2 = e3.squaredNorm(); radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2; radius = std::sqrt(radius) * Scalar(0.5); center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; } static inline Scalar maximumDistance_mesh(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, const Vec3s& query) { bool indirect_index = true; if (!indices) indirect_index = false; Scalar maxD = 0; for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Triangle32& t = ts[index]; for (Triangle32::IndexType j = 0; j < 3; ++j) { Triangle32::IndexType point_id = t[j]; const Vec3s& p = ps[point_id]; Scalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; } if (ps2) { for (Triangle32::IndexType j = 0; j < 3; ++j) { Triangle32::IndexType point_id = t[j]; const Vec3s& p = ps2[point_id]; Scalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; } } } return std::sqrt(maxD); } static inline Scalar maximumDistance_pointcloud(Vec3s* ps, Vec3s* ps2, unsigned int* indices, unsigned int n, const Vec3s& query) { bool indirect_index = true; if (!indices) indirect_index = false; Scalar maxD = 0; for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Vec3s& p = ps[index]; Scalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; if (ps2) { const Vec3s& v = ps2[index]; Scalar d = (v - query).squaredNorm(); if (d > maxD) maxD = d; } } return std::sqrt(maxD); } Scalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices, unsigned int n, const Vec3s& query) { if (ts) return maximumDistance_mesh(ps, ps2, ts, indices, n, query); else return maximumDistance_pointcloud(ps, ps2, indices, n, query); } } // namespace coal ================================================ FILE: src/BVH/BV_fitter.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/internal/BV_fitter.h" #include "coal/BVH/BVH_utility.h" #include "coal/internal/tools.h" #include namespace coal { static const Scalar kIOS_RATIO = 1.5; static const Scalar invSinA = 2; static const Scalar cosA = sqrt(Scalar(3)) / Scalar(2); static inline void axisFromEigen(Vec3s eigenV[3], Scalar eigenS[3], Matrix3s& axes) { int min, mid, max; if (eigenS[0] > eigenS[1]) { max = 0; min = 1; } else { min = 0; max = 1; } if (eigenS[2] < eigenS[min]) { mid = min; min = 2; } else if (eigenS[2] > eigenS[max]) { mid = max; max = 2; } else { mid = 2; } axes.col(0) << eigenV[0][max], eigenV[1][max], eigenV[2][max]; axes.col(1) << eigenV[0][mid], eigenV[1][mid], eigenV[2][mid]; axes.col(2) << eigenV[1][max] * eigenV[2][mid] - eigenV[1][mid] * eigenV[2][max], eigenV[0][mid] * eigenV[2][max] - eigenV[0][max] * eigenV[2][mid], eigenV[0][max] * eigenV[1][mid] - eigenV[0][mid] * eigenV[1][max]; } namespace OBB_fit_functions { void fit1(Vec3s* ps, OBB& bv) { bv.To.noalias() = ps[0]; bv.axes.setIdentity(); bv.extent.setZero(); } void fit2(Vec3s* ps, OBB& bv) { const Vec3s& p1 = ps[0]; const Vec3s& p2 = ps[1]; Vec3s p1p2 = p1 - p2; Scalar len_p1p2 = p1p2.norm(); p1p2.normalize(); bv.axes.col(0).noalias() = p1p2; generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2)); bv.extent << len_p1p2 * Scalar(0.5), 0, 0; bv.To.noalias() = (p1 + p2) / 2; } void fit3(Vec3s* ps, OBB& bv) { const Vec3s& p1 = ps[0]; const Vec3s& p2 = ps[1]; const Vec3s& p3 = ps[2]; Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; Scalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); int imax = 0; if (len[1] > len[0]) imax = 1; if (len[2] > len[imax]) imax = 2; bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); bv.axes.col(0).noalias() = e[imax].normalized(); bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0)); getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent); } void fit6(Vec3s* ps, OBB& bv) { OBB bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } void fitn(Vec3s* ps, unsigned int n, OBB& bv) { Matrix3s M; Vec3s E[3]; Scalar s[3] = {0, 0, 0}; // three eigen values getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); axisFromEigen(E, s, bv.axes); // set obb centers and extensions getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axes, bv.To, bv.extent); } } // namespace OBB_fit_functions namespace RSS_fit_functions { void fit1(Vec3s* ps, RSS& bv) { bv.Tr.noalias() = ps[0]; bv.axes.setIdentity(); bv.length[0] = 0; bv.length[1] = 0; bv.radius = 0; } void fit2(Vec3s* ps, RSS& bv) { const Vec3s& p1 = ps[0]; const Vec3s& p2 = ps[1]; bv.axes.col(0).noalias() = p1 - p2; Scalar len_p1p2 = bv.axes.col(0).norm(); bv.axes.col(0) /= len_p1p2; generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2)); bv.length[0] = len_p1p2; bv.length[1] = 0; bv.Tr = p2; bv.radius = 0; } void fit3(Vec3s* ps, RSS& bv) { const Vec3s& p1 = ps[0]; const Vec3s& p2 = ps[1]; const Vec3s& p3 = ps[2]; Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; Scalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); int imax = 0; if (len[1] > len[0]) imax = 1; if (len[2] > len[imax]) imax = 2; bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); bv.axes.col(0).noalias() = e[imax].normalized(); bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0)); getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.length, bv.radius); } void fit6(Vec3s* ps, RSS& bv) { RSS bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } void fitn(Vec3s* ps, unsigned int n, RSS& bv) { Matrix3s M; // row first matrix Vec3s E[3]; // row first eigen-vectors Scalar s[3] = {0, 0, 0}; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); axisFromEigen(E, s, bv.axes); // set rss origin, rectangle size and radius getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.length, bv.radius); } } // namespace RSS_fit_functions namespace kIOS_fit_functions { void fit1(Vec3s* ps, kIOS& bv) { bv.num_spheres = 1; bv.spheres[0].o.noalias() = ps[0]; bv.spheres[0].r = 0; bv.obb.axes.setIdentity(); bv.obb.extent.setZero(); bv.obb.To.noalias() = ps[0]; } void fit2(Vec3s* ps, kIOS& bv) { bv.num_spheres = 5; const Vec3s& p1 = ps[0]; const Vec3s& p2 = ps[1]; Vec3s p1p2 = p1 - p2; Scalar len_p1p2 = p1p2.norm(); p1p2.normalize(); Matrix3s& axes = bv.obb.axes; axes.col(0).noalias() = p1p2; generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2)); Scalar r0 = len_p1p2 * Scalar(0.5); bv.obb.extent << r0, 0, 0; bv.obb.To = (p1 + p2) * 0.5; bv.spheres[0].o = bv.obb.To; bv.spheres[0].r = r0; Scalar r1 = r0 * invSinA; Scalar r1cosA = r1 * cosA; bv.spheres[1].r = r1; bv.spheres[2].r = r1; Vec3s delta = axes.col(1) * r1cosA; bv.spheres[1].o = bv.spheres[0].o - delta; bv.spheres[2].o = bv.spheres[0].o + delta; bv.spheres[3].r = r1; bv.spheres[4].r = r1; delta = axes.col(2) * r1cosA; bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; } void fit3(Vec3s* ps, kIOS& bv) { bv.num_spheres = 3; const Vec3s& p1 = ps[0]; const Vec3s& p2 = ps[1]; const Vec3s& p3 = ps[2]; Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; Scalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); int imax = 0; if (len[1] > len[0]) imax = 1; if (len[2] > len[imax]) imax = 2; bv.obb.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); bv.obb.axes.col(0).noalias() = e[imax].normalized(); bv.obb.axes.col(1).noalias() = bv.obb.axes.col(2).cross(bv.obb.axes.col(0)); getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axes, bv.obb.To, bv.obb.extent); // compute radius and center Scalar r0; Vec3s center; circumCircleComputation(p1, p2, p3, center, r0); bv.spheres[0].o = center; bv.spheres[0].r = r0; Scalar r1 = r0 * invSinA; Vec3s delta = bv.obb.axes.col(2) * (r1 * cosA); bv.spheres[1].r = r1; bv.spheres[1].o = center - delta; bv.spheres[2].r = r1; bv.spheres[2].o = center + delta; } void fitn(Vec3s* ps, unsigned int n, kIOS& bv) { Matrix3s M; Vec3s E[3]; Scalar s[3] = {0, 0, 0}; // three eigen values; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); Matrix3s& axes = bv.obb.axes; axisFromEigen(E, s, axes); getExtentAndCenter(ps, NULL, NULL, NULL, n, axes, bv.obb.To, bv.obb.extent); // get center and extension const Vec3s& center = bv.obb.To; const Vec3s& extent = bv.obb.extent; Scalar r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); // decide the k in kIOS if (extent[0] > kIOS_RATIO * extent[2]) { if (extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5; else bv.num_spheres = 3; } else bv.num_spheres = 1; bv.spheres[0].o = center; bv.spheres[0].r = r0; if (bv.num_spheres >= 3) { Scalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; Scalar r11 = 0, r12 = 0; r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o); r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o); bv.spheres[1].o += axes.col(2) * (-r10 + r11); bv.spheres[2].o += axes.col(2) * (r10 - r12); bv.spheres[1].r = r10; bv.spheres[2].r = r10; } if (bv.num_spheres >= 5) { Scalar r10 = bv.spheres[1].r; Vec3s delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; Scalar r21 = 0, r22 = 0; r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o); r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o); bv.spheres[3].o += axes.col(1) * (-r10 + r21); bv.spheres[4].o += axes.col(1) * (r10 - r22); bv.spheres[3].r = r10; bv.spheres[4].r = r10; } } } // namespace kIOS_fit_functions namespace OBBRSS_fit_functions { void fit1(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit1(ps, bv.obb); RSS_fit_functions::fit1(ps, bv.rss); } void fit2(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit2(ps, bv.obb); RSS_fit_functions::fit2(ps, bv.rss); } void fit3(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit3(ps, bv.obb); RSS_fit_functions::fit3(ps, bv.rss); } void fitn(Vec3s* ps, unsigned int n, OBBRSS& bv) { OBB_fit_functions::fitn(ps, n, bv.obb); RSS_fit_functions::fitn(ps, n, bv.rss); } } // namespace OBBRSS_fit_functions template <> void fit(Vec3s* ps, unsigned int n, OBB& bv) { switch (n) { case 1: OBB_fit_functions::fit1(ps, bv); break; case 2: OBB_fit_functions::fit2(ps, bv); break; case 3: OBB_fit_functions::fit3(ps, bv); break; case 6: OBB_fit_functions::fit6(ps, bv); break; default: OBB_fit_functions::fitn(ps, n, bv); } } template <> void fit(Vec3s* ps, unsigned int n, RSS& bv) { switch (n) { case 1: RSS_fit_functions::fit1(ps, bv); break; case 2: RSS_fit_functions::fit2(ps, bv); break; case 3: RSS_fit_functions::fit3(ps, bv); break; default: RSS_fit_functions::fitn(ps, n, bv); } } template <> void fit(Vec3s* ps, unsigned int n, kIOS& bv) { switch (n) { case 1: kIOS_fit_functions::fit1(ps, bv); break; case 2: kIOS_fit_functions::fit2(ps, bv); break; case 3: kIOS_fit_functions::fit3(ps, bv); break; default: kIOS_fit_functions::fitn(ps, n, bv); } } template <> void fit(Vec3s* ps, unsigned int n, OBBRSS& bv) { switch (n) { case 1: OBBRSS_fit_functions::fit1(ps, bv); break; case 2: OBBRSS_fit_functions::fit2(ps, bv); break; case 3: OBBRSS_fit_functions::fit3(ps, bv); break; default: OBBRSS_fit_functions::fitn(ps, n, bv); } } template <> void fit(Vec3s* ps, unsigned int n, AABB& bv) { if (n <= 0) return; bv = AABB(ps[0]); for (unsigned int i = 1; i < n; ++i) { bv += ps[i]; } } OBB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { OBB bv; Matrix3s M; // row first matrix Vec3s E[3]; // row first eigen-vectors Scalar s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.axes); // set obb centers and extensions getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, bv.To, bv.extent); return bv; } OBBRSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { OBBRSS bv; Matrix3s M; Vec3s E[3]; Scalar s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.obb.axes); bv.rss.axes.noalias() = bv.obb.axes; getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent); Vec3s origin; Scalar l[2]; Scalar r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axes, origin, l, r); bv.rss.Tr = origin; bv.rss.length[0] = l[0]; bv.rss.length[1] = l[1]; bv.rss.radius = r; return bv; } RSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { RSS bv; Matrix3s M; // row first matrix Vec3s E[3]; // row first eigen-vectors Scalar s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.axes); // set rss origin, rectangle size and radius Vec3s origin; Scalar l[2]; Scalar r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, origin, l, r); bv.Tr = origin; bv.length[0] = l[0]; bv.length[1] = l[1]; bv.radius = r; return bv; } kIOS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { kIOS bv; Matrix3s M; // row first matrix Vec3s E[3]; // row first eigen-vectors Scalar s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); Matrix3s& axes = bv.obb.axes; axisFromEigen(E, s, axes); // get centers and extensions getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axes, bv.obb.To, bv.obb.extent); const Vec3s& center = bv.obb.To; const Vec3s& extent = bv.obb.extent; Scalar r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center); // decide k in kIOS if (extent[0] > kIOS_RATIO * extent[2]) { if (extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5; else bv.num_spheres = 3; } else bv.num_spheres = 1; bv.spheres[0].o = center; bv.spheres[0].r = r0; if (bv.num_spheres >= 3) { Scalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; Scalar r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o); Scalar r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o); bv.spheres[1].o += axes.col(2) * (-r10 + r11); bv.spheres[2].o += axes.col(2) * (r10 - r12); bv.spheres[1].r = r10; bv.spheres[2].r = r10; } if (bv.num_spheres >= 5) { Scalar r10 = bv.spheres[1].r; Vec3s delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; Scalar r21 = 0, r22 = 0; r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o); r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o); bv.spheres[3].o += axes.col(1) * (-r10 + r21); bv.spheres[4].o += axes.col(1) * (r10 - r22); bv.spheres[3].r = r10; bv.spheres[4].r = r10; } return bv; } AABB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { AABB bv; if (num_primitives == 0) return bv; if (type == BVH_MODEL_TRIANGLES) /// The primitive is triangle { Triangle32 t0 = tri_indices[primitive_indices[0]]; bv = AABB(vertices[t0[0]]); for (unsigned int i = 0; i < num_primitives; ++i) { Triangle32 t = tri_indices[primitive_indices[i]]; bv += vertices[t[0]]; bv += vertices[t[1]]; bv += vertices[t[2]]; if (prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[t[0]]; bv += prev_vertices[t[1]]; bv += prev_vertices[t[2]]; } } return bv; } else if (type == BVH_MODEL_POINTCLOUD) /// The primitive is point { bv = AABB(vertices[primitive_indices[0]]); for (unsigned int i = 0; i < num_primitives; ++i) { bv += vertices[primitive_indices[i]]; if (prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[primitive_indices[i]]; } } } return bv; } } // namespace coal ================================================ FILE: src/BVH/BV_splitter.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/internal/BV_splitter.h" namespace coal { template void computeSplitVector(const BV& bv, Vec3s& split_vector) { split_vector = bv.axes.col(0); } template <> void computeSplitVector(const kIOS& bv, Vec3s& split_vector) { /* switch(bv.num_spheres) { case 1: split_vector = Vec3s(1, 0, 0); break; case 3: { Vec3s v[3]; v[0] = bv.spheres[1].o - bv.spheres[0].o; v[0].normalize(); generateCoordinateSystem(v[0], v[1], v[2]); split_vector = v[1]; } break; case 5: { Vec3s v[2]; v[0] = bv.spheres[1].o - bv.spheres[0].o; v[1] = bv.spheres[3].o - bv.spheres[0].o; split_vector = v[0].cross(v[1]); split_vector.normalize(); } break; default: ; } */ split_vector = bv.obb.axes.col(0); } template <> void computeSplitVector(const OBBRSS& bv, Vec3s& split_vector) { split_vector = bv.obb.axes.col(0); } template void computeSplitValue_bvcenter(const BV& bv, Scalar& split_value) { Vec3s center = bv.center(); split_value = center[0]; } template void computeSplitValue_mean(const BV&, Vec3s* vertices, Triangle32* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, const Vec3s& split_vector, Scalar& split_value) { if (type == BVH_MODEL_TRIANGLES) { Vec3s c(Vec3s::Zero()); for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle32& t = triangles[primitive_indices[i]]; const Vec3s& p1 = vertices[t[0]]; const Vec3s& p2 = vertices[t[1]]; const Vec3s& p3 = vertices[t[2]]; c += p1 + p2 + p3; } split_value = c.dot(split_vector) / Scalar(3 * num_primitives); } else if (type == BVH_MODEL_POINTCLOUD) { Scalar sum = 0; for (unsigned int i = 0; i < num_primitives; ++i) { const Vec3s& p = vertices[primitive_indices[i]]; sum += p.dot(split_vector); } split_value = sum / Scalar(num_primitives); } } template void computeSplitValue_median(const BV&, Vec3s* vertices, Triangle32* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, const Vec3s& split_vector, Scalar& split_value) { std::vector proj(num_primitives); if (type == BVH_MODEL_TRIANGLES) { for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle32& t = triangles[primitive_indices[i]]; const Vec3s& p1 = vertices[t[0]]; const Vec3s& p2 = vertices[t[1]]; const Vec3s& p3 = vertices[t[2]]; Vec3s centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1], p1[2] + p2[2] + p3[2]); proj[i] = centroid3.dot(split_vector) / 3; } } else if (type == BVH_MODEL_POINTCLOUD) { for (unsigned int i = 0; i < num_primitives; ++i) { const Vec3s& p = vertices[primitive_indices[i]]; Vec3s v(p[0], p[1], p[2]); proj[i] = v.dot(split_vector); } } std::sort(proj.begin(), proj.end()); if (num_primitives % 2 == 1) { split_value = proj[(num_primitives - 1) / 2]; } else { split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; } } template <> void BVSplitter::computeRule_bvcenter(const OBB& bv, unsigned int*, unsigned int) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } template <> void BVSplitter::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template <> void BVSplitter::computeRule_median(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template <> void BVSplitter::computeRule_bvcenter(const RSS& bv, unsigned int*, unsigned int) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } template <> void BVSplitter::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template <> void BVSplitter::computeRule_median(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template <> void BVSplitter::computeRule_bvcenter(const kIOS& bv, unsigned int*, unsigned int) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } template <> void BVSplitter::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template <> void BVSplitter::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template <> void BVSplitter::computeRule_bvcenter(const OBBRSS& bv, unsigned int*, unsigned int) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } template <> void BVSplitter::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template <> void BVSplitter::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template <> bool BVSplitter::apply(const Vec3s& q) const { return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> bool BVSplitter::apply(const Vec3s& q) const { return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> bool BVSplitter::apply(const Vec3s& q) const { return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> bool BVSplitter::apply(const Vec3s& q) const { return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template class BVSplitter; template class BVSplitter; template class BVSplitter; template class BVSplitter; } // namespace coal ================================================ FILE: src/CMakeLists.txt ================================================ # # Software License Agreement (BSD License) # # Copyright (c) 2014, 2020-2024 CNRS-LAAS INRIA # Author: Florent Lamiraux # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of CNRS-LAAS. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. set(LIBRARY_NAME ${PROJECT_NAME}) set( ${LIBRARY_NAME}_SOURCES collision.cpp contact_patch.cpp contact_patch/contact_patch_solver.cpp contact_patch/contact_patch_simplifier.cpp contact_patch/polygon_convex_hull.cpp contact_patch_func_matrix.cpp distance_func_matrix.cpp collision_data.cpp collision_node.cpp collision_object.cpp BV/RSS.cpp BV/AABB.cpp BV/kIOS.cpp BV/kDOP.cpp BV/OBBRSS.cpp BV/OBB.cpp broadphase/default_broadphase_callbacks.cpp broadphase/broadphase_dynamic_AABB_tree.cpp broadphase/broadphase_dynamic_AABB_tree_array.cpp broadphase/broadphase_bruteforce.cpp broadphase/broadphase_collision_manager.cpp broadphase/broadphase_SaP.cpp broadphase/broadphase_SSaP.cpp broadphase/broadphase_interval_tree.cpp broadphase/detail/interval_tree.cpp broadphase/detail/interval_tree_node.cpp broadphase/detail/simple_interval.cpp broadphase/detail/spatial_hash.cpp broadphase/detail/morton.cpp narrowphase/gjk.cpp narrowphase/minkowski_difference.cpp narrowphase/support_functions.cpp narrowphase/details.h shape/geometric_shapes.cpp shape/geometric_shapes_utility.cpp distance/box_halfspace.cpp distance/box_plane.cpp distance/box_sphere.cpp distance/capsule_capsule.cpp distance/capsule_halfspace.cpp distance/capsule_plane.cpp distance/cone_halfspace.cpp distance/cone_plane.cpp distance/cylinder_halfspace.cpp distance/cylinder_plane.cpp distance/sphere_sphere.cpp distance/sphere_cylinder.cpp distance/sphere_halfspace.cpp distance/sphere_plane.cpp distance/sphere_capsule.cpp distance/ellipsoid_halfspace.cpp distance/ellipsoid_plane.cpp distance/convex_halfspace.cpp distance/convex_plane.cpp distance/triangle_halfspace.cpp distance/triangle_plane.cpp distance/triangle_triangle.cpp distance/triangle_sphere.cpp distance/halfspace_plane.cpp distance/plane_plane.cpp distance/halfspace_halfspace.cpp intersect.cpp math/transform.cpp traversal/traversal_recurse.cpp distance.cpp BVH/BVH_utility.cpp BVH/BV_fitter.cpp BVH/BVH_model.cpp BVH/BV_splitter.cpp collision_func_matrix.cpp collision_utility.cpp mesh_loader/assimp.cpp mesh_loader/loader.cpp hfield.cpp serialization/serialization.cpp ) if(COAL_HAS_OCTOMAP) list(APPEND ${LIBRARY_NAME}_SOURCES octree.cpp) endif(COAL_HAS_OCTOMAP) set(PROJECT_HEADERS_FULL_PATH) foreach(header ${${PROJECT_NAME}_HEADERS}) list(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_SOURCE_DIR}/${header}) endforeach() list( APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/config.hh ) list( APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/deprecated.hh ) list( APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/warning.hh ) add_library( ${LIBRARY_NAME} SHARED ${PROJECT_HEADERS_FULL_PATH} ${${LIBRARY_NAME}_SOURCES} ) add_library(${LIBRARY_NAME}::${LIBRARY_NAME} ALIAS ${LIBRARY_NAME}) set_standard_output_directory(${LIBRARY_NAME}) if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) add_library(hpp-fcl ALIAS ${LIBRARY_NAME}) add_library(hpp-fcl::hpp-fcl ALIAS ${LIBRARY_NAME}) endif() if(UNIX) GET_RELATIVE_RPATH(${CMAKE_INSTALL_LIBDIR} ${PROJECT_NAME}_INSTALL_RPATH) set_target_properties( ${PROJECT_NAME} PROPERTIES INSTALL_RPATH "${${PROJECT_NAME}_INSTALL_RPATH}" ) endif() if(SUFFIX_SO_VERSION) set_target_properties( ${PROJECT_NAME} PROPERTIES VERSION ${PROJECT_VERSION} SOVERSION ${PROJECT_VERSION_MAJOR} ) endif() CXX_FLAGS_BY_COMPILER_FRONTEND(MSVC "/bigobj" OUTPUT PUBLIC_OPTIONS FILTER) target_compile_options(${PROJECT_NAME} PUBLIC ${PUBLIC_OPTIONS}) CXX_FLAGS_BY_COMPILER_FRONTEND(MSVC "NOMINMAX" OUTPUT PUBLIC_DEFINITIONS) target_compile_definitions(${PROJECT_NAME} INTERFACE ${PUBLIC_DEFINITIONS}) # IDE sources and headers sorting ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES) ADD_HEADER_GROUP(PROJECT_HEADERS_FULL_PATH) MODERNIZE_TARGET_LINK_LIBRARIES( ${LIBRARY_NAME} SCOPE PRIVATE TARGETS assimp::assimp LIBRARIES ${assimp_LIBRARIES} INCLUDE_DIRS ${assimp_INCLUDE_DIR} ) target_link_libraries( ${LIBRARY_NAME} PUBLIC Boost::serialization Boost::filesystem ) if(COAL_ENABLE_LOGGING) target_link_libraries(${LIBRARY_NAME} PUBLIC Boost::log) # The compile flag `BOOST_LOG_DYN_LINK` is required here. target_compile_definitions( ${LIBRARY_NAME} PUBLIC COAL_ENABLE_LOGGING BOOST_LOG_DYN_LINK ) endif() if(COAL_USE_FLOAT_PRECISION) target_compile_definitions(${LIBRARY_NAME} PUBLIC COAL_USE_FLOAT_PRECISION) endif() if(COAL_BUILD_WITH_TRACY) target_compile_definitions(${LIBRARY_NAME} PUBLIC COAL_TRACY_ENABLE) target_link_libraries(${LIBRARY_NAME} PUBLIC Tracy::TracyClient) endif() if(WIN32) # There is an issue with MSVC 2017 and Eigen (due to std::aligned_storage). # See https://github.com/ceres-solver/ceres-solver/issues/481 target_compile_definitions( ${LIBRARY_NAME} PRIVATE _ENABLE_EXTENDED_ALIGNED_STORAGE ) endif(WIN32) if(COAL_TURN_ASSERT_INTO_EXCEPTION) target_compile_definitions( ${LIBRARY_NAME} PUBLIC COAL_TURN_ASSERT_INTO_EXCEPTION ) endif() if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) target_compile_definitions( ${LIBRARY_NAME} PUBLIC COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL ) endif() if(COAL_HAS_QHULL) target_compile_definitions(${LIBRARY_NAME} PRIVATE COAL_HAS_QHULL) target_link_libraries(${LIBRARY_NAME} PRIVATE Qhull::qhull_r Qhull::qhullcpp) endif() MODERNIZE_TARGET_LINK_LIBRARIES( ${PROJECT_NAME} SCOPE PUBLIC TARGETS Eigen3::Eigen INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR} ) target_include_directories( ${LIBRARY_NAME} PUBLIC $ $ $ ) # boost::span requires Boost >= 1.78.0. # On Ubuntu 22.04 the system Boost is 1.74 and is too old. # In this case, we add include/coal/third_party as include directory. if(Boost_VERSION VERSION_LESS 1.78) target_include_directories( ${LIBRARY_NAME} PUBLIC $ $ ) endif() if(octomap_FOUND) MODERNIZE_TARGET_LINK_LIBRARIES( ${PROJECT_NAME} SCOPE PUBLIC TARGETS octomap LIBRARIES ${OCTOMAP_LIBRARIES} INCLUDE_DIRS ${OCTOMAP_INCLUDE_DIRS} ) target_compile_definitions( ${LIBRARY_NAME} PUBLIC COAL_HAS_OCTOMAP COAL_HAVE_OCTOMAP OCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} OCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} OCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION} ) endif(octomap_FOUND) install( TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} ) ================================================ FILE: src/broadphase/broadphase_SSaP.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #include "coal/broadphase/broadphase_SSaP.h" #include "coal/tracy.hh" namespace coal { /** @brief Functor sorting objects according to the AABB lower x bound */ struct SortByXLow { bool operator()(const CollisionObject* a, const CollisionObject* b) const { if (a->getAABB().min_[0] < b->getAABB().min_[0]) return true; return false; } }; /** @brief Functor sorting objects according to the AABB lower y bound */ struct SortByYLow { bool operator()(const CollisionObject* a, const CollisionObject* b) const { if (a->getAABB().min_[1] < b->getAABB().min_[1]) return true; return false; } }; /** @brief Functor sorting objects according to the AABB lower z bound */ struct SortByZLow { bool operator()(const CollisionObject* a, const CollisionObject* b) const { if (a->getAABB().min_[2] < b->getAABB().min_[2]) return true; return false; } }; /** @brief Dummy collision object with a point AABB */ class COAL_DLLAPI DummyCollisionObject : public CollisionObject { public: DummyCollisionObject(const AABB& aabb_) : CollisionObject(shared_ptr()) { this->aabb = aabb_; } void computeLocalAABB() {} }; //============================================================================== void SSaPCollisionManager::unregisterObject(CollisionObject* obj) { setup(); DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); auto pos_start1 = objs_x.begin(); auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); while (pos_start1 < pos_end1) { if (*pos_start1 == obj) { objs_x.erase(pos_start1); break; } ++pos_start1; } auto pos_start2 = objs_y.begin(); auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); while (pos_start2 < pos_end2) { if (*pos_start2 == obj) { objs_y.erase(pos_start2); break; } ++pos_start2; } auto pos_start3 = objs_z.begin(); auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); while (pos_start3 < pos_end3) { if (*pos_start3 == obj) { objs_z.erase(pos_start3); break; } ++pos_start3; } } //============================================================================== SSaPCollisionManager::SSaPCollisionManager() : setup_(false) { // Do nothing } //============================================================================== void SSaPCollisionManager::registerObject(CollisionObject* obj) { objs_x.push_back(obj); objs_y.push_back(obj); objs_z.push_back(obj); setup_ = false; } //============================================================================== void SSaPCollisionManager::setup() { if (!setup_) { std::sort(objs_x.begin(), objs_x.end(), SortByXLow()); std::sort(objs_y.begin(), objs_y.end(), SortByYLow()); std::sort(objs_z.begin(), objs_z.end(), SortByZLow()); setup_ = true; } } //============================================================================== void SSaPCollisionManager::update() { setup_ = false; setup(); } //============================================================================== void SSaPCollisionManager::clear() { objs_x.clear(); objs_y.clear(); objs_z.clear(); setup_ = false; } //============================================================================== void SSaPCollisionManager::getObjects( std::vector& objs) const { objs.resize(objs_x.size()); std::copy(objs_x.begin(), objs_x.end(), objs.begin()); } //============================================================================== bool SSaPCollisionManager::checkColl( std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, CollisionObject* obj, CollisionCallBackBase* callback) const { while (pos_start < pos_end) { if (*pos_start != obj) // no collision between the same object { if ((*pos_start)->getAABB().overlap(obj->getAABB())) { if ((*callback)(*pos_start, obj)) return true; } } pos_start++; } return false; } //============================================================================== bool SSaPCollisionManager::checkDis( typename std::vector::const_iterator pos_start, typename std::vector::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const { while (pos_start < pos_end) { if (*pos_start != obj) // no distance between the same object { if ((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) { if ((*callback)(*pos_start, obj, min_dist)) return true; } } pos_start++; } return false; } //============================================================================== void SSaPCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SSaPCollisionManager::collide(CollisionObject*, " "CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; collide_(obj, callback); } //============================================================================== bool SSaPCollisionManager::collide_(CollisionObject* obj, CollisionCallBackBase* callback) const { static const unsigned int CUTOFF = 100; DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); bool coll_res = false; const auto pos_start1 = objs_x.begin(); const auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); long d1 = pos_end1 - pos_start1; if (d1 > CUTOFF) { const auto pos_start2 = objs_y.begin(); const auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); long d2 = pos_end2 - pos_start2; if (d2 > CUTOFF) { const auto pos_start3 = objs_z.begin(); const auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); long d3 = pos_end3 - pos_start3; if (d3 > CUTOFF) { if (d3 <= d2 && d3 <= d1) coll_res = checkColl(pos_start3, pos_end3, obj, callback); else { if (d2 <= d3 && d2 <= d1) coll_res = checkColl(pos_start2, pos_end2, obj, callback); else coll_res = checkColl(pos_start1, pos_end1, obj, callback); } } else coll_res = checkColl(pos_start3, pos_end3, obj, callback); } else coll_res = checkColl(pos_start2, pos_end2, obj, callback); } else coll_res = checkColl(pos_start1, pos_end1, obj, callback); return coll_res; } //============================================================================== void SSaPCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SSaPCollisionManager::distance(CollisionObject*, " "DistanceCallBackBase*)"); callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== bool SSaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const { static const unsigned int CUTOFF = 100; Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; Vec3s dummy_vector = obj->getAABB().max_; if (min_dist < (std::numeric_limits::max)()) dummy_vector += Vec3s(min_dist, min_dist, min_dist); typename std::vector::const_iterator pos_start1 = objs_x.begin(); typename std::vector::const_iterator pos_start2 = objs_y.begin(); typename std::vector::const_iterator pos_start3 = objs_z.begin(); typename std::vector::const_iterator pos_end1 = objs_x.end(); typename std::vector::const_iterator pos_end2 = objs_y.end(); typename std::vector::const_iterator pos_end3 = objs_z.end(); int status = 1; Scalar old_min_distance; while (1) { old_min_distance = min_dist; DummyCollisionObject dummyHigh((AABB(dummy_vector))); pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); long d1 = pos_end1 - pos_start1; bool dist_res = false; if (d1 > CUTOFF) { pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); long d2 = pos_end2 - pos_start2; if (d2 > CUTOFF) { pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); long d3 = pos_end3 - pos_start3; if (d3 > CUTOFF) { if (d3 <= d2 && d3 <= d1) dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist); else { if (d2 <= d3 && d2 <= d1) dist_res = checkDis(pos_start2, pos_end2, obj, callback, min_dist); else dist_res = checkDis(pos_start1, pos_end1, obj, callback, min_dist); } } else dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist); } else dist_res = checkDis(pos_start2, pos_end2, obj, callback, min_dist); } else { dist_res = checkDis(pos_start1, pos_end1, obj, callback, min_dist); } if (dist_res) return true; if (status == 1) { if (old_min_distance < (std::numeric_limits::max)()) break; else { // from infinity to a finite one, only need one additional loop // to check the possible missed ones to the right of the objs array if (min_dist < old_min_distance) { dummy_vector = obj->getAABB().max_ + Vec3s(min_dist, min_dist, min_dist); status = 0; } else // need more loop { if (dummy_vector.isApprox( obj->getAABB().max_, std::numeric_limits::epsilon() * 100)) dummy_vector = dummy_vector + delta; else dummy_vector = dummy_vector * 2 - obj->getAABB().max_; } } // yes, following is wrong, will result in too large distance. // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1; // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2; // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3; } else if (status == 0) break; } return false; } //============================================================================== int SSaPCollisionManager::selectOptimalAxis( const std::vector& objs_x, const std::vector& objs_y, const std::vector& objs_z, typename std::vector::const_iterator& it_beg, typename std::vector::const_iterator& it_end) { /// simple sweep and prune method Scalar delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0]; Scalar delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1]; Scalar delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2]; int axis = 0; if (delta_y > delta_x && delta_y > delta_z) axis = 1; else if (delta_z > delta_y && delta_z > delta_x) axis = 2; switch (axis) { case 0: it_beg = objs_x.begin(); it_end = objs_x.end(); break; case 1: it_beg = objs_y.begin(); it_end = objs_y.end(); break; case 2: it_beg = objs_z.begin(); it_end = objs_z.end(); break; } return axis; } //============================================================================== void SSaPCollisionManager::collide(CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SSaPCollisionManager::collide(CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; typename std::vector::const_iterator pos, run_pos, pos_end; int axis = selectOptimalAxis(objs_x, objs_y, objs_z, pos, pos_end); int axis2 = (axis + 1 > 2) ? 0 : (axis + 1); int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1); run_pos = pos; while ((run_pos < pos_end) && (pos < pos_end)) { CollisionObject* obj = *(pos++); while (1) { if ((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) { run_pos++; if (run_pos == pos_end) break; continue; } else { run_pos++; break; } } if (run_pos < pos_end) { typename std::vector::const_iterator run_pos2 = run_pos; while ((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) { CollisionObject* obj2 = *run_pos2; run_pos2++; if ((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) { if ((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) { if ((*callback)(obj, obj2)) return; } } if (run_pos2 == pos_end) break; } } } } //============================================================================== void SSaPCollisionManager::distance(DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SSaPCollisionManager::distance(DistanceCallBackBase*)"); callback->init(); if (size() == 0) return; typename std::vector::const_iterator it, it_end; selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); Scalar min_dist = (std::numeric_limits::max)(); for (; it != it_end; ++it) { if (distance_(*it, callback, min_dist)) return; } } //============================================================================== void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SSaPCollisionManager::collide(BroadPhaseCollisionManager*, " "CollisionCallBackBase*)"); callback->init(); SSaPCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { collide(callback); return; } typename std::vector::const_iterator it, end; if (this->size() < other_manager->size()) { for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it) if (other_manager->collide_(*it, callback)) return; } else { for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) if (collide_(*it, callback)) return; } } //============================================================================== void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SSaPCollisionManager::distance(BroadPhaseCollisionManager*, " "DistanceCallBackBase*)"); callback->init(); SSaPCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { distance(callback); return; } Scalar min_dist = (std::numeric_limits::max)(); typename std::vector::const_iterator it, end; if (this->size() < other_manager->size()) { for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it) if (other_manager->distance_(*it, callback, min_dist)) return; } else { for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) if (distance_(*it, callback, min_dist)) return; } } //============================================================================== bool SSaPCollisionManager::empty() const { return objs_x.empty(); } //============================================================================== size_t SSaPCollisionManager::size() const { return objs_x.size(); } } // namespace coal ================================================ FILE: src/broadphase/broadphase_SaP.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #include "coal/broadphase/broadphase_SaP.h" #include "coal/tracy.hh" namespace coal { //============================================================================== void SaPCollisionManager::unregisterObject(CollisionObject* obj) { auto it = AABB_arr.begin(); for (auto end = AABB_arr.end(); it != end; ++it) { if ((*it)->obj == obj) break; } AABB_arr.erase(it); obj_aabb_map.erase(obj); if (it == AABB_arr.end()) return; SaPAABB* curr = *it; *it = nullptr; for (int coord = 0; coord < 3; ++coord) { // first delete the lo endpoint of the interval. if (curr->lo->prev[coord] == nullptr) elist[coord] = curr->lo->next[coord]; else curr->lo->prev[coord]->next[coord] = curr->lo->next[coord]; curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord]; // then, delete the "hi" endpoint. if (curr->hi->prev[coord] == nullptr) elist[coord] = curr->hi->next[coord]; else curr->hi->prev[coord]->next[coord] = curr->hi->next[coord]; if (curr->hi->next[coord] != nullptr) curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord]; } delete curr->lo; delete curr->hi; delete curr; overlap_pairs.remove_if(isUnregistered(obj)); } //============================================================================== SaPCollisionManager::SaPCollisionManager() { elist[0] = nullptr; elist[1] = nullptr; elist[2] = nullptr; optimal_axis = 0; } //============================================================================== SaPCollisionManager::~SaPCollisionManager() { clear(); } //============================================================================== void SaPCollisionManager::registerObjects( const std::vector& other_objs) { if (other_objs.empty()) return; if (size() > 0) BroadPhaseCollisionManager::registerObjects(other_objs); else { std::vector endpoints(2 * other_objs.size()); for (size_t i = 0; i < other_objs.size(); ++i) { SaPAABB* sapaabb = new SaPAABB(); sapaabb->obj = other_objs[i]; sapaabb->lo = new EndPoint(); sapaabb->hi = new EndPoint(); sapaabb->cached = other_objs[i]->getAABB(); endpoints[2 * i] = sapaabb->lo; endpoints[2 * i + 1] = sapaabb->hi; sapaabb->lo->minmax = 0; sapaabb->hi->minmax = 1; sapaabb->lo->aabb = sapaabb; sapaabb->hi->aabb = sapaabb; AABB_arr.push_back(sapaabb); obj_aabb_map[other_objs[i]] = sapaabb; } Scalar scale[3]; for (int coord = 0; coord < 3; ++coord) { std::sort( endpoints.begin(), endpoints.end(), std::bind(std::less(), std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, coord), std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, coord))); endpoints[0]->prev[coord] = nullptr; endpoints[0]->next[coord] = endpoints[1]; for (size_t i = 1; i < endpoints.size() - 1; ++i) { endpoints[i]->prev[coord] = endpoints[i - 1]; endpoints[i]->next[coord] = endpoints[i + 1]; } endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2]; endpoints[endpoints.size() - 1]->next[coord] = nullptr; elist[coord] = endpoints[0]; scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord]; } int axis = 0; if (scale[axis] < scale[1]) axis = 1; if (scale[axis] < scale[2]) axis = 2; EndPoint* pos = elist[axis]; while (pos != nullptr) { EndPoint* pos_next = nullptr; SaPAABB* aabb = pos->aabb; EndPoint* pos_it = pos->next[axis]; while (pos_it != nullptr) { if (pos_it->aabb == aabb) { if (pos_next == nullptr) pos_next = pos_it; break; } if (pos_it->minmax == 0) { if (pos_next == nullptr) pos_next = pos_it; if (pos_it->aabb->cached.overlap(aabb->cached)) overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj); } pos_it = pos_it->next[axis]; } pos = pos_next; } } updateVelist(); } //============================================================================== void SaPCollisionManager::registerObject(CollisionObject* obj) { // Initialize a new SaPAABB associated with current Collision Object SaPAABB* new_sap = new SaPAABB; new_sap->cached = obj->getAABB(); new_sap->obj = obj; new_sap->lo = new EndPoint; new_sap->lo->minmax = 0; new_sap->lo->aabb = new_sap; new_sap->hi = new EndPoint; new_sap->hi->minmax = 1; new_sap->hi->aabb = new_sap; for (int coord = 0; coord < 3; ++coord) { EndPoint* current = elist[coord]; // first insert the lo end point if (current == nullptr) // empty list { elist[coord] = new_sap->lo; new_sap->lo->prev[coord] = new_sap->lo->next[coord] = nullptr; } else // otherwise, find the correct location in the list and insert { EndPoint* curr_lo = new_sap->lo; Scalar curr_lo_val = curr_lo->getVal()[coord]; while ((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != nullptr)) current = current->next[coord]; if (current->getVal()[coord] >= curr_lo_val) { curr_lo->prev[coord] = current->prev[coord]; curr_lo->next[coord] = current; if (current->prev[coord] == nullptr) // current was the first box elist[coord] = curr_lo; // new_sap->lo becomes the new first box on the axis else current->prev[coord]->next[coord] = curr_lo; current->prev[coord] = curr_lo; // new_sap->lo becomes the predecessor of current } else { // current->next[coord] == nullptr, so the current is just the // cell before current-> curr_lo->prev[coord] = current; curr_lo->next[coord] = nullptr; current->next[coord] = curr_lo; } } // now insert hi end point current = new_sap->lo; EndPoint* curr_hi = new_sap->hi; Scalar curr_hi_val = curr_hi->getVal()[coord]; if (coord == 0) { while ((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr)) { if (current != new_sap->lo) if (current->aabb->cached.overlap(new_sap->cached)) overlap_pairs.emplace_back(current->aabb->obj, obj); current = current->next[coord]; } } else { while ((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr)) current = current->next[coord]; } if (current->getVal()[coord] >= curr_hi_val) { curr_hi->prev[coord] = current->prev[coord]; curr_hi->next[coord] = current; if (current->prev[coord] != nullptr) current->prev[coord]->next[coord] = curr_hi; current->prev[coord] = curr_hi; } else { curr_hi->prev[coord] = current; curr_hi->next[coord] = nullptr; current->next[coord] = curr_hi; } } AABB_arr.push_back(new_sap); obj_aabb_map[obj] = new_sap; updateVelist(); } //============================================================================== void SaPCollisionManager::setup() { if (size() == 0) return; Scalar scale[3]; scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0); scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1); scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); int axis = 0; if (scale[axis] < scale[1]) axis = 1; if (scale[axis] < scale[2]) axis = 2; optimal_axis = axis; } //============================================================================== void SaPCollisionManager::update_(SaPAABB* updated_aabb) { if (updated_aabb->cached == updated_aabb->obj->getAABB()) return; SaPAABB* current = updated_aabb; const AABB current_aabb = current->obj->getAABB(); const Vec3s& new_min = current_aabb.min_; const Vec3s& new_max = current_aabb.max_; for (int coord = 0; coord < 3; ++coord) { int direction; // -1 reverse, 0 nochange, 1 forward EndPoint* temp; if (current->lo->getVal(coord) > new_min[coord]) direction = -1; else if (current->lo->getVal(coord) < new_min[coord]) direction = 1; else direction = 0; if (direction == -1) { // first update the "lo" endpoint of the interval if (current->lo->prev[coord] != nullptr) { temp = current->lo->prev[coord]; while ((temp != nullptr) && (temp->getVal(coord) > new_min[coord])) { if (temp->minmax == 1) if (temp->aabb->cached.overlap(current_aabb)) addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->prev[coord]; } if (temp == nullptr) { current->lo->prev[coord]->next[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = nullptr; current->lo->next[coord] = elist[coord]; elist[coord]->prev[coord] = current->lo; elist[coord] = current->lo; } else { current->lo->prev[coord]->next[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = temp; current->lo->next[coord] = temp->next[coord]; temp->next[coord]->prev[coord] = current->lo; temp->next[coord] = current->lo; } } // Update the value of the lower bound along axis coord current->lo->getVal(coord) = new_min[coord]; // update hi end point if (current->hi->prev[coord] != nullptr) { temp = current->hi->prev[coord]; while ((temp != nullptr) && (temp->getVal(coord) > new_max[coord])) { if ((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached))) removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->prev[coord]; } current->hi->prev[coord]->next[coord] = current->hi->next[coord]; if (current->hi->next[coord] != nullptr) current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp; // Wrong line current->hi->next[coord] = temp->next[coord]; if (temp->next[coord] != nullptr) temp->next[coord]->prev[coord] = current->hi; temp->next[coord] = current->hi; current->hi->getVal(coord) = new_max[coord]; } current->hi->getVal(coord) = new_max[coord]; } else if (direction == 1) { // here, we first update the "hi" endpoint. if (current->hi->next[coord] != nullptr) { temp = current->hi->next[coord]; while ((temp->next[coord] != nullptr) && (temp->getVal(coord) < new_max[coord])) { if (temp->minmax == 0) if (temp->aabb->cached.overlap(current_aabb)) addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->next[coord]; } if (temp->getVal(coord) < new_max[coord]) { current->hi->prev[coord]->next[coord] = current->hi->next[coord]; current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp; current->hi->next[coord] = nullptr; temp->next[coord] = current->hi; } else { current->hi->prev[coord]->next[coord] = current->hi->next[coord]; current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp->prev[coord]; current->hi->next[coord] = temp; temp->prev[coord]->next[coord] = current->hi; temp->prev[coord] = current->hi; } } current->hi->getVal(coord) = new_max[coord]; // then, update the "lo" endpoint of the interval. if (current->lo->next[coord] != nullptr) { temp = current->lo->next[coord]; while ((temp->next[coord] != nullptr) && (temp->getVal(coord) < new_min[coord])) { if ((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached))) removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->next[coord]; } if (current->lo->prev[coord] != nullptr) current->lo->prev[coord]->next[coord] = current->lo->next[coord]; else elist[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = temp->prev[coord]; current->lo->next[coord] = temp; if (temp->prev[coord] != nullptr) temp->prev[coord]->next[coord] = current->lo; else elist[coord] = current->lo; temp->prev[coord] = current->lo; current->lo->getVal(coord) = new_min[coord]; } } } } //============================================================================== void SaPCollisionManager::updateVelist() { for (int coord = 0; coord < 3; ++coord) { velist[coord].resize(size() * 2); EndPoint* current = elist[coord]; size_t id = 0; while (current) { velist[coord][id] = current; current = current->next[coord]; id++; } } } //============================================================================== void SaPCollisionManager::update(CollisionObject* updated_obj) { update_(obj_aabb_map[updated_obj]); updateVelist(); setup(); } //============================================================================== void SaPCollisionManager::update( const std::vector& updated_objs) { for (size_t i = 0; i < updated_objs.size(); ++i) update_(obj_aabb_map[updated_objs[i]]); updateVelist(); setup(); } //============================================================================== void SaPCollisionManager::update() { for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { update_(*it); } updateVelist(); setup(); } //============================================================================== void SaPCollisionManager::clear() { for (auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) { delete (*it)->hi; delete (*it)->lo; delete *it; *it = nullptr; } AABB_arr.clear(); overlap_pairs.clear(); elist[0] = nullptr; elist[1] = nullptr; elist[2] = nullptr; velist[0].clear(); velist[1].clear(); velist[2].clear(); obj_aabb_map.clear(); } //============================================================================== void SaPCollisionManager::getObjects( std::vector& objs) const { objs.resize(AABB_arr.size()); size_t i = 0; for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it, ++i) { objs[i] = (*it)->obj; } } //============================================================================== bool SaPCollisionManager::collide_(CollisionObject* obj, CollisionCallBackBase* callback) const { int axis = optimal_axis; const AABB& obj_aabb = obj->getAABB(); Scalar min_val = obj_aabb.min_[axis]; // Scalar max_val = obj_aabb.max_[axis]; EndPoint dummy; SaPAABB dummy_aabb; dummy_aabb.cached = obj_aabb; dummy.minmax = 1; dummy.aabb = &dummy_aabb; // compute stop_pos by binary search, this is cheaper than check it in while // iteration linearly const auto res_it = std::upper_bound( velist[axis].begin(), velist[axis].end(), &dummy, std::bind(std::less(), std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, axis), std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, axis))); EndPoint* end_pos = nullptr; if (res_it != velist[axis].end()) end_pos = *res_it; EndPoint* pos = elist[axis]; while (pos != end_pos) { if (pos->aabb->obj != obj) { if ((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) { if (pos->aabb->cached.overlap(obj->getAABB())) if ((*callback)(obj, pos->aabb->obj)) return true; } } pos = pos->next[axis]; } return false; } //============================================================================== void SaPCollisionManager::addToOverlapPairs(const SaPPair& p) { bool repeated = false; for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) { if (*it == p) { repeated = true; break; } } if (!repeated) overlap_pairs.push_back(p); } //============================================================================== void SaPCollisionManager::removeFromOverlapPairs(const SaPPair& p) { for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) { if (*it == p) { overlap_pairs.erase(it); break; } } // or overlap_pairs.remove_if(isNotValidPair(p)); } //============================================================================== void SaPCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SaPCollisionManager::collide(CollisionObject*, " "CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; collide_(obj, callback); } //============================================================================== bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const { Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); if (min_dist < (std::numeric_limits::max)()) { Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } int axis = optimal_axis; int status = 1; Scalar old_min_distance; EndPoint* start_pos = elist[axis]; while (1) { old_min_distance = min_dist; Scalar min_val = aabb.min_[axis]; // Scalar max_val = aabb.max_[axis]; EndPoint dummy; SaPAABB dummy_aabb; dummy_aabb.cached = aabb; dummy.minmax = 1; dummy.aabb = &dummy_aabb; const auto res_it = std::upper_bound( velist[axis].begin(), velist[axis].end(), &dummy, std::bind(std::less(), std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, axis), std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, axis))); EndPoint* end_pos = nullptr; if (res_it != velist[axis].end()) end_pos = *res_it; EndPoint* pos = start_pos; while (pos != end_pos) { // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and // then update start_pos to end_pos. but this seems slower. if ((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) { CollisionObject* curr_obj = pos->aabb->obj; if (curr_obj != obj) { if (!this->enable_tested_set_) { if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) { if ((*callback)(curr_obj, obj, min_dist)) return true; } } else { if (!this->inTestedSet(curr_obj, obj)) { if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) { if ((*callback)(curr_obj, obj, min_dist)) return true; } this->insertTestedSet(curr_obj, obj); } } } } pos = pos->next[axis]; } if (status == 1) { if (old_min_distance < (std::numeric_limits::max)()) break; else { if (min_dist < old_min_distance) { Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { if (aabb == obj->getAABB()) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } } else if (status == 0) break; } return false; } //============================================================================== void SaPCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SaPCollisionManager::distance(CollisionObject*, " "DistanceCallBackBase*)"); callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== void SaPCollisionManager::collide(CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SaPCollisionManager::collide(CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; for (auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end; ++it) { CollisionObject* obj1 = it->obj1; CollisionObject* obj2 = it->obj2; if ((*callback)(obj1, obj2)) return; } } //============================================================================== void SaPCollisionManager::distance(DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SaPCollisionManager::distance(DistanceCallBackBase*)"); callback->init(); if (size() == 0) return; this->enable_tested_set_ = true; this->tested_set.clear(); Scalar min_dist = (std::numeric_limits::max)(); for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { if (distance_((*it)->obj, callback, min_dist)) break; } this->enable_tested_set_ = false; this->tested_set.clear(); } //============================================================================== void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SaPCollisionManager::collide(BroadPhaseCollisionManager*, " "CollisionCallBackBase*)"); callback->init(); SaPCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { collide(callback); return; } if (this->size() < other_manager->size()) { for (auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it) { if (other_manager->collide_((*it)->obj, callback)) return; } } else { for (auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it) { if (collide_((*it)->obj, callback)) return; } } } //============================================================================== void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::SaPCollisionManager::distance(BroadPhaseCollisionManager*, " "DistanceCallBackBase*)"); callback->init(); SaPCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { distance(callback); return; } Scalar min_dist = (std::numeric_limits::max)(); if (this->size() < other_manager->size()) { for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { if (other_manager->distance_((*it)->obj, callback, min_dist)) return; } } else { for (auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it) { if (distance_((*it)->obj, callback, min_dist)) return; } } } //============================================================================== bool SaPCollisionManager::empty() const { return AABB_arr.empty(); } //============================================================================== size_t SaPCollisionManager::size() const { return AABB_arr.size(); } //============================================================================== const Vec3s& SaPCollisionManager::EndPoint::getVal() const { if (minmax) return aabb->cached.max_; else return aabb->cached.min_; } //============================================================================== Vec3s& SaPCollisionManager::EndPoint::getVal() { if (minmax) return aabb->cached.max_; else return aabb->cached.min_; } //============================================================================== Scalar SaPCollisionManager::EndPoint::getVal(int i) const { if (minmax) return aabb->cached.max_[i]; else return aabb->cached.min_[i]; } //============================================================================== Scalar& SaPCollisionManager::EndPoint::getVal(int i) { if (minmax) return aabb->cached.max_[i]; else return aabb->cached.min_[i]; } //============================================================================== SaPCollisionManager::SaPPair::SaPPair(CollisionObject* a, CollisionObject* b) { if (a < b) { obj1 = a; obj2 = b; } else { obj1 = b; obj2 = a; } } //============================================================================== bool SaPCollisionManager::SaPPair::operator==(const SaPPair& other) const { COAL_EQUAL_OPERATOR_CHECK(obj1 == other.obj1); COAL_EQUAL_OPERATOR_CHECK(obj2 == other.obj2); return true; } //============================================================================== SaPCollisionManager::isUnregistered::isUnregistered(CollisionObject* obj_) : obj(obj_) {} //============================================================================== bool SaPCollisionManager::isUnregistered::operator()( const SaPPair& pair) const { return (pair.obj1 == obj) || (pair.obj2 == obj); } //============================================================================== SaPCollisionManager::isNotValidPair::isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_), obj2(obj2_) { // Do nothing } //============================================================================== bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) { return (pair.obj1 == obj1) && (pair.obj2 == obj2); } } // namespace coal ================================================ FILE: src/broadphase/broadphase_bruteforce.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #include "coal/broadphase/broadphase_bruteforce.h" #include #include #include "coal/tracy.hh" namespace coal { //============================================================================== NaiveCollisionManager::NaiveCollisionManager() { // Do nothing } //============================================================================== void NaiveCollisionManager::registerObjects( const std::vector& other_objs) { std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs)); } //============================================================================== void NaiveCollisionManager::unregisterObject(CollisionObject* obj) { objs.remove(obj); } //============================================================================== void NaiveCollisionManager::registerObject(CollisionObject* obj) { objs.push_back(obj); } //============================================================================== void NaiveCollisionManager::setup() { // Do nothing } //============================================================================== void NaiveCollisionManager::update() { // Do nothing } //============================================================================== void NaiveCollisionManager::clear() { objs.clear(); } //============================================================================== void NaiveCollisionManager::getObjects( std::vector& objs_) const { objs_.resize(objs.size()); std::copy(objs.begin(), objs.end(), objs_.begin()); } //============================================================================== void NaiveCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::NaiveCollisionManager::collide(CollisionObject*, " "CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; for (auto* obj2 : objs) { if ((*callback)(obj, obj2)) return; } } //============================================================================== void NaiveCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::NaiveCollisionManager::distance(CollisionObject*, " "DistanceCallBackBase*)"); callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); for (auto* obj2 : objs) { if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { if ((*callback)(obj, obj2, min_dist)) return; } } } //============================================================================== void NaiveCollisionManager::collide(CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::NaiveCollisionManager::collide(CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; for (typename std::list::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) { typename std::list::const_iterator it2 = it1; it2++; for (; it2 != end; ++it2) { if ((*it1)->getAABB().overlap((*it2)->getAABB())) { if ((*callback)(*it1, *it2)) return; } } } } //============================================================================== void NaiveCollisionManager::distance(DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::NaiveCollisionManager::distance(CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); for (typename std::list::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) { typename std::list::const_iterator it2 = it1; it2++; for (; it2 != end; ++it2) { if ((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) { if ((*callback)(*it1, *it2, min_dist)) return; } } } } //============================================================================== void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::NaiveCollisionManager::collide(BroadPhaseCollisionManager*, " "CollisionCallBackBase*)"); callback->init(); NaiveCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { collide(callback); return; } for (auto* obj1 : objs) { for (auto* obj2 : other_manager->objs) { if (obj1->getAABB().overlap(obj2->getAABB())) { if ((*callback)(obj1, obj2)) return; } } } } //============================================================================== void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::NaiveCollisionManager::distance(BroadPhaseCollisionManager*, " "DistanceCallBackBase*)"); callback->init(); NaiveCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { distance(callback); return; } Scalar min_dist = (std::numeric_limits::max)(); for (auto* obj1 : objs) { for (auto* obj2 : other_manager->objs) { if (obj1->getAABB().distance(obj2->getAABB()) < min_dist) { if ((*callback)(obj1, obj2, min_dist)) return; } } } } //============================================================================== bool NaiveCollisionManager::empty() const { return objs.empty(); } //============================================================================== size_t NaiveCollisionManager::size() const { return objs.size(); } } // namespace coal ================================================ FILE: src/broadphase/broadphase_collision_manager.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #include "coal/broadphase/broadphase_collision_manager.h" namespace coal { namespace detail { struct CollisionCallBackFunctorWrapper : CollisionCallBackBase { CollisionCallBackFunctorWrapper(const CollisionCallBackFunctor& functor) : m_functor(&functor) {} void init() override {} bool collide(CollisionObject* o1, CollisionObject* o2) override { return (*m_functor)(o1, o2); } CollisionCallBackFunctor const* m_functor; }; struct DistanceCallBackFunctorWrapper : DistanceCallBackBase { DistanceCallBackFunctorWrapper(const DistanceCallBackFunctor& functor) : m_functor(&functor) {} void init() override {} bool distance(CollisionObject* o1, CollisionObject* o2, Scalar& dist) override { return (*m_functor)(o1, o2, dist); } DistanceCallBackFunctor const* m_functor; }; } // namespace detail //============================================================================== BroadPhaseCollisionManager::BroadPhaseCollisionManager() : enable_tested_set_(false) { // Do nothing } //============================================================================== BroadPhaseCollisionManager::~BroadPhaseCollisionManager() { // Do nothing } //============================================================================== void BroadPhaseCollisionManager::registerObjects( const std::vector& other_objs) { for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); } //============================================================================== void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) { COAL_UNUSED_VARIABLE(updated_obj); update(); } //============================================================================== void BroadPhaseCollisionManager::collide( CollisionObject* obj, const CollisionCallBackFunctor& fn) const { detail::CollisionCallBackFunctorWrapper wrapper{fn}; this->collide(obj, &wrapper); } //============================================================================== void BroadPhaseCollisionManager::collide( const CollisionCallBackFunctor& fn) const { detail::CollisionCallBackFunctorWrapper wrapper{fn}; this->collide(&wrapper); } //============================================================================== void BroadPhaseCollisionManager::collide( BroadPhaseCollisionManager* other_manager, const CollisionCallBackFunctor& fn) const { detail::CollisionCallBackFunctorWrapper wrapper{fn}; this->collide(other_manager, &wrapper); } //============================================================================== void BroadPhaseCollisionManager::distance( CollisionObject* obj, const DistanceCallBackFunctor& fn) const { detail::DistanceCallBackFunctorWrapper wrapper{fn}; this->distance(obj, &wrapper); } //============================================================================== void BroadPhaseCollisionManager::distance( const DistanceCallBackFunctor& fn) const { detail::DistanceCallBackFunctorWrapper wrapper{fn}; this->distance(&wrapper); } //============================================================================== void BroadPhaseCollisionManager::distance( BroadPhaseCollisionManager* other_manager, const DistanceCallBackFunctor& fn) const { detail::DistanceCallBackFunctorWrapper wrapper{fn}; this->distance(other_manager, &wrapper); } //============================================================================== void BroadPhaseCollisionManager::update( const std::vector& updated_objs) { COAL_UNUSED_VARIABLE(updated_objs); update(); } //============================================================================== bool BroadPhaseCollisionManager::inTestedSet(CollisionObject* a, CollisionObject* b) const { if (a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end(); else return tested_set.find(std::make_pair(b, a)) != tested_set.end(); } //============================================================================== void BroadPhaseCollisionManager::insertTestedSet(CollisionObject* a, CollisionObject* b) const { if (a < b) tested_set.insert(std::make_pair(a, b)); else tested_set.insert(std::make_pair(b, a)); } } // namespace coal ================================================ FILE: src/broadphase/broadphase_dynamic_AABB_tree.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include "coal/tracy.hh" #ifdef COAL_HAVE_OCTOMAP #include "coal/octree.h" #endif #include "coal/BV/BV.h" #include "coal/shape/geometric_shapes_utility.h" #include namespace coal { namespace detail { namespace dynamic_AABB_tree { #if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (!root2) { if (root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); if (!obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } } } else { if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, tf2, callback)) return true; if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, tf2, callback)) return true; } return false; } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = Scalar(root2->getOccupancy()); box->threshold_occupied = tree2->getOccupancyThres(); CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } else return false; } else return false; } OBB obb1, obb2; convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, callback)) return true; if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, callback)) return true; } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); if (collisionRecurse_(root1, tree2, child, child_bv, tf2, callback)) return true; } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); if (collisionRecurse_(root1, tree2, nullptr, child_bv, tf2, callback)) return true; } } } return false; } //============================================================================== bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3s& tf2, DistanceCallBackBase* callback, Scalar& min_dist) { if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); return (*callback)(static_cast(root1->data), &obj, min_dist); } else return false; } if (!tree2->isNodeOccupied(root2)) return false; if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { AABB aabb2; convertBV(root2_bv, tf2, aabb2); Scalar d1 = aabb2.distance(root1->children[0]->bv); Scalar d2 = aabb2.distance(root1->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, callback, min_dist)) return true; } } } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); AABB aabb2; convertBV(child_bv, tf2, aabb2); Scalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(root1, tree2, child, child_bv, tf2, callback, min_dist)) return true; } } } } return false; } //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (tf2.rotation().isIdentity()) return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), callback); else // has rotation return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, callback); } //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3s& tf2, DistanceCallBackBase* callback, Scalar& min_dist) { if (tf2.rotation().isIdentity()) return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), callback, min_dist); else return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, callback, min_dist); } #endif //============================================================================== bool leafCollide(CollisionObject* o1, CollisionObject* o2, CollisionCallBackBase* callback) { if ((o1->getNodeType() == GEOM_HALFSPACE || o1->getNodeType() == GEOM_PLANE) && (o2->getNodeType() == GEOM_HALFSPACE || o2->getNodeType() == GEOM_PLANE)) { // Halfspace-plane / Halfspace-plane collision, there is no need to check // AABBs. We can directly use the callback. return (*callback)(o1, o2); } bool overlap = false; if (o1->getNodeType() == GEOM_HALFSPACE || o1->getNodeType() == GEOM_PLANE) { if (o1->getNodeType() == GEOM_HALFSPACE) { const auto& halfspace = static_cast(*(o1->collisionGeometryPtr())); overlap = o2->getAABB().overlap(transform(halfspace, o1->getTransform())); } else { const auto& plane = static_cast(*(o1->collisionGeometryPtr())); overlap = o2->getAABB().overlap(transform(plane, o1->getTransform())); } } // else if (o2->getNodeType() == GEOM_HALFSPACE || o2->getNodeType() == GEOM_PLANE) { if (o2->getNodeType() == GEOM_HALFSPACE) { const auto& halfspace = static_cast(*(o2->collisionGeometryPtr())); overlap = o1->getAABB().overlap(transform(halfspace, o2->getTransform())); } else { const auto& plane = static_cast(*(o2->collisionGeometryPtr())); overlap = o1->getAABB().overlap(transform(plane, o2->getTransform())); } } // else { overlap = o1->getAABB().overlap(o2->getAABB()); } if (overlap) { return (*callback)(o1, o2); } return false; } //============================================================================== bool nodeCollide(DynamicAABBTreeCollisionManager::DynamicAABBNode* node1, DynamicAABBTreeCollisionManager::DynamicAABBNode* node2) { return node1->bv.overlap(node2->bv); } //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, CollisionCallBackBase* callback) { if (root1->isLeaf() && root2->isLeaf()) { CollisionObject* o1 = static_cast(root1->data); CollisionObject* o2 = static_cast(root2->data); return leafCollide(o1, o2, callback); } if (!nodeCollide(root1, root2)) { return false; } if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { if (collisionRecurse(root1->children[0], root2, callback)) return true; if (collisionRecurse(root1->children[1], root2, callback)) return true; } else { if (collisionRecurse(root1, root2->children[0], callback)) return true; if (collisionRecurse(root1, root2->children[1], callback)) return true; } return false; } //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, CollisionCallBackBase* callback) { if (root->isLeaf()) { CollisionObject* leaf = static_cast(root->data); return leafCollide(leaf, query, callback); } // Create a temporary node, attached to no tree. // This allows to reuse the `nodeCollide` function, which only checks for // overlap between the AABBs of two nodes. DynamicAABBTreeCollisionManager::DynamicAABBNode query_node; query_node.data = query; query_node.bv = query->getAABB(); query_node.parent = nullptr; query_node.children[1] = nullptr; if (!nodeCollide(root, &query_node)) { return false; } size_t select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1])); if (collisionRecurse(root->children[select_res], query, callback)) return true; if (collisionRecurse(root->children[1 - select_res], query, callback)) return true; return false; } //============================================================================== bool selfCollisionRecurse( DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionCallBackBase* callback) { if (root->isLeaf()) return false; if (selfCollisionRecurse(root->children[0], callback)) return true; if (selfCollisionRecurse(root->children[1], callback)) return true; if (collisionRecurse(root->children[0], root->children[1], callback)) return true; return false; } //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, DistanceCallBackBase* callback, Scalar& min_dist) { if (root1->isLeaf() && root2->isLeaf()) { CollisionObject* root1_obj = static_cast(root1->data); CollisionObject* root2_obj = static_cast(root2->data); return (*callback)(root1_obj, root2_obj, min_dist); } if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { Scalar d1 = root2->bv.distance(root1->children[0]->bv); Scalar d2 = root2->bv.distance(root1->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse(root1->children[1], root2, callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse(root1->children[0], root2, callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse(root1->children[0], root2, callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse(root1->children[1], root2, callback, min_dist)) return true; } } } else { Scalar d1 = root1->bv.distance(root2->children[0]->bv); Scalar d2 = root1->bv.distance(root2->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse(root1, root2->children[1], callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse(root1, root2->children[0], callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse(root1, root2->children[0], callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse(root1, root2->children[1], callback, min_dist)) return true; } } } return false; } //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, DistanceCallBackBase* callback, Scalar& min_dist) { if (root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return (*callback)(root_obj, query, min_dist); } Scalar d1 = query->getAABB().distance(root->children[0]->bv); Scalar d2 = query->getAABB().distance(root->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse(root->children[1], query, callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse(root->children[0], query, callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse(root->children[0], query, callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse(root->children[1], query, callback, min_dist)) return true; } } return false; } //============================================================================== bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, DistanceCallBackBase* callback, Scalar& min_dist) { if (root->isLeaf()) return false; if (selfDistanceRecurse(root->children[0], callback, min_dist)) return true; if (selfDistanceRecurse(root->children[1], callback, min_dist)) return true; if (distanceRecurse(root->children[0], root->children[1], callback, min_dist)) return true; return false; } } // namespace dynamic_AABB_tree } // namespace detail //============================================================================== DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager() { tree_topdown_balance_threshold = &dtree.bu_threshold; tree_topdown_level = &dtree.topdown_level; max_tree_nonbalanced_level = 10; tree_incremental_balance_pass = 10; *tree_topdown_balance_threshold = 2; *tree_topdown_level = 0; tree_init_level = 0; setup_ = false; // from experiment, this is the optimal setting octree_as_geometry_collide = true; octree_as_geometry_distance = false; } //============================================================================== void DynamicAABBTreeCollisionManager::registerObjects( const std::vector& other_objs) { if (other_objs.empty()) return; if (size() > 0) { BroadPhaseCollisionManager::registerObjects(other_objs); } else { std::vector leaves(other_objs.size()); table.rehash(other_objs.size()); for (size_t i = 0, size = other_objs.size(); i < size; ++i) { DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree node->bv = other_objs[i]->getAABB(); node->parent = nullptr; node->children[1] = nullptr; node->data = other_objs[i]; table[other_objs[i]] = node; leaves[i] = node; } dtree.init(leaves, tree_init_level); setup_ = true; } } //============================================================================== void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) { DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj); table[obj] = node; } //============================================================================== void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) { DynamicAABBNode* node = table[obj]; table.erase(obj); dtree.remove(node); } //============================================================================== void DynamicAABBTreeCollisionManager::setup() { if (!setup_) { size_t num = dtree.size(); if (num == 0) { setup_ = true; return; } size_t height = dtree.getMaxHeight(); if (((Scalar)height - std::log((Scalar)num) / std::log(2.0)) < max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else dtree.balanceTopdown(); setup_ = true; } } //============================================================================== void DynamicAABBTreeCollisionManager::update() { for (auto it = table.cbegin(); it != table.cend(); ++it) { CollisionObject* obj = it->first; DynamicAABBNode* node = it->second; node->bv = obj->getAABB(); if (node->bv.volume() < 0.) COAL_THROW_PRETTY("The bounding volume has a negative volume.", std::invalid_argument) } dtree.refit(); setup_ = false; setup(); } //============================================================================== void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) { const auto it = table.find(updated_obj); if (it != table.end()) { DynamicAABBNode* node = it->second; if (!(node->bv == updated_obj->getAABB())) dtree.update(node, updated_obj->getAABB()); } setup_ = false; } //============================================================================== void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) { update_(updated_obj); setup(); } //============================================================================== void DynamicAABBTreeCollisionManager::update( const std::vector& updated_objs) { for (size_t i = 0, size = updated_objs.size(); i < size; ++i) update_(updated_objs[i]); setup(); } //============================================================================== void DynamicAABBTreeCollisionManager::clear() { dtree.clear(); table.clear(); } //============================================================================== void DynamicAABBTreeCollisionManager::getObjects( std::vector& objs) const { objs.resize(this->size()); std::transform( table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } //============================================================================== void DynamicAABBTreeCollisionManager::collide( CollisionObject* obj, CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::DynamicAABBTreeCollisionManager::collide(CollisionObject*, " "CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; switch (obj->collisionGeometry()->getNodeType()) { #if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_collide) { const OcTree* octree = static_cast(obj->collisionGeometryPtr()); detail::dynamic_AABB_tree::collisionRecurse( dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), callback); } else detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, callback); } break; #endif default: detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, callback); } } //============================================================================== void DynamicAABBTreeCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::DynamicAABBTreeCollisionManager::distance(CollisionObject*, " "DistanceCallBackBase*)"); callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); switch (obj->collisionGeometry()->getNodeType()) { #if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_distance) { const OcTree* octree = static_cast(obj->collisionGeometryPtr()); detail::dynamic_AABB_tree::distanceRecurse( dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), callback, min_dist); } else detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, callback, min_dist); } break; #endif default: detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, callback, min_dist); } } //============================================================================== void DynamicAABBTreeCollisionManager::collide( CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::DynamicAABBTreeCollisionManager::collide(CollisionCallBackBase*)"); callback->init(); if (size() == 0) return; detail::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), callback); } //============================================================================== void DynamicAABBTreeCollisionManager::distance( DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::DynamicAABBTreeCollisionManager::distance(DistanceCallBackBase*)"); callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback, min_dist); } //============================================================================== void DynamicAABBTreeCollisionManager::collide( BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::DynamicAABBTreeCollisionManager::collide(" "BroadPhaseCollisionManager*, CollisionCallBackBase*)"); callback->init(); DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; detail::dynamic_AABB_tree::collisionRecurse( dtree.getRoot(), other_manager->dtree.getRoot(), callback); } //============================================================================== void DynamicAABBTreeCollisionManager::distance( BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const { COAL_TRACY_ZONE_SCOPED_N( "coal::DynamicAABBTreeCollisionManager::distance(" "BroadPhaseCollisionManager*, DistanceCallBackBase*)"); callback->init(); DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; Scalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree::distanceRecurse( dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist); } //============================================================================== bool DynamicAABBTreeCollisionManager::empty() const { return dtree.empty(); } //============================================================================== size_t DynamicAABBTreeCollisionManager::size() const { return dtree.size(); } //============================================================================== const detail::HierarchyTree& DynamicAABBTreeCollisionManager::getTree() const { return dtree; } detail::HierarchyTree& DynamicAABBTreeCollisionManager::getTree() { return dtree; } } // namespace coal ================================================ FILE: src/broadphase/broadphase_dynamic_AABB_tree_array.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #ifdef COAL_HAVE_OCTOMAP #include "coal/octree.h" #endif namespace coal { namespace detail { namespace dynamic_AABB_tree_array { #if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; if (!root2) { if (root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); if (!obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } } } else { if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, root2_bv, tf2, callback)) return true; if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, root2_bv, tf2, callback)) return true; } return false; } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = Scalar(root2->getOccupancy()); box->threshold_occupied = tree2->getOccupancyThres(); CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } else return false; } else return false; } OBB obb1, obb2; convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, callback)) return true; if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, callback)) return true; } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, callback)) return true; } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, tf2, callback)) return true; } } } return false; } //============================================================================== bool distanceRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3s& tf2, DistanceCallBackBase* callback, Scalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); return (*callback)(static_cast(root1->data), &obj, min_dist); } else return false; } if (!tree2->isNodeOccupied(root2)) return false; if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { AABB aabb2; convertBV(root2_bv, tf2, aabb2); Scalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv); Scalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, callback, min_dist)) return true; } } } else { for (unsigned int i = 0; i < 8; ++i) { if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); AABB aabb2; convertBV(child_bv, tf2, aabb2); Scalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, callback, min_dist)) return true; } } } } return false; } #endif //============================================================================== bool collisionRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, size_t root2_id, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = nodes2 + root2_id; if (root1->isLeaf() && root2->isLeaf()) { if (!root1->bv.overlap(root2->bv)) return false; return (*callback)(static_cast(root1->data), static_cast(root2->data)); } if (!root1->bv.overlap(root2->bv)) return false; if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { if (collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, callback)) return true; if (collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, callback)) return true; } else { if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], callback)) return true; if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], callback)) return true; } return false; } //============================================================================== inline COAL_DLLAPI bool collisionRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; if (!root->bv.overlap(query->getAABB())) return false; if (root->isLeaf()) { return (*callback)(static_cast(root->data), query); } size_t select_res = implementation_array::select( query->getAABB(), root->children[0], root->children[1], nodes); if (collisionRecurse(nodes, root->children[select_res], query, callback)) return true; if (collisionRecurse(nodes, root->children[1 - select_res], query, callback)) return true; return false; } //============================================================================== bool selfCollisionRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; if (root->isLeaf()) return false; if (selfCollisionRecurse(nodes, root->children[0], callback)) return true; if (selfCollisionRecurse(nodes, root->children[1], callback)) return true; if (collisionRecurse(nodes, root->children[0], nodes, root->children[1], callback)) return true; return false; } //============================================================================== bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, size_t root2_id, DistanceCallBackBase* callback, Scalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = nodes2 + root2_id; if (root1->isLeaf() && root2->isLeaf()) { CollisionObject* root1_obj = static_cast(root1->data); CollisionObject* root2_obj = static_cast(root2->data); return (*callback)(root1_obj, root2_obj, min_dist); } if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { Scalar d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); Scalar d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, callback, min_dist)) return true; } } } else { Scalar d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); Scalar d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], callback, min_dist)) return true; } } } return false; } //============================================================================== bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, DistanceCallBackBase* callback, Scalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; if (root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return (*callback)(root_obj, query, min_dist); } Scalar d1 = query->getAABB().distance((nodes + root->children[0])->bv); Scalar d2 = query->getAABB().distance((nodes + root->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { if (distanceRecurse(nodes, root->children[1], query, callback, min_dist)) return true; } if (d1 < min_dist) { if (distanceRecurse(nodes, root->children[0], query, callback, min_dist)) return true; } } else { if (d1 < min_dist) { if (distanceRecurse(nodes, root->children[0], query, callback, min_dist)) return true; } if (d2 < min_dist) { if (distanceRecurse(nodes, root->children[1], query, callback, min_dist)) return true; } } return false; } //============================================================================== bool selfDistanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, DistanceCallBackBase* callback, Scalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; if (root->isLeaf()) return false; if (selfDistanceRecurse(nodes, root->children[0], callback, min_dist)) return true; if (selfDistanceRecurse(nodes, root->children[1], callback, min_dist)) return true; if (distanceRecurse(nodes, root->children[0], nodes, root->children[1], callback, min_dist)) return true; return false; } #if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (tf2.rotation().isIdentity()) return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), callback); else return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, callback); } //============================================================================== bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3s& tf2, DistanceCallBackBase* callback, Scalar& min_dist) { if (tf2.rotation().isIdentity()) return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), callback, min_dist); else return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, callback, min_dist); } #endif } // namespace dynamic_AABB_tree_array } // namespace detail //============================================================================== DynamicAABBTreeArrayCollisionManager::DynamicAABBTreeArrayCollisionManager() { tree_topdown_balance_threshold = &dtree.bu_threshold; tree_topdown_level = &dtree.topdown_level; max_tree_nonbalanced_level = 10; tree_incremental_balance_pass = 10; *tree_topdown_balance_threshold = 2; *tree_topdown_level = 0; tree_init_level = 0; setup_ = false; // from experiment, this is the optimal setting octree_as_geometry_collide = true; octree_as_geometry_distance = false; } //============================================================================== void DynamicAABBTreeArrayCollisionManager::registerObjects( const std::vector& other_objs) { if (other_objs.empty()) return; if (size() > 0) { BroadPhaseCollisionManager::registerObjects(other_objs); } else { DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()]; table.rehash(other_objs.size()); for (size_t i = 0, size = other_objs.size(); i < size; ++i) { leaves[i].bv = other_objs[i]->getAABB(); leaves[i].parent = dtree.NULL_NODE; leaves[i].children[1] = dtree.NULL_NODE; leaves[i].data = other_objs[i]; table[other_objs[i]] = i; } int n_leaves = (int)other_objs.size(); dtree.init(leaves, n_leaves, tree_init_level); setup_ = true; } } //============================================================================== void DynamicAABBTreeArrayCollisionManager::registerObject( CollisionObject* obj) { size_t node = dtree.insert(obj->getAABB(), obj); table[obj] = node; } //============================================================================== void DynamicAABBTreeArrayCollisionManager::unregisterObject( CollisionObject* obj) { size_t node = table[obj]; table.erase(obj); dtree.remove(node); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::setup() { if (!setup_) { int num = (int)dtree.size(); if (num == 0) { setup_ = true; return; } int height = (int)dtree.getMaxHeight(); if ((Scalar)height - std::log((Scalar)num) / std::log(2.0) < max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else dtree.balanceTopdown(); setup_ = true; } } //============================================================================== void DynamicAABBTreeArrayCollisionManager::update() { for (auto it = table.cbegin(), end = table.cend(); it != end; ++it) { const CollisionObject* obj = it->first; size_t node = it->second; dtree.getNodes()[node].bv = obj->getAABB(); } dtree.refit(); setup_ = false; setup(); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::update_( CollisionObject* updated_obj) { const auto it = table.find(updated_obj); if (it != table.end()) { size_t node = it->second; if (!(dtree.getNodes()[node].bv == updated_obj->getAABB())) dtree.update(node, updated_obj->getAABB()); } setup_ = false; } //============================================================================== void DynamicAABBTreeArrayCollisionManager::update( CollisionObject* updated_obj) { update_(updated_obj); setup(); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::update( const std::vector& updated_objs) { for (size_t i = 0, size = updated_objs.size(); i < size; ++i) update_(updated_objs[i]); setup(); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::clear() { dtree.clear(); table.clear(); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::getObjects( std::vector& objs) const { objs.resize(this->size()); std::transform( table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::collide( CollisionObject* obj, CollisionCallBackBase* callback) const { callback->init(); if (size() == 0) return; switch (obj->collisionGeometry()->getNodeType()) { #if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_collide) { const OcTree* octree = static_cast(obj->collisionGeometryPtr()); detail::dynamic_AABB_tree_array::collisionRecurse( dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), callback); } else detail::dynamic_AABB_tree_array::collisionRecurse( dtree.getNodes(), dtree.getRoot(), obj, callback); } break; #endif default: detail::dynamic_AABB_tree_array::collisionRecurse( dtree.getNodes(), dtree.getRoot(), obj, callback); } } //============================================================================== void DynamicAABBTreeArrayCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); switch (obj->collisionGeometry()->getNodeType()) { #if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_distance) { const OcTree* octree = static_cast(obj->collisionGeometryPtr()); detail::dynamic_AABB_tree_array::distanceRecurse( dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), callback, min_dist); } else detail::dynamic_AABB_tree_array::distanceRecurse( dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist); } break; #endif default: detail::dynamic_AABB_tree_array::distanceRecurse( dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist); } } //============================================================================== void DynamicAABBTreeArrayCollisionManager::collide( CollisionCallBackBase* callback) const { callback->init(); if (size() == 0) return; detail::dynamic_AABB_tree_array::selfCollisionRecurse( dtree.getNodes(), dtree.getRoot(), callback); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::distance( DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree_array::selfDistanceRecurse( dtree.getNodes(), dtree.getRoot(), callback, min_dist); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::collide( BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const { callback->init(); DynamicAABBTreeArrayCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; detail::dynamic_AABB_tree_array::collisionRecurse( dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), callback); } //============================================================================== void DynamicAABBTreeArrayCollisionManager::distance( BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const { callback->init(); DynamicAABBTreeArrayCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; Scalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree_array::distanceRecurse( dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), callback, min_dist); } //============================================================================== bool DynamicAABBTreeArrayCollisionManager::empty() const { return dtree.empty(); } //============================================================================== size_t DynamicAABBTreeArrayCollisionManager::size() const { return dtree.size(); } //============================================================================== const detail::implementation_array::HierarchyTree& DynamicAABBTreeArrayCollisionManager::getTree() const { return dtree; } } // namespace coal ================================================ FILE: src/broadphase/broadphase_interval_tree.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #include "coal/broadphase/broadphase_interval_tree.h" namespace coal { //============================================================================== void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) { // must sorted before setup(); EndPoint p; p.value = obj->getAABB().min_[0]; auto start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p); p.value = obj->getAABB().max_[0]; auto end1 = std::upper_bound(start1, endpoints[0].end(), p); if (start1 < end1) { size_t start_id = (size_t)(start1 - endpoints[0].begin()); size_t end_id = (size_t)(end1 - endpoints[0].begin()); size_t cur_id = (size_t)(start_id); for (size_t i = start_id; i < end_id; ++i) { if (endpoints[0][(size_t)i].obj != obj) { if (i == cur_id) cur_id++; else { endpoints[0][(size_t)cur_id] = endpoints[0][(size_t)i]; cur_id++; } } } if (cur_id < end_id) endpoints[0].resize(endpoints[0].size() - 2); } p.value = obj->getAABB().min_[1]; auto start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p); p.value = obj->getAABB().max_[1]; auto end2 = std::upper_bound(start2, endpoints[1].end(), p); if (start2 < end2) { size_t start_id = (size_t)(start2 - endpoints[1].begin()); size_t end_id = (size_t)(end2 - endpoints[1].begin()); size_t cur_id = (size_t)(start_id); for (size_t i = start_id; i < end_id; ++i) { if (endpoints[1][i].obj != obj) { if (i == cur_id) cur_id++; else { endpoints[1][cur_id] = endpoints[1][i]; cur_id++; } } } if (cur_id < end_id) endpoints[1].resize(endpoints[1].size() - 2); } p.value = obj->getAABB().min_[2]; auto start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p); p.value = obj->getAABB().max_[2]; auto end3 = std::upper_bound(start3, endpoints[2].end(), p); if (start3 < end3) { size_t start_id = (size_t)(start3 - endpoints[2].begin()); size_t end_id = (size_t)(end3 - endpoints[2].begin()); size_t cur_id = (size_t)(start_id); for (size_t i = start_id; i < end_id; ++i) { if (endpoints[2][i].obj != obj) { if (i == cur_id) cur_id++; else { endpoints[2][cur_id] = endpoints[2][i]; cur_id++; } } } if (cur_id < end_id) endpoints[2].resize(endpoints[2].size() - 2); } // update the interval tree if (obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) { SAPInterval* ivl1 = obj_interval_maps[0][obj]; SAPInterval* ivl2 = obj_interval_maps[1][obj]; SAPInterval* ivl3 = obj_interval_maps[2][obj]; interval_trees[0]->deleteNode(ivl1); interval_trees[1]->deleteNode(ivl2); interval_trees[2]->deleteNode(ivl3); delete ivl1; delete ivl2; delete ivl3; obj_interval_maps[0].erase(obj); obj_interval_maps[1].erase(obj); obj_interval_maps[2].erase(obj); } } //============================================================================== IntervalTreeCollisionManager::IntervalTreeCollisionManager() : setup_(false) { for (int i = 0; i < 3; ++i) interval_trees[i] = nullptr; } //============================================================================== IntervalTreeCollisionManager::~IntervalTreeCollisionManager() { clear(); } //============================================================================== void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) { EndPoint p, q; p.obj = obj; q.obj = obj; p.minmax = 0; q.minmax = 1; p.value = obj->getAABB().min_[0]; q.value = obj->getAABB().max_[0]; endpoints[0].push_back(p); endpoints[0].push_back(q); p.value = obj->getAABB().min_[1]; q.value = obj->getAABB().max_[1]; endpoints[1].push_back(p); endpoints[1].push_back(q); p.value = obj->getAABB().min_[2]; q.value = obj->getAABB().max_[2]; endpoints[2].push_back(p); endpoints[2].push_back(q); setup_ = false; } //============================================================================== void IntervalTreeCollisionManager::setup() { if (!setup_) { std::sort(endpoints[0].begin(), endpoints[0].end()); std::sort(endpoints[1].begin(), endpoints[1].end()); std::sort(endpoints[2].begin(), endpoints[2].end()); for (int i = 0; i < 3; ++i) delete interval_trees[i]; for (int i = 0; i < 3; ++i) interval_trees[i] = new detail::IntervalTree; for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { EndPoint p = endpoints[0][i]; CollisionObject* obj = p.obj; if (p.minmax == 0) { SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj); SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj); SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj); interval_trees[0]->insert(ivl1); interval_trees[1]->insert(ivl2); interval_trees[2]->insert(ivl3); obj_interval_maps[0][obj] = ivl1; obj_interval_maps[1][obj] = ivl2; obj_interval_maps[2][obj] = ivl3; } } setup_ = true; } } //============================================================================== void IntervalTreeCollisionManager::update() { setup_ = false; for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { if (endpoints[0][i].minmax == 0) endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0]; else endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0]; } for (size_t i = 0, size = endpoints[1].size(); i < size; ++i) { if (endpoints[1][i].minmax == 0) endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1]; else endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1]; } for (size_t i = 0, size = endpoints[2].size(); i < size; ++i) { if (endpoints[2][i].minmax == 0) endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2]; else endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2]; } setup(); } //============================================================================== void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) { AABB old_aabb; const AABB& new_aabb = updated_obj->getAABB(); for (int i = 0; i < 3; ++i) { const auto it = obj_interval_maps[i].find(updated_obj); interval_trees[i]->deleteNode(it->second); old_aabb.min_[i] = it->second->low; old_aabb.max_[i] = it->second->high; it->second->low = new_aabb.min_[i]; it->second->high = new_aabb.max_[i]; interval_trees[i]->insert(it->second); } EndPoint dummy; typename std::vector::iterator it; for (int i = 0; i < 3; ++i) { dummy.value = old_aabb.min_[i]; it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); for (; it != endpoints[i].end(); ++it) { if (it->obj == updated_obj && it->minmax == 0) { it->value = new_aabb.min_[i]; break; } } dummy.value = old_aabb.max_[i]; it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); for (; it != endpoints[i].end(); ++it) { if (it->obj == updated_obj && it->minmax == 0) { it->value = new_aabb.max_[i]; break; } } std::sort(endpoints[i].begin(), endpoints[i].end()); } } //============================================================================== void IntervalTreeCollisionManager::update( const std::vector& updated_objs) { for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); } //============================================================================== void IntervalTreeCollisionManager::clear() { endpoints[0].clear(); endpoints[1].clear(); endpoints[2].clear(); delete interval_trees[0]; interval_trees[0] = nullptr; delete interval_trees[1]; interval_trees[1] = nullptr; delete interval_trees[2]; interval_trees[2] = nullptr; for (int i = 0; i < 3; ++i) { for (auto it = obj_interval_maps[i].cbegin(), end = obj_interval_maps[i].cend(); it != end; ++it) { delete it->second; } } for (int i = 0; i < 3; ++i) obj_interval_maps[i].clear(); setup_ = false; } //============================================================================== void IntervalTreeCollisionManager::getObjects( std::vector& objs) const { objs.resize(endpoints[0].size() / 2); size_t j = 0; for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { if (endpoints[0][i].minmax == 0) { objs[j] = endpoints[0][i].obj; j++; } } } //============================================================================== void IntervalTreeCollisionManager::collide( CollisionObject* obj, CollisionCallBackBase* callback) const { callback->init(); if (size() == 0) return; collide_(obj, callback); } //============================================================================== bool IntervalTreeCollisionManager::collide_( CollisionObject* obj, CollisionCallBackBase* callback) const { static const unsigned int CUTOFF = 100; std::deque results0, results1, results2; results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]); if (results0.size() > CUTOFF) { results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]); if (results1.size() > CUTOFF) { results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]); if (results2.size() > CUTOFF) { size_t d1 = results0.size(); size_t d2 = results1.size(); size_t d3 = results2.size(); if (d1 >= d2 && d1 >= d3) return checkColl(results0.begin(), results0.end(), obj, callback); else if (d2 >= d1 && d2 >= d3) return checkColl(results1.begin(), results1.end(), obj, callback); else return checkColl(results2.begin(), results2.end(), obj, callback); } else return checkColl(results2.begin(), results2.end(), obj, callback); } else return checkColl(results1.begin(), results1.end(), obj, callback); } else return checkColl(results0.begin(), results0.end(), obj, callback); } //============================================================================== void IntervalTreeCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; Scalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const { static const unsigned int CUTOFF = 100; Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); if (min_dist < (std::numeric_limits::max)()) { Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } int status = 1; Scalar old_min_distance; while (1) { bool dist_res = false; old_min_distance = min_dist; std::deque results0, results1, results2; results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]); if (results0.size() > CUTOFF) { results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]); if (results1.size() > CUTOFF) { results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]); if (results2.size() > CUTOFF) { size_t d1 = results0.size(); size_t d2 = results1.size(); size_t d3 = results2.size(); if (d1 >= d2 && d1 >= d3) dist_res = checkDist(results0.begin(), results0.end(), obj, callback, min_dist); else if (d2 >= d1 && d2 >= d3) dist_res = checkDist(results1.begin(), results1.end(), obj, callback, min_dist); else dist_res = checkDist(results2.begin(), results2.end(), obj, callback, min_dist); } else dist_res = checkDist(results2.begin(), results2.end(), obj, callback, min_dist); } else dist_res = checkDist(results1.begin(), results1.end(), obj, callback, min_dist); } else dist_res = checkDist(results0.begin(), results0.end(), obj, callback, min_dist); if (dist_res) return true; results0.clear(); results1.clear(); results2.clear(); if (status == 1) { if (old_min_distance < (std::numeric_limits::max)()) break; else { if (min_dist < old_min_distance) { Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { if (aabb == obj->getAABB()) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } } else if (status == 0) break; } return false; } //============================================================================== void IntervalTreeCollisionManager::collide( CollisionCallBackBase* callback) const { callback->init(); if (size() == 0) return; std::set active; std::set > overlap; size_t n = endpoints[0].size(); Scalar diff_x = endpoints[0][0].value - endpoints[0][n - 1].value; Scalar diff_y = endpoints[1][0].value - endpoints[1][n - 1].value; Scalar diff_z = endpoints[2][0].value - endpoints[2][n - 1].value; int axis = 0; if (diff_y > diff_x && diff_y > diff_z) axis = 1; else if (diff_z > diff_y && diff_z > diff_x) axis = 2; for (unsigned int i = 0; i < n; ++i) { const EndPoint& endpoint = endpoints[axis][i]; CollisionObject* index = endpoint.obj; if (endpoint.minmax == 0) { auto iter = active.begin(); auto end = active.end(); for (; iter != end; ++iter) { CollisionObject* active_index = *iter; const AABB& b0 = active_index->getAABB(); const AABB& b1 = index->getAABB(); int axis2 = (axis + 1) % 3; int axis3 = (axis + 2) % 3; if (b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) { std::pair >::iterator, bool> insert_res; if (active_index < index) insert_res = overlap.insert(std::make_pair(active_index, index)); else insert_res = overlap.insert(std::make_pair(index, active_index)); if (insert_res.second) { if ((*callback)(active_index, index)) return; } } } active.insert(index); } else active.erase(index); } } //============================================================================== void IntervalTreeCollisionManager::distance( DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; this->enable_tested_set_ = true; this->tested_set.clear(); Scalar min_dist = (std::numeric_limits::max)(); for (size_t i = 0; i < endpoints[0].size(); ++i) if (distance_(endpoints[0][i].obj, callback, min_dist)) break; this->enable_tested_set_ = false; this->tested_set.clear(); } //============================================================================== void IntervalTreeCollisionManager::collide( BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase* callback) const { callback->init(); IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { collide(callback); return; } if (this->size() < other_manager->size()) { for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) if (other_manager->collide_(endpoints[0][i].obj, callback)) return; } else { for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) if (collide_(other_manager->endpoints[0][i].obj, callback)) return; } } //============================================================================== void IntervalTreeCollisionManager::distance( BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase* callback) const { callback->init(); IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; if (this == other_manager) { distance(callback); return; } Scalar min_dist = (std::numeric_limits::max)(); if (this->size() < other_manager->size()) { for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) if (other_manager->distance_(endpoints[0][i].obj, callback, min_dist)) return; } else { for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) if (distance_(other_manager->endpoints[0][i].obj, callback, min_dist)) return; } } //============================================================================== bool IntervalTreeCollisionManager::empty() const { return endpoints[0].empty(); } //============================================================================== size_t IntervalTreeCollisionManager::size() const { return endpoints[0].size() / 2; } //============================================================================== bool IntervalTreeCollisionManager::checkColl( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, CollisionObject* obj, CollisionCallBackBase* callback) const { while (pos_start < pos_end) { SAPInterval* ivl = static_cast(*pos_start); if (ivl->obj != obj) { if (ivl->obj->getAABB().overlap(obj->getAABB())) { if ((*callback)(ivl->obj, obj)) return true; } } pos_start++; } return false; } //============================================================================== bool IntervalTreeCollisionManager::checkDist( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, Scalar& min_dist) const { while (pos_start < pos_end) { SAPInterval* ivl = static_cast(*pos_start); if (ivl->obj != obj) { if (!this->enable_tested_set_) { if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) { if ((*callback)(ivl->obj, obj, min_dist)) return true; } } else { if (!this->inTestedSet(ivl->obj, obj)) { if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) { if ((*callback)(ivl->obj, obj, min_dist)) return true; } this->insertTestedSet(ivl->obj, obj); } } } pos_start++; } return false; } //============================================================================== bool IntervalTreeCollisionManager::EndPoint::operator<( const typename IntervalTreeCollisionManager::EndPoint& p) const { return value < p.value; } //============================================================================== IntervalTreeCollisionManager::SAPInterval::SAPInterval(Scalar low_, Scalar high_, CollisionObject* obj_) : detail::SimpleInterval() { this->low = low_; this->high = high_; obj = obj_; } } // namespace coal ================================================ FILE: src/broadphase/default_broadphase_callbacks.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2020, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Sean Curtis (sean@tri.global) */ #include "coal/broadphase/default_broadphase_callbacks.h" #include namespace coal { bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* data) { assert(data != nullptr); auto* collision_data = static_cast(data); const CollisionRequest& request = collision_data->request; CollisionResult& result = collision_data->result; if (collision_data->done) return true; collide(o1, o2, request, result); if (result.isCollision() && result.numContacts() >= request.num_max_contacts) { collision_data->done = true; } return collision_data->done; } bool CollisionCallBackDefault::collide(CollisionObject* o1, CollisionObject* o2) { return defaultCollisionFunction(o1, o2, &data); } bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* data, Scalar& dist) { assert(data != nullptr); auto* cdata = static_cast(data); const DistanceRequest& request = cdata->request; DistanceResult& result = cdata->result; if (cdata->done) { dist = result.min_distance; return true; } distance(o1, o2, request, result); dist = result.min_distance; if (dist <= 0) return true; // in collision or in touch return cdata->done; } bool DistanceCallBackDefault::distance(CollisionObject* o1, CollisionObject* o2, Scalar& dist) { return defaultDistanceFunction(o1, o2, &data, dist); } CollisionCallBackCollect::CollisionCallBackCollect(const size_t max_size) : max_size(max_size) { collision_pairs.resize(max_size); } bool CollisionCallBackCollect::collide(CollisionObject* o1, CollisionObject* o2) { collision_pairs.push_back(std::make_pair(o1, o2)); return false; } size_t CollisionCallBackCollect::numCollisionPairs() const { return collision_pairs.size(); } const std::vector& CollisionCallBackCollect::getCollisionPairs() const { return collision_pairs; } void CollisionCallBackCollect::init() { collision_pairs.clear(); } bool CollisionCallBackCollect::exist(CollisionObject* o1, CollisionObject* o2) const { return exist(std::make_pair(o1, o2)); } bool CollisionCallBackCollect::exist(const CollisionPair& pair) const { return std::find(collision_pairs.begin(), collision_pairs.end(), pair) != collision_pairs.end(); } } // namespace coal ================================================ FILE: src/broadphase/detail/interval_tree.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_INTERVAL_TREE_INL_H #define COAL_INTERVAL_TREE_INL_H #include "coal/broadphase/detail/interval_tree.h" #include namespace coal { namespace detail { //============================================================================== IntervalTree::IntervalTree() { invalid_node = new IntervalTreeNode; invalid_node->left = invalid_node->right = invalid_node->parent = invalid_node; invalid_node->red = false; invalid_node->key = invalid_node->high = invalid_node->max_high = -(std::numeric_limits::max)(); invalid_node->stored_interval = nullptr; root = new IntervalTreeNode; root->parent = root->left = root->right = invalid_node; root->key = root->high = root->max_high = (std::numeric_limits::max)(); root->red = false; root->stored_interval = nullptr; /// the following are used for the query function recursion_node_stack_size = 128; recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size * sizeof(it_recursion_node)); recursion_node_stack_top = 1; recursion_node_stack[0].start_node = nullptr; } //============================================================================== IntervalTree::~IntervalTree() { IntervalTreeNode* x = root->left; std::deque nodes_to_free; if (x != invalid_node) { if (x->left != invalid_node) { nodes_to_free.push_back(x->left); } if (x->right != invalid_node) { nodes_to_free.push_back(x->right); } delete x; while (nodes_to_free.size() > 0) { x = nodes_to_free.back(); nodes_to_free.pop_back(); if (x->left != invalid_node) { nodes_to_free.push_back(x->left); } if (x->right != invalid_node) { nodes_to_free.push_back(x->right); } delete x; } } delete invalid_node; delete root; free(recursion_node_stack); } //============================================================================== void IntervalTree::leftRotate(IntervalTreeNode* x) { IntervalTreeNode* y; y = x->right; x->right = y->left; if (y->left != invalid_node) y->left->parent = x; y->parent = x->parent; if (x == x->parent->left) x->parent->left = y; else x->parent->right = y; y->left = x; x->parent = y; x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high)); y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high)); } //============================================================================== void IntervalTree::rightRotate(IntervalTreeNode* y) { IntervalTreeNode* x; x = y->left; y->left = x->right; if (invalid_node != x->right) x->right->parent = y; x->parent = y->parent; if (y == y->parent->left) y->parent->left = x; else y->parent->right = x; x->right = y; y->parent = x; y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high)); x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high)); } //============================================================================== void IntervalTree::recursiveInsert(IntervalTreeNode* z) { IntervalTreeNode* x; IntervalTreeNode* y; z->left = z->right = invalid_node; y = root; x = root->left; while (x != invalid_node) { y = x; if (x->key > z->key) x = x->left; else x = x->right; } z->parent = y; if ((y == root) || (y->key > z->key)) y->left = z; else y->right = z; } //============================================================================== void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) { while (x != root) { x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high)); x = x->parent; } } //============================================================================== IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) { IntervalTreeNode* y; IntervalTreeNode* x; IntervalTreeNode* new_node; x = new IntervalTreeNode(new_interval); recursiveInsert(x); fixupMaxHigh(x->parent); new_node = x; x->red = true; while (x->parent->red) { /// use sentinel instead of checking for root if (x->parent == x->parent->parent->left) { y = x->parent->parent->right; if (y->red) { x->parent->red = true; y->red = true; x->parent->parent->red = true; x = x->parent->parent; } else { if (x == x->parent->right) { x = x->parent; leftRotate(x); } x->parent->red = false; x->parent->parent->red = true; rightRotate(x->parent->parent); } } else { y = x->parent->parent->left; if (y->red) { x->parent->red = false; y->red = false; x->parent->parent->red = true; x = x->parent->parent; } else { if (x == x->parent->left) { x = x->parent; rightRotate(x); } x->parent->red = false; x->parent->parent->red = true; leftRotate(x->parent->parent); } } } root->left->red = false; return new_node; } //============================================================================== IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const { IntervalTreeNode* y; if (invalid_node != (y = x->right)) { while (y->left != invalid_node) y = y->left; return y; } else { y = x->parent; while (x == y->right) { x = y; y = y->parent; } if (y == root) return invalid_node; return y; } } //============================================================================== IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const { IntervalTreeNode* y; if (invalid_node != (y = x->left)) { while (y->right != invalid_node) y = y->right; return y; } else { y = x->parent; while (x == y->left) { if (y == root) return invalid_node; x = y; y = y->parent; } return y; } } //============================================================================== void IntervalTree::recursivePrint(IntervalTreeNode* x) const { if (x != invalid_node) { recursivePrint(x->left); x->print(invalid_node, root); recursivePrint(x->right); } } //============================================================================== void IntervalTree::print() const { recursivePrint(root->left); } //============================================================================== void IntervalTree::deleteFixup(IntervalTreeNode* x) { IntervalTreeNode* w; IntervalTreeNode* root_left_node = root->left; while ((!x->red) && (root_left_node != x)) { if (x == x->parent->left) { w = x->parent->right; if (w->red) { w->red = false; x->parent->red = true; leftRotate(x->parent); w = x->parent->right; } if ((!w->right->red) && (!w->left->red)) { w->red = true; x = x->parent; } else { if (!w->right->red) { w->left->red = false; w->red = true; rightRotate(w); w = x->parent->right; } w->red = x->parent->red; x->parent->red = false; w->right->red = false; leftRotate(x->parent); x = root_left_node; } } else { w = x->parent->left; if (w->red) { w->red = false; x->parent->red = true; rightRotate(x->parent); w = x->parent->left; } if ((!w->right->red) && (!w->left->red)) { w->red = true; x = x->parent; } else { if (!w->left->red) { w->right->red = false; w->red = true; leftRotate(w); w = x->parent->left; } w->red = x->parent->red; x->parent->red = false; w->left->red = false; rightRotate(x->parent); x = root_left_node; } } } x->red = false; } //============================================================================== void IntervalTree::deleteNode(SimpleInterval* ivl) { IntervalTreeNode* node = recursiveSearch(root, ivl); if (node) deleteNode(node); } //============================================================================== IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const { if (node != invalid_node) { if (node->stored_interval == ivl) return node; IntervalTreeNode* left = recursiveSearch(node->left, ivl); if (left != invalid_node) return left; IntervalTreeNode* right = recursiveSearch(node->right, ivl); if (right != invalid_node) return right; } return invalid_node; } //============================================================================== SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) { IntervalTreeNode* y; IntervalTreeNode* x; SimpleInterval* node_to_delete = z->stored_interval; y = ((z->left == invalid_node) || (z->right == invalid_node)) ? z : getSuccessor(z); x = (y->left == invalid_node) ? y->right : y->left; if (root == (x->parent = y->parent)) { root->left = x; } else { if (y == y->parent->left) { y->parent->left = x; } else { y->parent->right = x; } } /// @brief y should not be invalid_node in this case /// y is the node to splice out and x is its child if (y != z) { y->max_high = -(std::numeric_limits::max)(); y->left = z->left; y->right = z->right; y->parent = z->parent; z->left->parent = z->right->parent = y; if (z == z->parent->left) z->parent->left = y; else z->parent->right = y; fixupMaxHigh(x->parent); if (!(y->red)) { y->red = z->red; deleteFixup(x); } else y->red = z->red; delete z; } else { fixupMaxHigh(x->parent); if (!(y->red)) deleteFixup(x); delete y; } return node_to_delete; } //============================================================================== /// @brief returns 1 if the intervals overlap, and 0 otherwise bool overlap(Scalar a1, Scalar a2, Scalar b1, Scalar b2) { if (a1 <= b1) { return (b1 <= a2); } else { return (a1 <= b2); } } //============================================================================== std::deque IntervalTree::query(Scalar low, Scalar high) { std::deque result_stack; IntervalTreeNode* x = root->left; bool run = (x != invalid_node); current_parent = 0; while (run) { if (overlap(low, high, x->key, x->high)) { result_stack.push_back(x->stored_interval); recursion_node_stack[current_parent].try_right_branch = true; } if (x->left->max_high >= low) { if (recursion_node_stack_top == recursion_node_stack_size) { recursion_node_stack_size *= 2; recursion_node_stack = (it_recursion_node*)realloc( recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node)); if (recursion_node_stack == nullptr) abort(); } recursion_node_stack[recursion_node_stack_top].start_node = x; recursion_node_stack[recursion_node_stack_top].try_right_branch = false; recursion_node_stack[recursion_node_stack_top].parent_index = current_parent; current_parent = recursion_node_stack_top++; x = x->left; } else x = x->right; run = (x != invalid_node); while ((!run) && (recursion_node_stack_top > 1)) { if (recursion_node_stack[--recursion_node_stack_top].try_right_branch) { x = recursion_node_stack[recursion_node_stack_top].start_node->right; current_parent = recursion_node_stack[recursion_node_stack_top].parent_index; recursion_node_stack[current_parent].try_right_branch = true; run = (x != invalid_node); } } } return result_stack; } } // namespace detail } // namespace coal #endif ================================================ FILE: src/broadphase/detail/interval_tree_node.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H #define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H #include "coal/broadphase/detail/interval_tree_node.h" #include #include namespace coal { namespace detail { //============================================================================== IntervalTreeNode::IntervalTreeNode() { // Do nothing } //============================================================================== IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) : stored_interval(new_interval), key(new_interval->low), high(new_interval->high), max_high(high) { // Do nothing } //============================================================================== IntervalTreeNode::~IntervalTreeNode() { // Do nothing } //============================================================================== void IntervalTreeNode::print(IntervalTreeNode* invalid_node, IntervalTreeNode* root) const { stored_interval->print(); std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high; std::cout << " l->key = "; if (left == invalid_node) std::cout << "nullptr"; else std::cout << left->key; std::cout << " r->key = "; if (right == invalid_node) std::cout << "nullptr"; else std::cout << right->key; std::cout << " p->key = "; if (parent == root) std::cout << "nullptr"; else std::cout << parent->key; std::cout << " red = " << (int)red << std::endl; } } // namespace detail } // namespace coal #endif ================================================ FILE: src/broadphase/detail/morton-inl.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * Copyright (c) 2016, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_MORTON_INL_H #define COAL_MORTON_INL_H #include "coal/broadphase/detail/morton.h" namespace coal { /// @cond IGNORE namespace detail { //============================================================================== template uint32_t quantize(S x, uint32_t n) { return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0)); } //============================================================================== template morton_functor::morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== template uint32_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); return detail::morton_code(x, y, z); } //============================================================================== template morton_functor::morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== template uint64_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); return detail::morton_code60(x, y, z); } //============================================================================== template constexpr size_t morton_functor::bits() { return 60; } //============================================================================== template constexpr size_t morton_functor::bits() { return 30; } //============================================================================== template morton_functor>::morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== template std::bitset morton_functor>::operator()( const Vec3s& point) const { S x = (point[0] - base[0]) * inv[0]; S y = (point[1] - base[1]) * inv[1]; S z = (point[2] - base[2]) * inv[2]; int start_bit = bits() - 1; std::bitset bset; x *= 2; y *= 2; z *= 2; for (size_t i = 0; i < bits() / 3; ++i) { bset[start_bit--] = ((z < 1) ? 0 : 1); bset[start_bit--] = ((y < 1) ? 0 : 1); bset[start_bit--] = ((x < 1) ? 0 : 1); x = ((x >= 1) ? 2 * (x - 1) : 2 * x); y = ((y >= 1) ? 2 * (y - 1) : 2 * y); z = ((z >= 1) ? 2 * (z - 1) : 2 * z); } return bset; } //============================================================================== template constexpr size_t morton_functor>::bits() { return N; } } // namespace detail /// @endcond } // namespace coal #endif ================================================ FILE: src/broadphase/detail/morton.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * Copyright (c) 2016, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #include "coal/broadphase/detail/morton-inl.h" namespace coal { /// @cond IGNORE namespace detail { //============================================================================== uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z) { x = (x | (x << 16)) & 0x030000FF; x = (x | (x << 8)) & 0x0300F00F; x = (x | (x << 4)) & 0x030C30C3; x = (x | (x << 2)) & 0x09249249; y = (y | (y << 16)) & 0x030000FF; y = (y | (y << 8)) & 0x0300F00F; y = (y | (y << 4)) & 0x030C30C3; y = (y | (y << 2)) & 0x09249249; z = (z | (z << 16)) & 0x030000FF; z = (z | (z << 8)) & 0x0300F00F; z = (z | (z << 4)) & 0x030C30C3; z = (z | (z << 2)) & 0x09249249; return x | (y << 1) | (z << 2); } //============================================================================== uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z) { uint32_t lo_x = x & 1023u; uint32_t lo_y = y & 1023u; uint32_t lo_z = z & 1023u; uint32_t hi_x = x >> 10u; uint32_t hi_y = y >> 10u; uint32_t hi_z = z >> 10u; return (uint64_t(morton_code(hi_x, hi_y, hi_z)) << 30) | uint64_t(morton_code(lo_x, lo_y, lo_z)); } } // namespace detail /// @endcond } // namespace coal ================================================ FILE: src/broadphase/detail/simple_interval.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H #define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H #include "coal/broadphase/detail/simple_interval.h" #include namespace coal { namespace detail { //============================================================================== SimpleInterval::~SimpleInterval() { // Do nothing } //============================================================================== void SimpleInterval::print() { // Do nothing } } // namespace detail } // namespace coal #endif ================================================ FILE: src/broadphase/detail/spatial_hash.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #ifndef COAL_BROADPHASE_SPATIALHASH_INL_H #define COAL_BROADPHASE_SPATIALHASH_INL_H #include "coal/broadphase/detail/spatial_hash.h" #include namespace coal { namespace detail { //============================================================================== SpatialHash::SpatialHash(const AABB& scene_limit_, Scalar cell_size_) : cell_size(cell_size_), scene_limit(scene_limit_) { width[0] = static_cast(std::ceil(scene_limit.width() / cell_size)); width[1] = static_cast(std::ceil(scene_limit.height() / cell_size)); width[2] = static_cast(std::ceil(scene_limit.depth() / cell_size)); } //============================================================================== std::vector SpatialHash::operator()(const AABB& aabb) const { unsigned int min_x = static_cast( std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size)); unsigned int max_x = static_cast( std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size)); unsigned int min_y = static_cast( std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size)); unsigned int max_y = static_cast( std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size)); unsigned int min_z = static_cast( std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size)); unsigned int max_z = static_cast( std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size)); std::vector keys( static_cast((max_x - min_x) * (max_y - min_y) * (max_z - min_z))); size_t id = 0; for (unsigned int x = min_x; x < max_x; ++x) { for (unsigned int y = min_y; y < max_y; ++y) { for (unsigned int z = min_z; z < max_z; ++z) { keys[id++] = x + y * width[0] + z * width[0] * width[1]; } } } return keys; } } // namespace detail } // namespace coal #endif ================================================ FILE: src/collision.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/collision.h" #include "coal/collision_utility.h" #include "coal/collision_func_matrix.h" #include "coal/narrowphase/narrowphase.h" #include "coal/tracy.hh" namespace coal { CollisionFunctionMatrix& getCollisionFunctionLookTable() { static CollisionFunctionMatrix table; return table; } // reorder collision results in the order the call has been made. void CollisionResult::swapObjects() { for (std::vector::iterator it = contacts.begin(); it != contacts.end(); ++it) { std::swap(it->o1, it->o2); std::swap(it->b1, it->b2); it->nearest_points[0].swap(it->nearest_points[1]); it->normal *= -1; } } std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const CollisionRequest& request, CollisionResult& result) { return collide(o1->collisionGeometryPtr(), o1->getTransform(), o2->collisionGeometryPtr(), o2->getTransform(), request, result); } std::size_t collide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { COAL_TRACY_ZONE_SCOPED_N("coal::collide"); // If security margin is set to -infinity, return that there is no collision if (request.security_margin == -std::numeric_limits::infinity()) { result.clear(); return false; } GJKSolver solver(request); const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); std::size_t res; if (request.num_max_contacts == 0) { COAL_THROW_PRETTY("Invalid number of max contacts (current value is 0).", std::invalid_argument); res = 0; } else { OBJECT_TYPE object_type1 = o1->getObjectType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.collision_matrix[node_type2][node_type1]) { COAL_THROW_PRETTY("Collision function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); res = 0; } else { res = looktable.collision_matrix[node_type2][node_type1]( o2, tf2, o1, tf1, &solver, request, result); result.swapObjects(); result.nearest_points[0].swap(result.nearest_points[1]); result.normal *= -1; } } else { if (!looktable.collision_matrix[node_type1][node_type2]) { COAL_THROW_PRETTY("Collision function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); res = 0; } else res = looktable.collision_matrix[node_type1][node_type2]( o1, tf1, o2, tf2, &solver, request, result); } } // Cache narrow phase solver result. If the option in the request is selected, // also store the solver result in the request for the next call. result.cached_gjk_guess = solver.cached_guess; result.cached_support_func_guess = solver.support_func_cached_guess; request.updateGuess(result); return res; } ComputeCollision::ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2) : o1(o1), o2(o2) { const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); OBJECT_TYPE object_type1 = o1->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type2 = o2->getNodeType(); swap_geoms = object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD); if ((swap_geoms && !looktable.collision_matrix[node_type2][node_type1]) || (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2])) { COAL_THROW_PRETTY("Collision function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } if (swap_geoms) func = looktable.collision_matrix[node_type2][node_type1]; else func = looktable.collision_matrix[node_type1][node_type2]; } std::size_t ComputeCollision::run(const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const { COAL_TRACY_ZONE_SCOPED_N("coal::ComputeCollision::run"); // If security margin is set to -infinity, return that there is no collision if (request.security_margin == -std::numeric_limits::infinity()) { result.clear(); return false; } std::size_t res; if (swap_geoms) { res = func(o2, tf2, o1, tf1, &solver, request, result); result.swapObjects(); result.nearest_points[0].swap(result.nearest_points[1]); result.normal *= -1; } else { res = func(o1, tf1, o2, tf2, &solver, request, result); } // Cache narrow phase solver result. If the option in the request is selected, // also store the solver result in the request for the next call. result.cached_gjk_guess = solver.cached_guess; result.cached_support_func_guess = solver.support_func_cached_guess; request.updateGuess(result); return res; } std::size_t ComputeCollision::operator()(const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const { solver.set(request); std::size_t res; if (request.enable_timings) { Timer timer; res = run(tf1, tf2, request, result); result.timings = timer.elapsed(); } else res = run(tf1, tf2, request, result); return res; } } // namespace coal ================================================ FILE: src/collision_data.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/collision_data.h" namespace coal { bool CollisionRequest::isSatisfied(const CollisionResult& result) const { return result.isCollision() && (num_max_contacts <= result.numContacts()); } bool DistanceRequest::isSatisfied(const DistanceResult& result) const { return (result.min_distance <= 0); } } // namespace coal ================================================ FILE: src/collision_func_matrix.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/collision_func_matrix.h" #include "coal/internal/traversal_node_setup.h" #include <../src/collision_node.h> #include "coal/narrowphase/narrowphase.h" #include "coal/internal/shape_shape_func.h" #include "coal/shape/geometric_shapes_traits.h" #include <../src/traits_traversal.h> namespace coal { #ifdef COAL_HAS_OCTOMAP template std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); if (request.security_margin < 0) COAL_THROW_PRETTY("Negative security margin are not handled yet for Octree", std::invalid_argument); typename TraversalTraitsCollision::CollisionTraversal_t node( request); const TypeA* obj1 = dynamic_cast(o1); const TypeB* obj2 = dynamic_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result); collide(&node, request, result); return result.numContacts(); } #endif namespace details { template struct bvh_shape_traits { enum { Options = RelativeTransformationIsIdentity }; }; #define BVH_SHAPE_DEFAULT_TO_ORIENTED(bv) \ template \ struct bvh_shape_traits { \ enum { Options = 0 }; \ } BVH_SHAPE_DEFAULT_TO_ORIENTED(OBB); BVH_SHAPE_DEFAULT_TO_ORIENTED(RSS); BVH_SHAPE_DEFAULT_TO_ORIENTED(kIOS); BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS); #undef BVH_SHAPE_DEFAULT_TO_ORIENTED } // namespace details /// \tparam _Options takes two values. /// - RelativeTransformationIsIdentity if object 1 should be moved /// into the frame of object 2 before computing collisions. /// - 0 if the query should be made with non-aligned object frames. template ::Options> struct COAL_LOCAL BVHShapeCollider { static std::size_t collide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); if (request.security_margin < 0) COAL_THROW_PRETTY( "Negative security margin are not handled yet for BVHModel", std::invalid_argument); if (_Options & RelativeTransformationIsIdentity) return aligned(o1, tf1, o2, tf2, nsolver, request, result); else return oriented(o1, tf1, o2, tf2, nsolver, request, result); } // namespace coal static std::size_t aligned(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); MeshShapeCollisionTraversalNode node(request); const BVHModel* obj1 = static_cast*>(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3s tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result); coal::collide(&node, request, result); delete obj1_tmp; return result.numContacts(); } static std::size_t oriented(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); MeshShapeCollisionTraversalNode node(request); const BVHModel* obj1 = static_cast*>(o1); const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); coal::collide(&node, request, result); return result.numContacts(); } }; /// @brief Collider functor for HeightField data structure /// \tparam _Options takes two values. /// - RelativeTransformationIsIdentity if object 1 should be moved /// into the frame of object 2 before computing collisions. /// - 0 if the query should be made with non-aligned object frames. template struct COAL_LOCAL HeightFieldShapeCollider { typedef HeightField HF; static std::size_t collide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); const HF& height_field = static_cast(*o1); const Shape& shape = static_cast(*o2); HeightFieldShapeCollisionTraversalNode node(request); initialize(node, height_field, tf1, shape, tf2, nsolver, result); coal::collide(&node, request, result); return result.numContacts(); } }; namespace details { template std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); OrientedMeshCollisionTraversalNode node(request); const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); initialize(node, *obj1, tf1, *obj2, tf2, result); collide(&node, request, result); return result.numContacts(); } } // namespace details template std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); // TODO(louis): each time we call collide on BVH-BVH, we: // - Allocate 2 new BVHs // - Copy and transform the vertices in both BVHs so that they are in the // same frame // - Recompute BVs of the BVH structure // -> all that just to avoid doing (a few) rotations/translations of AABBs. // Is it really worth it? MeshCollisionTraversalNode node(request); const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3s tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); Transform3s tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result); coal::collide(&node, request, result); delete obj1_tmp; delete obj2_tmp; return result.numContacts(); } template <> std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( o1, tf1, o2, tf2, request, result); } template <> std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( o1, tf1, o2, tf2, request, result); } template <> std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( o1, tf1, o2, tf2, request, result); } template std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* /*nsolver*/, const CollisionRequest& request, CollisionResult& result) { return BVHCollide(o1, tf1, o2, tf2, request, result); } CollisionFunctionMatrix::CollisionFunctionMatrix() { for (int i = 0; i < NODE_COUNT; ++i) { for (int j = 0; j < NODE_COUNT; ++j) collision_matrix[i][j] = NULL; } // clang-format off collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_CONVEX16] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_CONVEX32] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_TRIANGLE] = &ShapeShapeCollide; collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CONVEX16] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CONVEX32] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CONVEX16] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CONVEX32] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CONVEX16] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CONVEX32] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider, Box>::collide; collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider, Capsule>::collide; collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider, Cone>::collide; collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder>::collide; collision_matrix[BV_KDOP16][GEOM_CONVEX16] = &BVHShapeCollider, ConvexBase16>::collide; collision_matrix[BV_KDOP16][GEOM_CONVEX32] = &BVHShapeCollider, ConvexBase32>::collide; collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid>::collide; collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider, Box>::collide; collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider, Capsule>::collide; collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider, Cone>::collide; collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder>::collide; collision_matrix[BV_KDOP18][GEOM_CONVEX16] = &BVHShapeCollider, ConvexBase16>::collide; collision_matrix[BV_KDOP18][GEOM_CONVEX32] = &BVHShapeCollider, ConvexBase32>::collide; collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid>::collide; collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider, Box>::collide; collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider, Capsule>::collide; collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider, Cone>::collide; collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder>::collide; collision_matrix[BV_KDOP24][GEOM_CONVEX16] = &BVHShapeCollider, ConvexBase16>::collide; collision_matrix[BV_KDOP24][GEOM_CONVEX32] = &BVHShapeCollider, ConvexBase32>::collide; collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid>::collide; collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CONVEX16] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CONVEX32] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CONVEX16] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CONVEX32] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_CAPSULE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_CONE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_CYLINDER] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_CONVEX16] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_CONVEX32] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_ELLIPSOID] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_CAPSULE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_CONE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_CYLINDER] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_CONVEX16] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_CONVEX32] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_ELLIPSOID] = &HeightFieldShapeCollider::collide; collision_matrix[BV_AABB][BV_AABB] = &BVHCollide; collision_matrix[BV_OBB][BV_OBB] = &BVHCollide; collision_matrix[BV_RSS][BV_RSS] = &BVHCollide; collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide >; collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide >; collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide >; collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide; collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide; #ifdef COAL_HAS_OCTOMAP collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_CONVEX16] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_CONVEX32] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OctreeCollide; collision_matrix[GEOM_BOX][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_CONE][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_CONVEX16][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_CONVEX32][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][BV_AABB] = &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_OBB] = &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_RSS] = &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_kIOS] = &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OctreeCollide > >; collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OctreeCollide > >; collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OctreeCollide > >; collision_matrix[GEOM_OCTREE][HF_AABB] = &OctreeCollide >; collision_matrix[GEOM_OCTREE][HF_OBBRSS] = &OctreeCollide >; collision_matrix[BV_AABB][GEOM_OCTREE] = &OctreeCollide, OcTree>; collision_matrix[BV_OBB][GEOM_OCTREE] = &OctreeCollide, OcTree>; collision_matrix[BV_RSS][GEOM_OCTREE] = &OctreeCollide, OcTree>; collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &OctreeCollide, OcTree>; collision_matrix[BV_kIOS][GEOM_OCTREE] = &OctreeCollide, OcTree>; collision_matrix[BV_KDOP16][GEOM_OCTREE] = &OctreeCollide >, OcTree>; collision_matrix[BV_KDOP18][GEOM_OCTREE] = &OctreeCollide >, OcTree>; collision_matrix[BV_KDOP24][GEOM_OCTREE] = &OctreeCollide >, OcTree>; collision_matrix[HF_AABB][GEOM_OCTREE] = &OctreeCollide, OcTree>; collision_matrix[HF_OBBRSS][GEOM_OCTREE] = &OctreeCollide, OcTree>; // clang-format on #endif } // template struct CollisionFunctionMatrix; } // namespace coal ================================================ FILE: src/collision_node.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include <../src/collision_node.h> #include "coal/internal/traversal_recurse.h" namespace coal { void checkResultLowerBound(const CollisionResult& result, Scalar sqrDistLowerBound) { COAL_UNUSED_VARIABLE(result); const Scalar dummy_precision = std::sqrt(Eigen::NumTraits::epsilon()); COAL_UNUSED_VARIABLE(dummy_precision); if (sqrDistLowerBound == 0) { COAL_ASSERT(result.distance_lower_bound <= dummy_precision, "Distance lower bound should not be positive.", std::logic_error); } else { COAL_ASSERT(result.distance_lower_bound * result.distance_lower_bound - sqrDistLowerBound < dummy_precision, "Distance lower bound and sqrDistLowerBound should coincide.", std::logic_error); } } void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request, CollisionResult& result, BVHFrontList* front_list, bool recursive) { if (front_list && front_list->size() > 0) { propagateBVHFrontListCollisionRecurse(node, request, result, front_list); } else { Scalar sqrDistLowerBound = 0; if (recursive) collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound); else collisionNonRecurse(node, front_list, sqrDistLowerBound); if (!std::isnan(sqrDistLowerBound)) { checkResultLowerBound(result, sqrDistLowerBound); } } } void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, unsigned int qsize) { node->preprocess(); if (qsize <= 2) distanceRecurse(node, 0, 0, front_list); else distanceQueueRecurse(node, 0, 0, front_list, qsize); node->postprocess(); } } // namespace coal ================================================ FILE: src/collision_node.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef COAL_COLLISION_NODE_H #define COAL_COLLISION_NODE_H /// @cond INTERNAL #include "coal/BVH/BVH_front.h" #include "coal/internal/traversal_node_base.h" #include "coal/internal/traversal_node_bvhs.h" /// @brief collision and distance function on traversal nodes. these functions /// provide a higher level abstraction for collision functions provided in /// collision_func_matrix namespace coal { /// collision on collision traversal node /// /// @param node node containing both objects to test, /// @retval squared lower bound to the distance between the objects if they /// do not collide. /// @param front_list list of nodes visited by the query, can be used to /// accelerate computation /// \todo should be COAL_LOCAL but used in unit test. COAL_DLLAPI void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request, CollisionResult& result, BVHFrontList* front_list = NULL, bool recursive = true); /// @brief distance computation on distance traversal node; can use front list /// to accelerate \todo should be COAL_LOCAL but used in unit test. COAL_DLLAPI void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL, unsigned int qsize = 2); } // namespace coal /// @endcond #endif ================================================ FILE: src/collision_object.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/collision_object.h" namespace coal { bool CollisionGeometry::isUncertain() const { return !isOccupied() && !isFree(); } } // namespace coal ================================================ FILE: src/collision_utility.cpp ================================================ // Copyright (c) 2017, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // // This file is part of Coal. // Coal 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 // 3 of the License, or (at your option) any later version. // // Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // Coal. If not, see . #include "coal/collision_utility.h" #include "coal/BVH/BVH_utility.h" namespace coal { namespace details { template inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, const Transform3s& pose, const AABB& aabb) { // Ensure AABB is already computed if (model->aabb_radius < 0) COAL_THROW_PRETTY("Collision geometry AABB should be computed first.", std::invalid_argument); AABB objAabb = rotate(translate(model->aabb_local, pose.getTranslation()), pose.getRotation()); if (!objAabb.overlap(aabb)) { // No intersection. return nullptr; } const BVHModel* m = static_cast*>(model); return BVHExtract(*m, pose, aabb); } CollisionGeometry* extractBVH(const CollisionGeometry* model, const Transform3s& pose, const AABB& aabb) { switch (model->getNodeType()) { case BV_AABB: return extractBVHtpl(model, pose, aabb); case BV_OBB: return extractBVHtpl(model, pose, aabb); case BV_RSS: return extractBVHtpl(model, pose, aabb); case BV_kIOS: return extractBVHtpl(model, pose, aabb); case BV_OBBRSS: return extractBVHtpl(model, pose, aabb); case BV_KDOP16: return extractBVHtpl >(model, pose, aabb); case BV_KDOP18: return extractBVHtpl >(model, pose, aabb); case BV_KDOP24: return extractBVHtpl >(model, pose, aabb); default: COAL_THROW_PRETTY("Unknown type of bounding volume", std::runtime_error); } } } // namespace details CollisionGeometry* extract(const CollisionGeometry* model, const Transform3s& pose, const AABB& aabb) { switch (model->getObjectType()) { case OT_BVH: return details::extractBVH(model, pose, aabb); // case OT_GEOM: return model; default: COAL_THROW_PRETTY("Extraction is not implemented for this type of object", std::runtime_error); } } } // namespace coal ================================================ FILE: src/contact_patch/contact_patch_simplifier.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2025, INRIA * All rights reserved. * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/contact_patch/contact_patch_simplifier.h" #include "coal/alloca.h" #include #include #include #include #include #include namespace coal { void ContactPatchSimplifierNaive::compute(const ContactPatch& patch_in, std::size_t target_vertices, ContactPatch& patch_out) { const auto& pts = patch_in.points(); const std::size_t n = pts.size(); simplified_buffer_.clear(); simplified_buffer_.reserve(std::min(target_vertices, n)); if (n == 0 || target_vertices == 0 || target_vertices >= n) { simplified_buffer_.assign(pts.begin(), pts.end()); } else if (target_vertices == 1) { Vec2s barycenter = Vec2s::Zero(); for (std::size_t i = 0; i < n; ++i) { barycenter += pts[i]; } barycenter /= static_cast(n); simplified_buffer_.push_back(barycenter); } else if (target_vertices == 2) { const Vec2s& first = pts[0]; std::size_t farthest_idx = 0; Scalar max_dist_sq = Scalar(0); for (std::size_t i = 1; i < n; ++i) { const Scalar dist_sq = (pts[i] - first).squaredNorm(); if (dist_sq > max_dist_sq) { max_dist_sq = dist_sq; farthest_idx = i; } } simplified_buffer_.push_back(first); simplified_buffer_.push_back(pts[farthest_idx]); } else { // Brute-force: enumerate all C(n, k) subsets of vertices (preserving CCW // order) and keep the one that maximises the polygon area. const std::size_t k = target_vertices; std::vector indices(k); std::iota(indices.begin(), indices.end(), std::size_t(0)); // Shoelace formula for the polygon formed by the selected indices. const auto compute_area = [&](const std::vector& idx) { Scalar area = Scalar(0); for (std::size_t i = 0; i < k; ++i) { const std::size_t j = (i + 1) % k; area += pts[idx[i]](0) * pts[idx[j]](1); area -= pts[idx[j]](0) * pts[idx[i]](1); } return std::abs(area) * Scalar(0.5); }; Scalar best_area = Scalar(-1); std::vector best_indices(indices); while (true) { const Scalar area = compute_area(indices); if (area > best_area) { best_area = area; best_indices = indices; } // Advance to the next combination in lexicographic order. // Find the rightmost index that can still be incremented. int i = static_cast(k) - 1; while (i >= 0 && indices[static_cast(i)] == n - k + static_cast(i)) { --i; } if (i < 0) break; ++indices[static_cast(i)]; for (std::size_t j = static_cast(i) + 1; j < k; ++j) { indices[j] = indices[j - 1] + 1; } } for (std::size_t i = 0; i < k; ++i) { simplified_buffer_.push_back(pts[best_indices[i]]); } } if (&patch_in != &patch_out) { patch_out = patch_in; } auto& polygon = patch_out.points(); polygon.assign(simplified_buffer_.begin(), simplified_buffer_.end()); } void ContactPatchSimplifierNaive::simplify(ContactPatch& patch, std::size_t target_vertices) { compute(patch, target_vertices, patch); } namespace { // ----------------------------------------------- // Utils for non-trivial contact patch simplifiers // ----------------------------------------------- using Index = std::size_t; template inline T clamp(T val, T min_val, T max_val) { if (val < min_val) { return min_val; } if (val > max_val) { return max_val; } return val; } inline Scalar double_triangle_area(const Vec2s& a, const Vec2s& b, const Vec2s& c) { const Vec2s ab = b - a; const Vec2s ac = c - a; return ab.x() * ac.y() - ab.y() * ac.x(); } Scalar compute_triangle_area(const std::vector& pts, const std::vector& prev, const std::vector& next, int vertex) { const int prev_idx = prev[static_cast(vertex)]; const int next_idx = next[static_cast(vertex)]; if (prev_idx == vertex || next_idx == vertex) { return Scalar(0); } const Vec2s& a = pts[static_cast(prev_idx)]; const Vec2s& b = pts[static_cast(vertex)]; const Vec2s& c = pts[static_cast(next_idx)]; return std::abs(double_triangle_area(a, b, c)) * Scalar(0.5); } Scalar compute_triangle_area_kgon(const std::vector& pts, // int i, int j, int k) { int n = static_cast(pts.size()); i %= n; j %= n; k %= n; if (i == j || j == k || i == k) return Scalar(0); const Vec2s& a = pts[static_cast(i)]; const Vec2s& b = pts[static_cast(j)]; const Vec2s& c = pts[static_cast(k)]; return std::abs(double_triangle_area(a, b, c)) * Scalar(0.5); } Scalar compute_rooted_kgon(const std::vector& pts, int root, int k, boost::span left_c, boost::span right_c, boost::span best_v, boost::span dp_area, boost::span dp_prev) { int n = static_cast(pts.size()); for (int m = 1; m < k; ++m) { int len = right_c[Index(m - 1)] - left_c[Index(m - 1)] + 1; for (int i = 0; i < len; ++i) { dp_area[Index(m * 2 * n + i)] = Scalar(-1.0); dp_prev[Index(m * 2 * n + i)] = -1; } } for (int i = left_c[0]; i <= right_c[0]; ++i) { dp_area[Index(1 * 2 * n + (i - left_c[0]))] = Scalar(0.0); dp_prev[Index(1 * 2 * n + (i - left_c[0]))] = root; } for (int m = 2; m < k; ++m) { for (int i = left_c[Index(m - 1)]; i <= right_c[Index(m - 1)]; ++i) { Scalar best_area = Scalar(-1.0); int best_j = -1; int j_start = left_c[Index(m - 2)]; int j_end = std::min(right_c[Index(m - 2)], i - 1); for (int j = j_start; j <= j_end; ++j) { Scalar prev_val = dp_area[Index((m - 1) * 2 * n + (j - left_c[Index(m - 2)]))]; if (prev_val < Scalar(0.0)) continue; Scalar area = prev_val + compute_triangle_area_kgon(pts, root, j, i); if (area > best_area) { best_area = area; best_j = j; } } dp_area[Index(m * 2 * n + (i - left_c[Index(m - 1)]))] = best_area; dp_prev[Index(m * 2 * n + (i - left_c[Index(m - 1)]))] = best_j; } } Scalar max_total_area = Scalar(-1.0); int best_end = -1; for (int i = left_c[Index(k - 2)]; i <= right_c[Index(k - 2)]; ++i) { Scalar val = dp_area[Index((k - 1) * 2 * n + (i - left_c[Index(k - 2)]))]; if (val > max_total_area) { max_total_area = val; best_end = i; } } std::fill(best_v.begin(), best_v.end(), 0); best_v[0] = root; if (best_end != -1) { int curr = best_end; for (int m = k - 1; m >= 1; --m) { best_v[Index(m)] = curr; curr = dp_prev[Index(m * 2 * n + (curr - left_c[Index(m - 1)]))]; } } return max_total_area; } void solve_recursive(const std::vector& pts, int k, int root_start, int root_end, boost::span left_bound, boost::span right_bound, Scalar& global_max_area, boost::span global_best_v, boost::span dp_area, boost::span dp_prev) { if (root_start > root_end) return; int mid_root = root_start + (root_end - root_start) / 2; COAL_MAKE_ALLOCA_BOOST_SPAN(int, mid_v, static_cast(k)); Scalar mid_area = compute_rooted_kgon(pts, mid_root, k, left_bound, right_bound, mid_v, dp_area, dp_prev); if (mid_area > global_max_area) { global_max_area = mid_area; for (int i = 0; i < k; ++i) { global_best_v[static_cast(i)] = mid_v[static_cast(i)]; } } if (root_start < mid_root) { COAL_MAKE_ALLOCA_BOOST_SPAN(int, new_right, static_cast(k - 1)); for (int m = 1; m < k; ++m) new_right[static_cast(m - 1)] = mid_v[static_cast(m)]; solve_recursive(pts, k, root_start, mid_root - 1, left_bound, new_right, global_max_area, global_best_v, dp_area, dp_prev); } if (mid_root < root_end) { COAL_MAKE_ALLOCA_BOOST_SPAN(int, new_left, static_cast(k - 1)); for (int m = 1; m < k; ++m) new_left[static_cast(m - 1)] = mid_v[static_cast(m)]; solve_recursive(pts, k, mid_root + 1, root_end, new_left, right_bound, global_max_area, global_best_v, dp_area, dp_prev); } } } // namespace // Let's start by a remark: if you start at vertex p0 of a 2D polygon, // (p0 is any vertex in the polygon, then p1, p2... are the counter-clockwise // sequence of vertices forming the polygon), you can subdivide this polygon // by the sequence of triangles ((p0, p1, p2), (p1, p2, p3), ... (pi, pi+1, // pi+2)...). // These triangles don't overlap and the sum of their area is the area of the // polygon. In short, you can view a polygon as a "fan" of triangles starting at // p0. As said, this vertex p0 is arbitrary and we could use any vertex of the // polygon to produce a new fan. // What we want to do is select a vertex p0 and a sequence of vertices such that // the fan starting at p0 and produced by the sequence has the maximum possible // area. // // A naive way to do that would be to test all possible combinations of // (starting vertex, sequence of vertices). // Instead, we use dynamics programming to find this combination. // // - We want to find a sequence of size d inside our polygon of size n. // - We start at an anchor vertex p0 (a vertex of the polygon). // - We construct dp[i][k] using DP. dp[i][k] is a table that iteratively // computes the highest possible area of the sequence of k vertices starting // at p0 and ending at pi. It is updated as: dp[0][1] = 0 (anchor with only // itself as sequence has a 0 area). Then for i+1 >= k (and k <= n): dp[i][k] // = max(dp[j][k-1] + area(p0, pj, pi), for j in [k-2, i-1]) // - Once dp has been constructed for anchor p0, we search dp[i][n] (for i >= // n-1) to find the best terminating pi and recover the associated sequence of // vertices. // - This produces the "best" fan for anchor p0. We compare it to the previous // "best" (anchor, sequence) and keep the best of the two. // - We repeat this procedure with the next anchor. // // Note: this implementation is heavily inspired by CGAL's 2D inscribed k-gon // method. See here for more info: // https://doc.cgal.org/latest/Inscribed_areas/group__PkgInscribedAreasRef.html void ContactPatchSimplifierMaxArea::compute(const ContactPatch& patch_in, Index target_vertices, ContactPatch& patch_out) { const auto& pts = patch_in.points(); const Index n = pts.size(); simplified_buffer_.clear(); simplified_buffer_.reserve(std::min(target_vertices, n)); if (n == 0 || target_vertices == 0 || target_vertices >= n) { simplified_buffer_.assign(pts.begin(), pts.end()); } else if (target_vertices == 1) { Vec2s barycenter = Vec2s::Zero(); for (Index i = 0; i < n; ++i) { barycenter += pts[i]; } barycenter /= static_cast(n); simplified_buffer_.push_back(barycenter); } else if (target_vertices == 2) { const Vec2s& first = pts[0]; Index farthest_idx = 0; Scalar max_dist_sq = Scalar(0); for (Index i = 1; i < n; ++i) { const Scalar dist_sq = (pts[i] - first).squaredNorm(); if (dist_sq > max_dist_sq) { max_dist_sq = dist_sq; farthest_idx = i; } } simplified_buffer_.push_back(first); simplified_buffer_.push_back(pts[farthest_idx]); } else { const std::size_t desired = clamp(target_vertices, 3, n); COAL_MAKE_ALLOCA_BOOST_SPAN(Scalar, dp_area, static_cast(desired * 2 * n)); COAL_MAKE_ALLOCA_BOOST_SPAN(int, dp_prev, static_cast(desired * 2 * n)); COAL_MAKE_ALLOCA_BOOST_SPAN(int, left_bound, desired - 1); COAL_MAKE_ALLOCA_BOOST_SPAN(int, right_bound, desired - 1); for (std::size_t m = 1; m < desired; ++m) { left_bound[m - 1] = int(m); right_bound[m - 1] = int(n - desired + m); } COAL_MAKE_ALLOCA_BOOST_SPAN(int, P0, desired); Scalar area0 = compute_rooted_kgon(pts, 0, int(desired), left_bound, right_bound, P0, dp_area, dp_prev); COAL_MAKE_ALLOCA_BOOST_SPAN(int, P0_ext, desired + 1); for (std::size_t i = 0; i < desired; ++i) { P0_ext[i] = P0[i]; } P0_ext[desired] = int(n); COAL_MAKE_ALLOCA_BOOST_SPAN(int, P1, desired); COAL_MAKE_ALLOCA_BOOST_SPAN(int, left_bound1, desired - 1); COAL_MAKE_ALLOCA_BOOST_SPAN(int, right_bound1, desired - 1); for (std::size_t m = 1; m < desired; ++m) { left_bound1[m - 1] = P0_ext[m]; right_bound1[m - 1] = P0_ext[m + 1]; } Scalar area1 = compute_rooted_kgon(pts, P0[1], int(desired), left_bound1, right_bound1, P1, dp_area, dp_prev); Scalar global_max_area = std::max(area0, area1); COAL_MAKE_ALLOCA_BOOST_SPAN(int, global_best_v, desired); if (area0 > area1) { for (std::size_t i = 0; i < desired; ++i) { global_best_v[i] = P0[i]; } } else { for (std::size_t i = 0; i < desired; ++i) { global_best_v[i] = P1[i]; } } if (P0[1] - P0[0] > 1) { COAL_MAKE_ALLOCA_BOOST_SPAN(int, arg_left_bound, desired - 1); COAL_MAKE_ALLOCA_BOOST_SPAN(int, arg_right_bound, desired - 1); for (std::size_t i = 0; i < desired - 1; ++i) { arg_left_bound[i] = P0_ext[i + 1]; arg_right_bound[i] = P1[i + 1]; } solve_recursive(pts, int(desired), P0[0] + 1, P0[1] - 1, arg_left_bound, arg_right_bound, global_max_area, global_best_v, dp_area, dp_prev); } for (std::size_t i = 0; i < desired; ++i) { int v = global_best_v[i]; simplified_buffer_.push_back(pts[static_cast(v % int(n))]); } } if (&patch_in != &patch_out) { patch_out = patch_in; } auto& polygon = patch_out.points(); polygon.assign(simplified_buffer_.begin(), simplified_buffer_.end()); } void ContactPatchSimplifierMaxArea::simplify(ContactPatch& patch, Index target_vertices) { compute(patch, target_vertices, patch); } // This greedy version is based on the Visvalingam–Whyatt rule. // Basically, instead of constructing a sequence of indices, we instead look // at which vertices can be removed without reducing the overall area too much // compared to the original polygon. // // First we compute the area of all the triangles (pi, pi+1, pi+2) and put them // in a heap. // Note that we can associate a triangle to each vertex and vice versa. // Then, until we reach the desired number of points in the sub-polygon, // we remove the smallest triangle, remove the associated vertex (if not already // removed), compute the area of the newly formed triangles using the vertex's // neighbors, and put these new triangles in the heap. void ContactPatchSimplifierGreedy::compute(const ContactPatch& patch_in, Index target_vertices, ContactPatch& patch_out) { const auto& pts = patch_in.points(); const Index n = pts.size(); simplified_buffer_.clear(); simplified_buffer_.reserve(std::min(target_vertices, static_cast(n))); if (n == 0 || target_vertices == 0 || target_vertices >= n) { simplified_buffer_.assign(pts.begin(), pts.end()); } else if (target_vertices == 1) { // Return the barycenter of the patch Vec2s barycenter = Vec2s::Zero(); for (Index i = 0; i < n; ++i) { barycenter += pts[i]; } barycenter /= static_cast(n); simplified_buffer_.push_back(barycenter); } else if (target_vertices == 2) { // Return the first point and the point farthest from it const Vec2s& first = pts[0]; Index farthest_idx = 0; Scalar max_dist_sq = Scalar(0); for (Index i = 1; i < n; ++i) { const Scalar dist_sq = (pts[i] - first).squaredNorm(); if (dist_sq > max_dist_sq) { max_dist_sq = dist_sq; farthest_idx = i; } } simplified_buffer_.push_back(first); simplified_buffer_.push_back(pts[farthest_idx]); } else { const Index desired = clamp(target_vertices, 3, n); prev_.resize(n); next_.resize(n); removed_.resize(n); // note: versions_ is used to only compare up to data nodes. // That way we don't have to pop outdated nodes from the heap. versions_.resize(n); heap_storage_.clear(); heap_storage_.reserve(n); for (Index i = 0; i < n; ++i) { prev_[i] = static_cast((i + n - 1) % n); next_[i] = static_cast((i + 1) % n); removed_[i] = false; versions_[i] = 0; const Scalar area = compute_triangle_area(pts, prev_, next_, static_cast(i)); heap_storage_.push_back({i, area, versions_[i]}); } const auto comp = [](const HeapEntry& lhs, const HeapEntry& rhs) { return lhs.area > rhs.area; }; std::make_heap(heap_storage_.begin(), heap_storage_.end(), comp); Index remaining = n; while (remaining > desired && !heap_storage_.empty()) { std::pop_heap(heap_storage_.begin(), heap_storage_.end(), comp); HeapEntry node = heap_storage_.back(); heap_storage_.pop_back(); if (removed_[node.idx]) { continue; } if (node.version != versions_[node.idx]) { continue; } const Index prev_idx = static_cast(prev_[node.idx]); const Index next_idx = static_cast(next_[node.idx]); if (prev_idx == node.idx || next_idx == node.idx) { break; } removed_[node.idx] = true; --remaining; next_[prev_idx] = static_cast(next_idx); prev_[next_idx] = static_cast(prev_idx); if (!removed_[prev_idx]) { ++versions_[prev_idx]; const Scalar area_prev = compute_triangle_area( pts, prev_, next_, static_cast(prev_idx)); heap_storage_.push_back({prev_idx, area_prev, versions_[prev_idx]}); std::push_heap(heap_storage_.begin(), heap_storage_.end(), comp); } if (!removed_[next_idx]) { ++versions_[next_idx]; const Scalar area_next = compute_triangle_area( pts, prev_, next_, static_cast(next_idx)); heap_storage_.push_back({next_idx, area_next, versions_[next_idx]}); std::push_heap(heap_storage_.begin(), heap_storage_.end(), comp); } } for (Index i = 0; i < n; ++i) { if (!removed_[i]) { simplified_buffer_.push_back(pts[i]); } } } if (&patch_in != &patch_out) { patch_out = patch_in; } auto& polygon = patch_out.points(); polygon.assign(simplified_buffer_.begin(), simplified_buffer_.end()); } void ContactPatchSimplifierGreedy::simplify(ContactPatch& patch, Index target_vertices) { compute(patch, target_vertices, patch); } } // namespace coal ================================================ FILE: src/contact_patch/contact_patch_solver.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \authors Louis Montaut */ #include "coal/contact_patch/contact_patch_solver.h" namespace coal { namespace details { /// @brief Templated shape support set functions. template void getShapeSupportSetTpl(const ShapeBase* shape, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t num_sampled_supports = 6, Scalar tol = Scalar(1e-3)) { const ShapeType* shape_ = static_cast(shape); getShapeSupportSet<_SupportOptions>(shape_, support_set, hint, support_data, num_sampled_supports, tol); } /// @brief Templated shape support set functions for ConvexBase. template void getConvexBaseSupportSetTpl(const ShapeBase* shape, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t num_sampled_supports = 6, Scalar tol = Scalar(1e-3)) { const ConvexBaseTpl* convex = static_cast*>(shape); if (support_data.polygon.capacity() < ::coal::ContactPatchSolver::default_num_preallocated_supports) { support_data.polygon.reserve( ::coal::ContactPatchSolver::default_num_preallocated_supports); } if ((size_t)(convex->num_points) > ConvexBaseTpl::num_vertices_large_convex_threshold) { const LargeConvex* convex_ = static_cast*>(convex); support_data.visited.assign(convex_->num_points, false); support_data.last_dir.setZero(); return getShapeSupportSet<_SupportOptions>( convex_, support_set, hint, support_data, num_sampled_supports, tol); } else { const SmallConvex* convex_ = static_cast*>(convex); return getShapeSupportSet<_SupportOptions>( convex_, support_set, hint, support_data, num_sampled_supports, tol); } } } // namespace details // ============================================================================ ContactPatchSolver::SupportSetFunction ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape, ShapeSupportData& support_data) { // Note: because the swept-sphere radius was already taken into account when // constructing the contact patch frame, there is actually no need to take the // swept-sphere radius of shapes into account. The origin of the contact patch // frame already encodes this information. using Options = details::SupportOptions; switch (shape->getNodeType()) { case GEOM_TRIANGLE: return details::getShapeSupportSetTpl; case GEOM_BOX: { const size_t num_corners_box = 8; support_data.polygon.reserve(num_corners_box); return details::getShapeSupportSetTpl; } case GEOM_SPHERE: return details::getShapeSupportSetTpl; case GEOM_ELLIPSOID: return details::getShapeSupportSetTpl; case GEOM_CAPSULE: return details::getShapeSupportSetTpl; case GEOM_CONE: return details::getShapeSupportSetTpl; case GEOM_CYLINDER: return details::getShapeSupportSetTpl; case GEOM_CONVEX16: return details::getConvexBaseSupportSetTpl; case GEOM_CONVEX32: return details::getConvexBaseSupportSetTpl; default: COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); } } } // namespace coal ================================================ FILE: src/contact_patch/polygon_convex_hull.cpp ================================================ #include "coal/contact_patch/polygon_convex_hull.h" #include "coal/alloca.h" #include namespace coal { void computePolygonConvexHull(const std::vector& cloud, std::vector& cvx_hull) { cvx_hull.clear(); if (cloud.size() <= 2) { // Point or segment, nothing to do. for (const Vec2s& point : cloud) { cvx_hull.emplace_back(point); } return; } // temporary copy of cloud to work on COAL_MAKE_ALLOCA_BOOST_SPAN(Vec2s, cloud_, cloud.size()); for (size_t i = 0; i < cloud.size(); ++i) { cloud_[i] = cloud[i]; } if (cloud_.size() == 3) { // We have a triangle, we only need to arrange it in a counter clockwise // fashion. // // Put the vector which has the lowest y coordinate first. if (cloud_[0](1) > cloud_[1](1)) { std::swap(cloud_[0], cloud_[1]); } if (cloud_[0](1) > cloud_[2](1)) { std::swap(cloud_[0], cloud_[2]); } const Vec2s& a = cloud_[0]; const Vec2s& b = cloud_[1]; const Vec2s& c = cloud_[2]; const Scalar det = (b(0) - a(0)) * (c(1) - a(1)) - (b(1) - a(1)) * (c(0) - a(0)); if (det < 0) { std::swap(cloud_[1], cloud_[2]); } for (const Vec2s& point : cloud_) { cvx_hull.emplace_back(point); } return; } // The following is an implementation of the O(nlog(n)) graham scan // algorithm, used to compute convex-hulls in 2D. // See https://en.wikipedia.org/wiki/Graham_scan // // Step 1 - Compute first element of the convex-hull by computing the support // in the direction (0, -1) (take the element of the set which has the lowest // y coordinate). size_t support_idx = 0; Scalar support_val = cloud_[0](1); for (size_t i = 1; i < cloud_.size(); ++i) { const Scalar val = cloud_[i](1); if (val < support_val) { support_val = val; support_idx = i; } } Vec2s tmp = cloud_[0]; cloud_[0] = cloud_[support_idx]; cloud_[support_idx] = tmp; cvx_hull.clear(); cvx_hull.emplace_back(cloud_[0]); const Vec2s v = cvx_hull[0]; // Step 2 - Sort the rest of the point cloud_ according to the angle made with // v. Note: we use stable_sort instead of sort because sort can fail if two // values are identical. std::sort( cloud_.begin() + 1, cloud_.end(), [&v](const Vec2s& p1, const Vec2s& p2) { // p1 is "smaller" than p2 if det(p1 - v, p2 - v) >= 0 const Scalar det = (p1(0) - v(0)) * (p2(1) - v(1)) - (p1(1) - v(1)) * (p2(0) - v(0)); if (std::abs(det) <= Eigen::NumTraits::dummy_precision()) { // If two points are identical or (v, p1, p2) are colinear, p1 is // "smaller" if it is closer to v. return ((p1 - v).squaredNorm() <= (p2 - v).squaredNorm()); } return det > 0; }); // Step 3 - We iterate over the now ordered point of cloud_ and add the points // to the cvx-hull if they successively form "left turns" only. A left turn // is: considering the last three points of the cvx-hull, if they form a // right-hand basis (determinant > 0) then they make a left turn. auto isRightSided = [](const Vec2s& p1, const Vec2s& p2, const Vec2s& p3) { // Checks if (p2 - p1, p3 - p1) forms a right-sided base based on // det(p2 - p1, p3 - p1) const Scalar det = (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(1)) * (p3(0) - p1(0)); // Note: we set a dummy precision threshold so that identical points or // colinear pionts are not added to the cvx-hull. return det > Eigen::NumTraits::dummy_precision(); }; // We initialize the cvx-hull algo by adding the first three // (distinct) points of the set. // These three points are guaranteed, due to the previous sorting, // to form a right sided basis, hence to form a left turn. size_t cloud_beginning_idx = 1; while (cvx_hull.size() < 3) { const Vec2s& vec = cloud_[cloud_beginning_idx]; if ((cvx_hull.back() - vec).squaredNorm() > Eigen::NumTraits::epsilon()) { cvx_hull.emplace_back(vec); } ++cloud_beginning_idx; } // The convex-hull should wrap counter-clockwise, i.e. three successive // points should always form a right-sided basis. Every time we do a turn // in the wrong direction, we remove the last point of the convex-hull. // When we do a turn in the correct direction, we add a point to the // convex-hull. for (size_t i = cloud_beginning_idx; i < cloud_.size(); ++i) { const Vec2s& vec = cloud_[i]; while (cvx_hull.size() > 1 && !isRightSided(cvx_hull[cvx_hull.size() - 2], cvx_hull[cvx_hull.size() - 1], vec)) { cvx_hull.pop_back(); } cvx_hull.emplace_back(vec); } } } // namespace coal ================================================ FILE: src/contact_patch.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/contact_patch.h" #include "coal/collision_utility.h" #include "coal/tracy.hh" namespace coal { ContactPatchFunctionMatrix& getContactPatchFunctionLookTable() { static ContactPatchFunctionMatrix table; return table; } void computeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) { COAL_TRACY_ZONE_SCOPED_N("coal::computeContactPatch"); if (!collision_result.isCollision() || request.max_num_patch == 0) { // do nothing return; } // Before doing any computation, we initialize and clear the input result. result.set(request); ContactPatchSolver csolver(request); OBJECT_TYPE object_type1 = o1->getObjectType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); const ContactPatchFunctionMatrix& looktable = getContactPatchFunctionLookTable(); if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.contact_patch_matrix[node_type2][node_type1]) { COAL_THROW_PRETTY("Computing contact patches between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } looktable.contact_patch_matrix[node_type2][node_type1]( o2, tf2, o1, tf1, collision_result, &csolver, request, result); result.swapObjects(); return; } if (!looktable.contact_patch_matrix[node_type1][node_type2]) { COAL_THROW_PRETTY("Contact patch computation between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } return looktable.contact_patch_matrix[node_type1][node_type2]( o1, tf1, o2, tf2, collision_result, &csolver, request, result); } void computeContactPatch(const CollisionObject* o1, const CollisionObject* o2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) { return computeContactPatch(o1->collisionGeometryPtr(), o1->getTransform(), o2->collisionGeometryPtr(), o2->getTransform(), collision_result, request, result); } ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2) : o1(o1), o2(o2) { const ContactPatchFunctionMatrix& looktable = getContactPatchFunctionLookTable(); OBJECT_TYPE object_type1 = this->o1->getObjectType(); NODE_TYPE node_type1 = this->o1->getNodeType(); OBJECT_TYPE object_type2 = this->o2->getObjectType(); NODE_TYPE node_type2 = this->o2->getNodeType(); this->swap_geoms = object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD); if ((this->swap_geoms && !looktable.contact_patch_matrix[node_type2][node_type1]) || (!this->swap_geoms && !looktable.contact_patch_matrix[node_type1][node_type2])) { COAL_THROW_PRETTY("Collision function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } if (this->swap_geoms) { this->func = looktable.contact_patch_matrix[node_type2][node_type1]; } else { this->func = looktable.contact_patch_matrix[node_type1][node_type2]; } } void ComputeContactPatch::run(const Transform3s& tf1, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const { COAL_TRACY_ZONE_SCOPED_N("coal::ComputeContactPatch::run"); if (!collision_result.isCollision() || request.max_num_patch == 0) { // do nothing return; } // Before doing any computation, we initialize and clear the input result. result.set(request); if (this->swap_geoms) { this->func(this->o2, tf2, this->o1, tf1, collision_result, &(this->csolver), request, result); result.swapObjects(); } else { this->func(this->o1, tf1, this->o2, tf2, collision_result, &(this->csolver), request, result); } } void ComputeContactPatch::operator()(const Transform3s& tf1, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const { this->csolver.set(request); this->run(tf1, tf2, collision_result, request, result); } } // namespace coal ================================================ FILE: src/contact_patch_func_matrix.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/contact_patch_func_matrix.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_contact_patch_func.h" #include "coal/BV/BV.h" namespace coal { template struct BVHShapeComputeContactPatch { static void run(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { COAL_UNUSED_VARIABLE(o1); COAL_UNUSED_VARIABLE(tf1); COAL_UNUSED_VARIABLE(o2); COAL_UNUSED_VARIABLE(tf2); COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; } const Contact& contact = collision_result.getContact(i); ContactPatch& contact_patch = result.getUnusedContactPatch(); constructContactPatchFrameFromContact(contact, contact_patch); contact_patch.addPoint(contact.pos); } } }; template struct HeightFieldShapeComputeContactPatch { static void run(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { COAL_UNUSED_VARIABLE(o1); COAL_UNUSED_VARIABLE(tf1); COAL_UNUSED_VARIABLE(o2); COAL_UNUSED_VARIABLE(tf2); COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; } const Contact& contact = collision_result.getContact(i); ContactPatch& contact_patch = result.getUnusedContactPatch(); constructContactPatchFrameFromContact(contact, contact_patch); contact_patch.addPoint(contact.pos); } } }; template struct BVHComputeContactPatch { static void run(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { COAL_UNUSED_VARIABLE(o1); COAL_UNUSED_VARIABLE(tf1); COAL_UNUSED_VARIABLE(o2); COAL_UNUSED_VARIABLE(tf2); COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; } const Contact& contact = collision_result.getContact(i); ContactPatch& contact_patch = result.getUnusedContactPatch(); constructContactPatchFrameFromContact(contact, contact_patch); contact_patch.addPoint(contact.pos); } } }; COAL_LOCAL void contact_patch_function_not_implemented( const CollisionGeometry* o1, const Transform3s& /*tf1*/, const CollisionGeometry* o2, const Transform3s& /*tf2*/, const CollisionResult& /*collision_result*/, const ContactPatchSolver* /*csolver*/, const ContactPatchRequest& /*request*/, ContactPatchResult& /*result*/) { NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); COAL_THROW_PRETTY("Contact patch function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { for (int i = 0; i < NODE_COUNT; ++i) { for (int j = 0; j < NODE_COUNT; ++j) contact_patch_matrix[i][j] = nullptr; } // clang-format off contact_patch_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_SPHERE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_CAPSULE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_CONE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_CYLINDER] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_CONVEX16] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_CONVEX32] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_PLANE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; // TODO(louis): properly handle non-convex shapes like BVH, Octrees and Hfields. // The following functions work. However apart from the contact frame, these functions don't // compute more information than a call to `collide`. contact_patch_matrix[BV_AABB][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_CONE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_CONVEX16] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_CONVEX32] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_CONE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_CONVEX16] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_CONVEX32] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_CONE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_CONVEX16] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_CONVEX32] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeComputeContactPatch, Box>::run; contact_patch_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeComputeContactPatch, Sphere>::run; contact_patch_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeComputeContactPatch, Capsule>::run; contact_patch_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeComputeContactPatch, Cone>::run; contact_patch_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeComputeContactPatch, Cylinder>::run; contact_patch_matrix[BV_KDOP16][GEOM_CONVEX16] = &BVHShapeComputeContactPatch, ConvexBase16>::run; contact_patch_matrix[BV_KDOP16][GEOM_CONVEX32] = &BVHShapeComputeContactPatch, ConvexBase32>::run; contact_patch_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeComputeContactPatch, Plane>::run; contact_patch_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch, Halfspace>::run; contact_patch_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch, Ellipsoid>::run; contact_patch_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeComputeContactPatch, Box>::run; contact_patch_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeComputeContactPatch, Sphere>::run; contact_patch_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeComputeContactPatch, Capsule>::run; contact_patch_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeComputeContactPatch, Cone>::run; contact_patch_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeComputeContactPatch, Cylinder>::run; contact_patch_matrix[BV_KDOP18][GEOM_CONVEX16] = &BVHShapeComputeContactPatch, ConvexBase16>::run; contact_patch_matrix[BV_KDOP18][GEOM_CONVEX32] = &BVHShapeComputeContactPatch, ConvexBase32>::run; contact_patch_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeComputeContactPatch, Plane>::run; contact_patch_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch, Halfspace>::run; contact_patch_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch, Ellipsoid>::run; contact_patch_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeComputeContactPatch, Box>::run; contact_patch_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeComputeContactPatch, Sphere>::run; contact_patch_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeComputeContactPatch, Capsule>::run; contact_patch_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeComputeContactPatch, Cone>::run; contact_patch_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeComputeContactPatch, Cylinder>::run; contact_patch_matrix[BV_KDOP24][GEOM_CONVEX16] = &BVHShapeComputeContactPatch, ConvexBase16>::run; contact_patch_matrix[BV_KDOP24][GEOM_CONVEX32] = &BVHShapeComputeContactPatch, ConvexBase32>::run; contact_patch_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeComputeContactPatch, Plane>::run; contact_patch_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch, Halfspace>::run; contact_patch_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch, Ellipsoid>::run; contact_patch_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_CONVEX16] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_CONVEX32] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_CONVEX16] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_CONVEX32] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_CAPSULE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_CONE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_CYLINDER] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_CONVEX16] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_CONVEX32] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_ELLIPSOID] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_CAPSULE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_CONE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_CYLINDER] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_CONVEX16] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_CONVEX32] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_ELLIPSOID] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][BV_AABB] = &BVHComputeContactPatch::run; contact_patch_matrix[BV_OBB][BV_OBB] = &BVHComputeContactPatch::run; contact_patch_matrix[BV_RSS][BV_RSS] = &BVHComputeContactPatch::run; contact_patch_matrix[BV_KDOP16][BV_KDOP16] = &BVHComputeContactPatch>::run; contact_patch_matrix[BV_KDOP18][BV_KDOP18] = &BVHComputeContactPatch>::run; contact_patch_matrix[BV_KDOP24][BV_KDOP24] = &BVHComputeContactPatch>::run; contact_patch_matrix[BV_kIOS][BV_kIOS] = &BVHComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHComputeContactPatch::run; // TODO(louis): octrees #ifdef COAL_HAS_OCTOMAP contact_patch_matrix[GEOM_OCTREE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_BOX] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_SPHERE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_CONE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_CONVEX] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_PLANE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_TRIANGLE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][BV_AABB] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][BV_OBB] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][BV_RSS] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][BV_OBBRSS] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][BV_kIOS] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][BV_KDOP16] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][BV_KDOP18] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][BV_KDOP24] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][HF_AABB] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][HF_OBBRSS] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_BOX][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_SPHERE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_CONE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_CONVEX][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_PLANE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_TRIANGLE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[BV_AABB][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[BV_OBB][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[BV_RSS][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[BV_OBBRSS][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[BV_kIOS][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[BV_KDOP16][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[BV_KDOP18][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[BV_KDOP24][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[HF_AABB][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[HF_OBBRSS][GEOM_OCTREE] = &contact_patch_function_not_implemented; #endif // clang-format on } } // namespace coal ================================================ FILE: src/distance/box_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Box& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const Scalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/box_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Box& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const Scalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/box_sphere.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Box& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::boxSphereDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Sphere& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); const Scalar distance = details::boxSphereDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } } // namespace internal } // namespace coal ================================================ FILE: src/distance/capsule_capsule.cpp ================================================ /* * Software License Agreement (BSD License) * Copyright (c) 2015-2021, CNRS-LAAS INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "coal/tracy.hh" // Note that partial specialization of template functions is not allowed. // Therefore, two implementations with the default narrow phase solvers are // provided. If another narrow phase solver were to be used, the default // template implementation would be called, unless the function is also // specialized for this new type. // // One solution would be to make narrow phase solvers derive from an abstract // class and specialize the template for this abstract class. namespace coal { struct GJKSolver; namespace internal { /// Clamp num / denom in [0, 1] Scalar clamp(const Scalar& num, const Scalar& denom) { assert(denom >= 0.); if (num <= 0.) return 0.; else if (num >= denom) return 1.; else return num / denom; } /// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd void clamped_linear(Vec3s& a_sd, const Vec3s& a, const Scalar& s_n, const Scalar& s_d, const Vec3s& d) { assert(s_d >= 0.); if (s_n <= 0.) a_sd = a; else if (s_n >= s_d) a_sd = a + d; else a_sd = a + s_n / s_d * d; } // Compute the distance between C1 and C2 by computing the distance // between the two segments supporting the capsules. // Match algorithm of Real-Time Collision Detection, Christer Ericson - Closest // Point of Two Line Segments /// @param wp1, wp2: witness points on the capsules /// @param normal: normal pointing from capsule1 to capsule2 template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& wp1, Vec3s& wp2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Capsule* capsule1 = static_cast(o1); const Capsule* capsule2 = static_cast(o2); Scalar EPSILON = std::numeric_limits::epsilon() * 100; // We assume that capsules are centered at the origin. const coal::Vec3s& c1 = tf1.getTranslation(); const coal::Vec3s& c2 = tf2.getTranslation(); // We assume that capsules are oriented along z-axis. Scalar halfLength1 = capsule1->halfLength; Scalar halfLength2 = capsule2->halfLength; Scalar radius1 = (capsule1->radius + capsule1->getSweptSphereRadius()); Scalar radius2 = (capsule2->radius + capsule2->getSweptSphereRadius()); // direction of capsules // ||d1|| = 2 * halfLength1 const coal::Vec3s d1 = 2 * halfLength1 * tf1.getRotation().col(2); const coal::Vec3s d2 = 2 * halfLength2 * tf2.getRotation().col(2); // Starting point of the segments // p1 + d1 is the end point of the segment const coal::Vec3s p1 = c1 - d1 / 2; const coal::Vec3s p2 = c2 - d2 / 2; const coal::Vec3s r = p1 - p2; Scalar a = d1.dot(d1); Scalar b = d1.dot(d2); Scalar c = d1.dot(r); Scalar e = d2.dot(d2); Scalar f = d2.dot(r); // S1 is parametrized by the equation p1 + s * d1 // S2 is parametrized by the equation p2 + t * d2 Vec3s w1, w2; if (a <= EPSILON) { w1 = p1; if (e <= EPSILON) // Check if the segments degenerate into points w2 = p2; else // First segment is degenerated clamped_linear(w2, p2, f, e, d2); } else if (e <= EPSILON) { // Second segment is degenerated clamped_linear(w1, p1, -c, a, d1); w2 = p2; } else { // Always non-negative, equal 0 if the segments are colinear Scalar denom = Scalar(fmax(a * e - b * b, 0)); Scalar s; Scalar t; if (denom > EPSILON) { s = clamp((b * f - c * e), denom); t = b * s + f; } else { s = 0.; t = f; } if (t <= 0.0) { w2 = p2; clamped_linear(w1, p1, -c, a, d1); } else if (t >= e) { clamped_linear(w1, p1, (b - c), a, d1); w2 = p2 + d2; } else { w1 = p1 + s * d1; w2 = p2 + t / e * d2; } } // witness points achieving the distance between the two segments Scalar distance = (w1 - w2).norm(); // capsule spcecific distance computation distance = distance - (radius1 + radius2); // Normal points from o1 to o2 normal = (w2 - w1).normalized(); wp1 = w1 + radius1 * normal; wp2 = w2 - radius2 * normal; return distance; } } // namespace internal } // namespace coal ================================================ FILE: src/distance/capsule_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Capsule& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const Scalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/capsule_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Capsule& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const Scalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/cone_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Cone& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const Scalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/cone_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Cone& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const Scalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/convex_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2020, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Joseph Mirabel */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { namespace internal { #define ConvexHalfspaceShapeShapeDistance(ConvexBaseType) \ template <> \ Scalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, \ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { \ COAL_TRACY_ZONE_SCOPED_N( \ "coal::internal::ShapeShapeDistance"); \ const ConvexBaseType& s1 = static_cast(*o1); \ const Halfspace& s2 = static_cast(*o2); \ const Scalar distance = \ details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); \ normal = -normal; \ return distance; \ } \ \ template <> \ Scalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, \ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { \ COAL_TRACY_ZONE_SCOPED_N( \ "coal::internal::ShapeShapeDistance"); \ const Halfspace& s1 = static_cast(*o1); \ const ConvexBaseType& s2 = static_cast(*o2); \ return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); \ } ConvexHalfspaceShapeShapeDistance(ConvexBase16); ConvexHalfspaceShapeShapeDistance(ConvexBase32); } // namespace internal } // namespace coal ================================================ FILE: src/distance/convex_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { namespace internal { #define ConvexPlaneShapeShapeDistance(ConvexBaseType) \ template <> \ Scalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, \ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { \ COAL_TRACY_ZONE_SCOPED_N( \ "coal::internal::ShapeShapeDistance"); \ const ConvexBaseType& s1 = static_cast(*o1); \ const Plane& s2 = static_cast(*o2); \ const Scalar distance = \ details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); \ normal = -normal; \ return distance; \ } \ \ template <> \ Scalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3s& tf1, \ const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, \ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { \ COAL_TRACY_ZONE_SCOPED_N( \ "coal::internal::ShapeShapeDistance"); \ const Plane& s1 = static_cast(*o1); \ const ConvexBaseType& s2 = static_cast(*o2); \ return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); \ } ConvexPlaneShapeShapeDistance(ConvexBase16); ConvexPlaneShapeShapeDistance(ConvexBase32); } // namespace internal } // namespace coal ================================================ FILE: src/distance/cylinder_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Cylinder& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const Scalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/cylinder_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Cylinder& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const Scalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/ellipsoid_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021-2022, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Ellipsoid& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const Scalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const Ellipsoid& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/ellipsoid_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Ellipsoid& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const Scalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const Ellipsoid& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/halfspace_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); return details::halfspaceHalfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/halfspace_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); return details::halfspacePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); Scalar distance = details::halfspacePlaneDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } } // namespace internal } // namespace coal ================================================ FILE: src/distance/plane_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); return details::planePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/sphere_capsule.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021-2022, CNRS * Author: Louis Montaut * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Sphere& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::sphereCapsuleDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Capsule& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); const Scalar distance = details::sphereCapsuleDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } } // namespace internal } // namespace coal ================================================ FILE: src/distance/sphere_cylinder.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Sphere& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::sphereCylinderDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Cylinder& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); const Scalar distance = details::sphereCylinderDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } } // namespace internal } // namespace coal ================================================ FILE: src/distance/sphere_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Sphere& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const Scalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/sphere_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Sphere& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const Scalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N("coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/sphere_sphere.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2018-2019, CNRS * Author: Florent Lamiraux * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "coal/internal/traversal_node_base.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" // Note that partial specialization of template functions is not allowed. // Therefore, two implementations with the default narrow phase solvers are // provided. If another narrow phase solver were to be used, the default // template implementation would be called, unless the function is also // specialized for this new type. // // One solution would be to make narrow phase solvers derive from an abstract // class and specialize the template for this abstract class. namespace coal { struct GJKSolver; namespace internal { template <> Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Sphere& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::sphereSphereDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/triangle_halfspace.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2020, Center National de la Recherche Scientifique * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Joseph Mirabel */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const TriangleP& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const Scalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Halfspace& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/triangle_plane.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const TriangleP& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const Scalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Plane& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/triangle_sphere.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const TriangleP& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); const Scalar distance = details::sphereTriangleDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); const Sphere& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::sphereTriangleDistance(s1, tf1, s2, tf2, p1, p2, normal); } } // namespace internal } // namespace coal ================================================ FILE: src/distance/triangle_triangle.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #include "coal/shape/geometric_shapes.h" #include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" #include "coal/tracy.hh" namespace coal { namespace internal { template <> Scalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* solver, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { COAL_TRACY_ZONE_SCOPED_N( "coal::internal::ShapeShapeDistance"); // Transform the triangles in world frame const TriangleP& s1 = static_cast(*o1); const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b), tf1.transform(s1.c)); const TriangleP& s2 = static_cast(*o2); const TriangleP t2(tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c)); // Reset GJK algorithm // We don't need to take into account swept-sphere radius in GJK iterations; // the result will be corrected after GJK terminates. solver->minkowski_difference .set<::coal::details::SupportOptions::NoSweptSphere>(&t1, &t2); solver->gjk.reset(solver->gjk_max_iterations, solver->gjk_tolerance); // Get GJK initial guess Vec3s guess; if (solver->gjk_initial_guess == GJKInitialGuess::CachedGuess || solver->enable_cached_guess) { guess = solver->cached_guess; } else { guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3; } support_func_guess_t support_hint; solver->epa.status = details::EPA::DidNotRun; // EPA is never called in this function Vec3ps guess_ = guess.cast(); details::GJK::Status gjk_status = solver->gjk.evaluate(solver->minkowski_difference, guess_, support_hint); solver->cached_guess = solver->gjk.getGuessFromSimplex().cast(); solver->support_func_cached_guess = solver->gjk.support_hint; // Retrieve witness points and normal Vec3ps p1_, p2_, normal_; solver->gjk.getWitnessPointsAndNormal(solver->minkowski_difference, p1_, p2_, normal_); p1 = p1_.cast(); p2 = p2_.cast(); normal = normal_.cast(); Scalar distance = Scalar(solver->gjk.distance); if (gjk_status == details::GJK::Collision) { Scalar penetrationDepth = details::computePenetration(t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal); distance = -penetrationDepth; } else { // No collision // TODO On degenerated case, the closest point may be wrong // (i.e. an object face normal is colinear to gjk.ray // assert (dist == (w0 - w1).norm()); assert(solver->gjk.ray.norm() > solver->gjk.getTolerance()); } // assert(false && "should not reach this point"); // return false; return distance; } } // namespace internal } // namespace coal ================================================ FILE: src/distance.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/distance.h" #include "coal/collision_utility.h" #include "coal/distance_func_matrix.h" #include "coal/narrowphase/narrowphase.h" #include "coal/tracy.hh" namespace coal { DistanceFunctionMatrix& getDistanceFunctionLookTable() { static DistanceFunctionMatrix table; return table; } Scalar distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result) { return distance(o1->collisionGeometryPtr(), o1->getTransform(), o2->collisionGeometryPtr(), o2->getTransform(), request, result); } Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { COAL_TRACY_ZONE_SCOPED_N("coal::distance"); GJKSolver solver(request); const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); OBJECT_TYPE object_type1 = o1->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type2 = o2->getNodeType(); Scalar res = (std::numeric_limits::max)(); if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.distance_matrix[node_type2][node_type1]) { COAL_THROW_PRETTY("Distance function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } else { res = looktable.distance_matrix[node_type2][node_type1]( o2, tf2, o1, tf1, &solver, request, result); std::swap(result.o1, result.o2); result.nearest_points[0].swap(result.nearest_points[1]); result.normal *= -1; } } else { if (!looktable.distance_matrix[node_type1][node_type2]) { COAL_THROW_PRETTY("Distance function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } else { res = looktable.distance_matrix[node_type1][node_type2]( o1, tf1, o2, tf2, &solver, request, result); } } // Cache narrow phase solver result. If the option in the request is selected, // also store the solver result in the request for the next call. result.cached_gjk_guess = solver.cached_guess; result.cached_support_func_guess = solver.support_func_cached_guess; request.updateGuess(result); return res; } ComputeDistance::ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2) : o1(o1), o2(o2) { const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); OBJECT_TYPE object_type1 = o1->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type2 = o2->getNodeType(); swap_geoms = object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD); if ((swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) || (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) { COAL_THROW_PRETTY("Distance function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } if (swap_geoms) func = looktable.distance_matrix[node_type2][node_type1]; else func = looktable.distance_matrix[node_type1][node_type2]; } Scalar ComputeDistance::run(const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) const { COAL_TRACY_ZONE_SCOPED_N("coal::ComputeDistance::run"); Scalar res; if (swap_geoms) { res = func(o2, tf2, o1, tf1, &solver, request, result); std::swap(result.o1, result.o2); result.nearest_points[0].swap(result.nearest_points[1]); result.normal *= -1; } else { res = func(o1, tf1, o2, tf2, &solver, request, result); } // Cache narrow phase solver result. If the option in the request is selected, // also store the solver result in the request for the next call. result.cached_gjk_guess = solver.cached_guess; result.cached_support_func_guess = solver.support_func_cached_guess; request.updateGuess(result); return res; } Scalar ComputeDistance::operator()(const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) const { solver.set(request); Scalar res; if (request.enable_timings) { Timer timer; res = run(tf1, tf2, request, result); result.timings = timer.elapsed(); } else res = run(tf1, tf2, request, result); return res; } } // namespace coal ================================================ FILE: src/distance_func_matrix.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/distance_func_matrix.h" #include <../src/collision_node.h> #include "coal/internal/shape_shape_func.h" #include "coal/internal/traversal_node_setup.h" #include "coal/internal/shape_shape_func.h" #include <../src/traits_traversal.h> namespace coal { #ifdef COAL_HAS_OCTOMAP template Scalar Distance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; typename TraversalTraitsDistance::CollisionTraversal_t node; const TypeA* obj1 = static_cast(o1); const TypeB* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); distance(&node); return result.min_distance; } #endif COAL_LOCAL Scalar distance_function_not_implemented( const CollisionGeometry* o1, const Transform3s& /*tf1*/, const CollisionGeometry* o2, const Transform3s& /*tf2*/, const GJKSolver* /*nsolver*/, const DistanceRequest& /*request*/, DistanceResult& /*result*/) { NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); COAL_THROW_PRETTY("Distance function between node type " << std::string(get_node_type_name(node_type1)) << " and node type " << std::string(get_node_type_name(node_type2)) << " is not yet supported.", std::invalid_argument); } template struct COAL_LOCAL BVHShapeDistancer { static Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; MeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3s tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); ::coal::distance(&node); delete obj1_tmp; return result.min_distance; } // namespace coal }; namespace details { template Scalar orientedBVHShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; OrientedMeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); ::coal::distance(&node); return result.min_distance; } } // namespace details template struct COAL_LOCAL BVHShapeDistancer { static Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodeRSS, RSS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); } }; template struct COAL_LOCAL BVHShapeDistancer { static Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodekIOS, kIOS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); } }; template struct COAL_LOCAL BVHShapeDistancer { static Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodeOBBRSS, OBBRSS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); } }; template struct COAL_LOCAL HeightFieldShapeDistancer { static Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { COAL_UNUSED_VARIABLE(o1); COAL_UNUSED_VARIABLE(tf1); COAL_UNUSED_VARIABLE(o2); COAL_UNUSED_VARIABLE(tf2); COAL_UNUSED_VARIABLE(nsolver); COAL_UNUSED_VARIABLE(request); // TODO(jcarpent) COAL_THROW_PRETTY( "Distance between a height field and a shape is not implemented", std::invalid_argument); // if(request.isSatisfied(result)) return result.min_distance; // HeightFieldShapeDistanceTraversalNode node; // // const HeightField* obj1 = static_cast* // >(o1); const T_SH* obj2 = static_cast(o2); // // initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); // fcl::distance(&node); return result.min_distance; } }; template Scalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; MeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3s tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); Transform3s tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); distance(&node); delete obj1_tmp; delete obj2_tmp; return result.min_distance; } namespace details { template Scalar orientedMeshDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; OrientedMeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); initialize(node, *obj1, tf1, *obj2, tf2, request, result); distance(&node); return result.min_distance; } } // namespace details template <> Scalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance( o1, tf1, o2, tf2, request, result); } template <> Scalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance( o1, tf1, o2, tf2, request, result); } template <> Scalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance( o1, tf1, o2, tf2, request, result); } template Scalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* /*nsolver*/, const DistanceRequest& request, DistanceResult& result) { return BVHDistance(o1, tf1, o2, tf2, request, result); } DistanceFunctionMatrix::DistanceFunctionMatrix() { for (int i = 0; i < NODE_COUNT; ++i) { for (int j = 0; j < NODE_COUNT; ++j) distance_matrix[i][j] = NULL; } // clang-format off distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX16] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX32] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeDistance; // clang-format on /* AABB distance not implemented */ /* distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; */ // clang-format off distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CONVEX16] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CONVEX32] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CONVEX16] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CONVEX32] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; // clang-format on /* KDOP distance not implemented */ /* distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer, Box>::distance; distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer, Sphere>::distance; distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule>::distance; distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer, Cone>::distance; distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder>::distance; distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer, ConvexBase>::distance; distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer, Plane>::distance; distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace>::distance; distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer, Box>::distance; distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer, Sphere>::distance; distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule>::distance; distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer, Cone>::distance; distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder>::distance; distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer, ConvexBase>::distance; distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer, Plane>::distance; distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace>::distance; distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer, Box>::distance; distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer, Sphere>::distance; distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule>::distance; distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer, Cone>::distance; distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder>::distance; distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer, ConvexBase>::distance; distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer, Plane>::distance; distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace>::distance; */ // clang-format off distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CONVEX16] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CONVEX32] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CONVEX16] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CONVEX32] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_CAPSULE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_CONE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_CYLINDER] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_CONVEX16] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_CONVEX32] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_ELLIPSOID] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_CAPSULE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_CONE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_CYLINDER] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_CONVEX16] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_CONVEX32] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_ELLIPSOID] = &HeightFieldShapeDistancer::distance; distance_matrix[BV_AABB][BV_AABB] = &BVHDistance; distance_matrix[BV_OBB][BV_OBB] = &BVHDistance; distance_matrix[BV_RSS][BV_RSS] = &BVHDistance; distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance; distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance; #ifdef COAL_HAS_OCTOMAP distance_matrix[GEOM_OCTREE][GEOM_BOX] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_CONE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_CONVEX16] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_CONVEX32] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &Distance; distance_matrix[GEOM_BOX][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_CONE][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_CONVEX16][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_CONVEX32][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_OCTREE][BV_AABB] = &Distance >; distance_matrix[GEOM_OCTREE][BV_OBB] = &Distance >; distance_matrix[GEOM_OCTREE][BV_RSS] = &Distance >; distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &Distance >; distance_matrix[GEOM_OCTREE][BV_kIOS] = &Distance >; distance_matrix[GEOM_OCTREE][BV_KDOP16] = &Distance > >; distance_matrix[GEOM_OCTREE][BV_KDOP18] = &Distance > >; distance_matrix[GEOM_OCTREE][BV_KDOP24] = &Distance > >; distance_matrix[BV_AABB][GEOM_OCTREE] = &Distance, OcTree>; distance_matrix[BV_OBB][GEOM_OCTREE] = &Distance, OcTree>; distance_matrix[BV_RSS][GEOM_OCTREE] = &Distance, OcTree>; distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &Distance, OcTree>; distance_matrix[BV_kIOS][GEOM_OCTREE] = &Distance, OcTree>; distance_matrix[BV_KDOP16][GEOM_OCTREE] = &Distance >, OcTree>; distance_matrix[BV_KDOP18][GEOM_OCTREE] = &Distance >, OcTree>; distance_matrix[BV_KDOP24][GEOM_OCTREE] = &Distance >, OcTree>; distance_matrix[GEOM_OCTREE][HF_AABB] = &distance_function_not_implemented; distance_matrix[GEOM_OCTREE][HF_OBBRSS] = &distance_function_not_implemented; distance_matrix[HF_AABB][GEOM_OCTREE] = &distance_function_not_implemented; distance_matrix[HF_OBBRSS][GEOM_OCTREE] = &distance_function_not_implemented; #endif // clang-format on } // template struct DistanceFunctionMatrix; } // namespace coal ================================================ FILE: src/hfield.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Justin Carpentier */ #include "coal/hfield.h" #include "coal/BV/BV.h" #include "coal/shape/convex.h" #include "coal/internal/BV_splitter.h" #include "coal/internal/BV_fitter.h" #include #include namespace coal { template <> NODE_TYPE HeightField::getNodeType() const { return HF_AABB; } template <> NODE_TYPE HeightField::getNodeType() const { return BV_UNKNOWN; // HF_OBB; } template <> NODE_TYPE HeightField::getNodeType() const { return BV_UNKNOWN; // HF_RSS; } template <> NODE_TYPE HeightField::getNodeType() const { return BV_UNKNOWN; // BV_kIOS; } template <> NODE_TYPE HeightField::getNodeType() const { return HF_OBBRSS; } template <> NODE_TYPE HeightField >::getNodeType() const { return BV_UNKNOWN; // BV_KDOP16; } template <> NODE_TYPE HeightField >::getNodeType() const { return BV_UNKNOWN; // BV_KDOP18; } template <> NODE_TYPE HeightField >::getNodeType() const { return BV_UNKNOWN; // BV_KDOP24; } // template class HeightField >; // template class HeightField >; // template class HeightField >; template class HeightField; template class HeightField; template class HeightField; // template class HeightField; template class HeightField; } // namespace coal ================================================ FILE: src/intersect.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/internal/intersect.h" #include "coal/internal/tools.h" #include #include #include #include namespace coal { bool Intersect::buildTrianglePlane(const Vec3s& v1, const Vec3s& v2, const Vec3s& v3, Vec3s* n, Scalar* t) { Vec3s n_ = (v2 - v1).cross(v3 - v1); Scalar norm2 = n_.squaredNorm(); if (norm2 > 0) { *n = n_ / sqrt(norm2); *t = n->dot(v1); return true; } return false; } void TriangleDistance::segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q, const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y) { Vec3s T; Scalar A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; Vec3s TMP; T = Q - P; A_dot_A = A.dot(A); B_dot_B = B.dot(B); A_dot_B = A.dot(B); A_dot_T = A.dot(T); B_dot_T = B.dot(T); // t parameterizes ray P,A // u parameterizes ray Q,B Scalar t, u; // compute t for the closest point on ray P,A to // ray Q,B Scalar denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B; t = (A_dot_T * B_dot_B - B_dot_T * A_dot_B) / denom; // clamp result so t is on the segment P,A if ((t < 0) || std::isnan(t)) t = 0; else if (t > 1) t = 1; // find u for point on ray Q,B closest to point at t u = (t * A_dot_B - B_dot_T) / B_dot_B; // if u is on segment Q,B, t and u correspond to // closest points, otherwise, clamp u, recompute and // clamp t if ((u <= 0) || std::isnan(u)) { Y = Q; t = A_dot_T / A_dot_A; if ((t <= 0) || std::isnan(t)) { X = P; VEC = Q - P; } else if (t >= 1) { X = P + A; VEC = Q - X; } else { X = P + A * t; TMP = T.cross(A); VEC = A.cross(TMP); } } else if (u >= 1) { Y = Q + B; t = (A_dot_B + A_dot_T) / A_dot_A; if ((t <= 0) || std::isnan(t)) { X = P; VEC = Y - P; } else if (t >= 1) { X = P + A; VEC = Y - X; } else { X = P + A * t; T = Y - P; TMP = T.cross(A); VEC = A.cross(TMP); } } else { Y = Q + B * u; if ((t <= 0) || std::isnan(t)) { X = P; TMP = T.cross(B); VEC = B.cross(TMP); } else if (t >= 1) { X = P + A; T = Q - X; TMP = T.cross(B); VEC = B.cross(TMP); } else { X = P + A * t; VEC = A.cross(B); if (VEC.dot(T) < 0) { VEC = VEC * (-1); } } } } Scalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P, Vec3s& Q) { // Compute vectors along the 6 sides Vec3s Sv[3]; Vec3s Tv[3]; Vec3s VEC; Sv[0] = S[1] - S[0]; Sv[1] = S[2] - S[1]; Sv[2] = S[0] - S[2]; Tv[0] = T[1] - T[0]; Tv[1] = T[2] - T[1]; Tv[2] = T[0] - T[2]; // For each edge pair, the vector connecting the closest points // of the edges defines a slab (parallel planes at head and tail // enclose the slab). If we can show that the off-edge vertex of // each triangle is outside of the slab, then the closest points // of the edges are the closest points for the triangles. // Even if these tests fail, it may be helpful to know the closest // points found, and whether the triangles were shown disjoint Vec3s V, Z, minP, minQ; Scalar mindd; int shown_disjoint = 0; mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { // Find closest points on edges i & j, plus the // vector (and distance squared) between these points segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); V = Q - P; Scalar dd = V.dot(V); // Verify this closest point pair only if the distance // squared is less than the minimum found thus far. if (dd <= mindd) { minP = P; minQ = Q; mindd = dd; Z = S[(i + 2) % 3] - P; Scalar a = Z.dot(VEC); Z = T[(j + 2) % 3] - Q; Scalar b = Z.dot(VEC); if ((a <= 0) && (b >= 0)) return dd; Scalar p = V.dot(VEC); if (a < 0) a = 0; if (b > 0) b = 0; if ((p - a + b) > 0) shown_disjoint = 1; } } } // No edge pairs contained the closest points. // either: // 1. one of the closest points is a vertex, and the // other point is interior to a face. // 2. the triangles are overlapping. // 3. an edge of one triangle is parallel to the other's face. If // cases 1 and 2 are not true, then the closest points from the 9 // edge pairs checks above can be taken as closest points for the // triangles. // 4. possibly, the triangles were degenerate. When the // triangle points are nearly colinear or coincident, one // of above tests might fail even though the edges tested // contain the closest points. // First check for case 1 Vec3s Sn; Scalar Snl; Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle Snl = Sn.dot(Sn); // Compute square of length of normal // If cross product is long enough, if (Snl > 1e-15) { // Get projection lengths of T points Vec3s Tp; V = S[0] - T[0]; Tp[0] = V.dot(Sn); V = S[0] - T[1]; Tp[1] = V.dot(Sn); V = S[0] - T[2]; Tp[2] = V.dot(Sn); // If Sn is a separating direction, // find point with smallest projection int point = -1; if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) { if (Tp[0] < Tp[1]) point = 0; else point = 1; if (Tp[2] < Tp[point]) point = 2; } else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) { if (Tp[0] > Tp[1]) point = 0; else point = 1; if (Tp[2] > Tp[point]) point = 2; } // If Sn is a separating direction, if (point >= 0) { shown_disjoint = 1; // Test whether the point found, when projected onto the // other triangle, lies within the face. V = T[point] - S[0]; Z = Sn.cross(Sv[0]); if (V.dot(Z) > 0) { V = T[point] - S[1]; Z = Sn.cross(Sv[1]); if (V.dot(Z) > 0) { V = T[point] - S[2]; Z = Sn.cross(Sv[2]); if (V.dot(Z) > 0) { // T[point] passed the test - it's a closest point for // the T triangle; the other point is on the face of S P = T[point] + Sn * (Tp[point] / Snl); Q = T[point]; return (P - Q).squaredNorm(); } } } } } Vec3s Tn; Scalar Tnl; Tn = Tv[0].cross(Tv[1]); Tnl = Tn.dot(Tn); if (Tnl > 1e-15) { Vec3s Sp; V = T[0] - S[0]; Sp[0] = V.dot(Tn); V = T[0] - S[1]; Sp[1] = V.dot(Tn); V = T[0] - S[2]; Sp[2] = V.dot(Tn); int point = -1; if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) { if (Sp[0] < Sp[1]) point = 0; else point = 1; if (Sp[2] < Sp[point]) point = 2; } else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) { if (Sp[0] > Sp[1]) point = 0; else point = 1; if (Sp[2] > Sp[point]) point = 2; } if (point >= 0) { shown_disjoint = 1; V = S[point] - T[0]; Z = Tn.cross(Tv[0]); if (V.dot(Z) > 0) { V = S[point] - T[1]; Z = Tn.cross(Tv[1]); if (V.dot(Z) > 0) { V = S[point] - T[2]; Z = Tn.cross(Tv[2]); if (V.dot(Z) > 0) { P = S[point]; Q = S[point] + Tn * (Sp[point] / Tnl); return (P - Q).squaredNorm(); } } } } } // Case 1 can't be shown. // If one of these tests showed the triangles disjoint, // we assume case 3 or 4, otherwise we conclude case 2, // that the triangles overlap. if (shown_disjoint) { P = minP; Q = minQ; return mindd; } else return 0; } Scalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, const Vec3s& S3, const Vec3s& T1, const Vec3s& T2, const Vec3s& T3, Vec3s& P, Vec3s& Q) { Vec3s S[3]; Vec3s T[3]; S[0] = S1; S[1] = S2; S[2] = S3; T[0] = T1; T[1] = T2; T[2] = T3; return sqrTriDistance(S, T, P, Q); } Scalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], const Matrix3s& R, const Vec3s& Tl, Vec3s& P, Vec3s& Q) { Vec3s T_transformed[3]; T_transformed[0] = R * T[0] + Tl; T_transformed[1] = R * T[1] + Tl; T_transformed[2] = R * T[2] + Tl; return sqrTriDistance(S, T_transformed, P, Q); } Scalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], const Transform3s& tf, Vec3s& P, Vec3s& Q) { Vec3s T_transformed[3]; T_transformed[0] = tf.transform(T[0]); T_transformed[1] = tf.transform(T[1]); T_transformed[2] = tf.transform(T[2]); return sqrTriDistance(S, T_transformed, P, Q); } Scalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, const Vec3s& S3, const Vec3s& T1, const Vec3s& T2, const Vec3s& T3, const Matrix3s& R, const Vec3s& Tl, Vec3s& P, Vec3s& Q) { Vec3s T1_transformed = R * T1 + Tl; Vec3s T2_transformed = R * T2 + Tl; Vec3s T3_transformed = R * T3 + Tl; return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); } Scalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, const Vec3s& S3, const Vec3s& T1, const Vec3s& T2, const Vec3s& T3, const Transform3s& tf, Vec3s& P, Vec3s& Q) { Vec3s T1_transformed = tf.transform(T1); Vec3s T2_transformed = tf.transform(T2); Vec3s T3_transformed = tf.transform(T3); return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); } } // namespace coal ================================================ FILE: src/math/transform.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/math/transform.h" namespace coal { void relativeTransform(const Transform3s& tf1, const Transform3s& tf2, Transform3s& tf) { tf = tf1.inverseTimes(tf2); } void relativeTransform2(const Transform3s& tf1, const Transform3s& tf2, Transform3s& tf) { Matrix3s R(tf2.getRotation() * tf1.getRotation().transpose()); tf = Transform3s(R, tf2.getTranslation() - R * tf1.getTranslation()); } } // namespace coal ================================================ FILE: src/mesh_loader/assimp.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2019-2021 CNRS - LAAS, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) #define nullptr NULL #endif #include "coal/mesh_loader/assimp.h" // Assimp >= 5.0 is forcing the use of C++11 keywords. A fix has been submitted // https://github.com/assimp/assimp/pull/2758. The next lines fixes the bug for // current version of Coal. #include #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) && \ defined(AI_NO_EXCEPT) #undef AI_NO_EXCEPT #define AI_NO_EXCEPT #endif #include #include #include #include #include #include namespace coal { namespace internal { Loader::Loader() : importer(new Assimp::Importer()) { // set list of ignored parameters (parameters used for rendering) importer->SetPropertyInteger( AI_CONFIG_PP_RVC_FLAGS, aiComponent_TANGENTS_AND_BITANGENTS | aiComponent_COLORS | aiComponent_BONEWEIGHTS | aiComponent_ANIMATIONS | aiComponent_LIGHTS | aiComponent_CAMERAS | aiComponent_TEXTURES | aiComponent_TEXCOORDS | aiComponent_MATERIALS | aiComponent_NORMALS); // remove LINES and POINTS importer->SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT); } Loader::~Loader() { if (importer) delete importer; } void Loader::load(const std::string& resource_path) { scene = importer->ReadFile( resource_path.c_str(), aiProcess_SortByPType | aiProcess_Triangulate | aiProcess_RemoveComponent | aiProcess_ImproveCacheLocality | aiProcess_FindDegenerates | aiProcess_JoinIdenticalVertices); if (!scene) { const std::string exception_message( std::string("Could not load resource ") + resource_path + std::string("\n") + importer->GetErrorString() + std::string("\n") + "Hint: the mesh directory may be wrong."); COAL_THROW_PRETTY(exception_message.c_str(), std::invalid_argument); } if (!scene->HasMeshes()) COAL_THROW_PRETTY( (std::string("No meshes found in file ") + resource_path).c_str(), std::invalid_argument); } /** * @brief Recursive procedure for building a mesh * * @param[in] scale Scale to apply when reading the ressource * @param[in] scene Pointer to the assimp scene * @param[in] node Current node of the scene * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ unsigned recurseBuildMesh(const coal::Vec3s& scale, const aiScene* scene, const aiNode* node, unsigned vertices_offset, TriangleAndVertices& tv) { if (!node) return 0; aiMatrix4x4 transform = node->mTransformation; aiNode* pnode = node->mParent; while (pnode) { // Don't convert to y-up orientation, which is what the root node in // Assimp does if (pnode->mParent != NULL) { transform = pnode->mTransformation * transform; } pnode = pnode->mParent; } unsigned nbVertices = 0; for (uint32_t i = 0; i < node->mNumMeshes; i++) { aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; // Add the vertices for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) { aiVector3D p = input_mesh->mVertices[j]; p *= transform; tv.vertices_.push_back( coal::Vec3s(p.x * scale[0], p.y * scale[1], p.z * scale[2])); } // add the indices for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) { aiFace& face = input_mesh->mFaces[j]; assert(face.mNumIndices == 3 && "The size of the face is not valid."); tv.triangles_.push_back( coal::Triangle32(vertices_offset + face.mIndices[0], vertices_offset + face.mIndices[1], vertices_offset + face.mIndices[2])); } nbVertices += input_mesh->mNumVertices; } for (uint32_t i = 0; i < node->mNumChildren; ++i) { nbVertices += recurseBuildMesh(scale, scene, node->mChildren[i], nbVertices, tv); } return nbVertices; } void buildMesh(const coal::Vec3s& scale, const aiScene* scene, unsigned vertices_offset, TriangleAndVertices& tv) { recurseBuildMesh(scale, scene, scene->mRootNode, vertices_offset, tv); } } // namespace internal } // namespace coal ================================================ FILE: src/mesh_loader/loader.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2016, CNRS - LAAS * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "coal/mesh_loader/loader.h" #include "coal/mesh_loader/assimp.h" #include #ifdef COAL_HAS_OCTOMAP #include "coal/octree.h" #endif #include "coal/BV/BV.h" namespace coal { bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const { const CachedMeshLoader::Key& a = *this; for (int i = 0; i < 3; ++i) { if (a.scale[i] < b.scale[i]) return true; else if (a.scale[i] > b.scale[i]) return false; } return std::less()(a.filename, b.filename); } template BVHModelPtr_t _load(const std::string& filename, const Vec3s& scale) { shared_ptr > polyhedron(new BVHModel); loadPolyhedronFromResource(filename, scale, polyhedron); return polyhedron; } BVHModelPtr_t MeshLoader::load(const std::string& filename, const Vec3s& scale) { switch (bvType_) { case BV_AABB: return _load(filename, scale); case BV_OBB: return _load(filename, scale); case BV_RSS: return _load(filename, scale); case BV_kIOS: return _load(filename, scale); case BV_OBBRSS: return _load(filename, scale); case BV_KDOP16: return _load >(filename, scale); case BV_KDOP18: return _load >(filename, scale); case BV_KDOP24: return _load >(filename, scale); default: COAL_THROW_PRETTY("Unhandled bouding volume type.", std::invalid_argument); } } CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) { #ifdef COAL_HAS_OCTOMAP shared_ptr octree(new octomap::OcTree(filename)); return CollisionGeometryPtr_t(new coal::OcTree(octree)); #else COAL_THROW_PRETTY("Coal compiled without OctoMap. Cannot create OcTrees.", std::logic_error); #endif } BVHModelPtr_t CachedMeshLoader::load(const std::string& filename, const Vec3s& scale) { Key key(filename, scale); std::time_t mtime = 0; try { mtime = boost::filesystem::last_write_time(filename); Cache_t::const_iterator _cached = cache_.find(key); if (_cached != cache_.end() && _cached->second.mtime == mtime) // File found in cache and mtime is the same return _cached->second.model; } catch (boost::filesystem::filesystem_error&) { // Could not stat. Make sure we will try to load the file so that // there will be a file not found error. } BVHModelPtr_t geom = MeshLoader::load(filename, scale); Value val; val.model = geom; val.mtime = mtime; cache_[key] = val; return geom; } } // namespace coal ================================================ FILE: src/narrowphase/details.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan, Florent Lamiraux */ #ifndef COAL_SRC_NARROWPHASE_DETAILS_H #define COAL_SRC_NARROWPHASE_DETAILS_H #include "coal/internal/traversal_node_setup.h" #include "coal/narrowphase/narrowphase.h" namespace coal { namespace details { // Compute the point on a line segment that is the closest point on the // segment to to another point. The code is inspired by the explanation // given by Dan Sunday's page: // http://geomalgorithms.com/a02-_lines.html static inline void lineSegmentPointClosestToPoint(const Vec3s& p, const Vec3s& s1, const Vec3s& s2, Vec3s& sp) { Vec3s v = s2 - s1; Vec3s w = p - s1; Scalar c1 = w.dot(v); Scalar c2 = v.dot(v); if (c1 <= 0) { sp = s1; } else if (c2 <= c1) { sp = s2; } else { Scalar b = c1 / c2; Vec3s Pb = s1 + v * b; sp = Pb; } } /// @param p1 witness point on the Sphere. /// @param p2 witness point on the Capsule. /// @param normal pointing from shape 1 to shape 2 (sphere to capsule). /// @return the distance between the two shapes (negative if penetration). inline Scalar sphereCapsuleDistance(const Sphere& s1, const Transform3s& tf1, const Capsule& s2, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { Vec3s pos1(tf2.transform(Vec3s(0., 0., s2.halfLength))); Vec3s pos2(tf2.transform(Vec3s(0., 0., -s2.halfLength))); Vec3s s_c = tf1.getTranslation(); Vec3s segment_point; lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point); normal = segment_point - s_c; Scalar norm(normal.norm()); Scalar r1 = s1.radius + s1.getSweptSphereRadius(); Scalar r2 = s2.radius + s2.getSweptSphereRadius(); Scalar dist = norm - r1 - r2; static const Scalar eps(std::numeric_limits::epsilon()); if (norm > eps) { normal.normalize(); } else { normal << 1, 0, 0; } p1 = s_c + normal * r1; p2 = segment_point - normal * r2; return dist; } /// @param p1 witness point on the Sphere. /// @param p2 witness point on the Cylinder. /// @param normal pointing from shape 1 to shape 2 (sphere to cylinder). /// @return the distance between the two shapes (negative if penetration). inline Scalar sphereCylinderDistance(const Sphere& s1, const Transform3s& tf1, const Cylinder& s2, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { static const Scalar eps(sqrt(std::numeric_limits::epsilon())); Scalar r1(s1.radius); Scalar r2(s2.radius); Scalar lz2(s2.halfLength); // boundaries of the cylinder axis Vec3s A(tf2.transform(Vec3s(0, 0, -lz2))); Vec3s B(tf2.transform(Vec3s(0, 0, lz2))); // Position of the center of the sphere Vec3s S(tf1.getTranslation()); // axis of the cylinder Vec3s u(tf2.getRotation().col(2)); /// @todo a tiny performance improvement could be achieved using the abscissa /// with S as the origin assert((B - A - (s2.halfLength * 2) * u).norm() < eps); Vec3s AS(S - A); // abscissa of S on cylinder axis with A as the origin Scalar s(u.dot(AS)); Vec3s P(A + s * u); Vec3s PS(S - P); Scalar dPS = PS.norm(); // Normal to cylinder axis such that plane (A, u, v) contains sphere // center Vec3s v(0, 0, 0); Scalar dist; if (dPS > eps) { // S is not on cylinder axis v = (1 / dPS) * PS; } if (s <= 0) { if (dPS <= r2) { // closest point on cylinder is on cylinder disc basis dist = -s - r1; p1 = S + r1 * u; p2 = A + dPS * v; normal = u; } else { // closest point on cylinder is on cylinder circle basis p2 = A + r2 * v; Vec3s Sp2(p2 - S); Scalar dSp2 = Sp2.norm(); if (dSp2 > eps) { normal = (1 / dSp2) * Sp2; p1 = S + r1 * normal; dist = dSp2 - r1; assert(fabs(dist) - (p1 - p2).norm() < eps); } else { // Center of sphere is on cylinder boundary normal = p2 - .5 * (A + B); assert(u.dot(normal) >= 0); normal.normalize(); dist = -r1; p1 = S + r1 * normal; } } } else if (s <= (s2.halfLength * 2)) { // 0 < s <= s2.lz normal = -v; dist = dPS - r1 - r2; p2 = P + r2 * v; p1 = S - r1 * v; } else { // lz < s if (dPS <= r2) { // closest point on cylinder is on cylinder disc basis dist = s - (s2.halfLength * 2) - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u; } else { // closest point on cylinder is on cylinder circle basis p2 = B + r2 * v; Vec3s Sp2(p2 - S); Scalar dSp2 = Sp2.norm(); if (dSp2 > eps) { normal = (1 / dSp2) * Sp2; p1 = S + r1 * normal; dist = dSp2 - r1; assert(fabs(dist) - (p1 - p2).norm() < eps); } else { // Center of sphere is on cylinder boundary normal = p2 - .5 * (A + B); normal.normalize(); p1 = S + r1 * normal; dist = -r1; } } } // Take swept-sphere radius into account const Scalar ssr1 = s1.getSweptSphereRadius(); const Scalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; dist -= (ssr1 + ssr2); } return dist; } /// @param p1 witness point on the first Sphere. /// @param p2 witness point on the second Sphere. /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2). /// @return the distance between the two spheres (negative if penetration). inline Scalar sphereSphereDistance(const Sphere& s1, const Transform3s& tf1, const Sphere& s2, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const coal::Vec3s& center1 = tf1.getTranslation(); const coal::Vec3s& center2 = tf2.getTranslation(); Scalar r1 = (s1.radius + s1.getSweptSphereRadius()); Scalar r2 = (s2.radius + s2.getSweptSphereRadius()); Vec3s c1c2 = center2 - center1; Scalar cdist = c1c2.norm(); Vec3s unit(1, 0, 0); if (cdist > Eigen::NumTraits::epsilon()) unit = c1c2 / cdist; Scalar dist = cdist - r1 - r2; normal = unit; p1.noalias() = center1 + r1 * unit; p2.noalias() = center2 - r2 * unit; return dist; } /** @brief the minimum distance from a point to a line */ inline Scalar segmentSqrDistance(const Vec3s& from, const Vec3s& to, const Vec3s& p, Vec3s& nearest) { Vec3s diff = p - from; Vec3s v = to - from; Scalar t = v.dot(diff); if (t > 0) { Scalar dotVV = v.squaredNorm(); if (t < dotVV) { t /= dotVV; diff -= v * t; } else { t = 1; diff -= v; } } else t = 0; nearest.noalias() = from + v * t; return diff.squaredNorm(); } /// @brief Whether a point's projection is in a triangle inline bool projectInTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3, const Vec3s& normal, const Vec3s& p) { Vec3s edge1(p2 - p1); Vec3s edge2(p3 - p2); Vec3s edge3(p1 - p3); Vec3s p1_to_p(p - p1); Vec3s p2_to_p(p - p2); Vec3s p3_to_p(p - p3); Vec3s edge1_normal(edge1.cross(normal)); Vec3s edge2_normal(edge2.cross(normal)); Vec3s edge3_normal(edge3.cross(normal)); Scalar r1, r2, r3; r1 = edge1_normal.dot(p1_to_p); r2 = edge2_normal.dot(p2_to_p); r3 = edge3_normal.dot(p3_to_p); if ((r1 > 0 && r2 > 0 && r3 > 0) || (r1 <= 0 && r2 <= 0 && r3 <= 0)) { return true; } return false; } /// @param p1 witness point on the first Sphere. /// @param p2 witness point on the second Sphere. /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2). /// @return the distance between the two shapes (negative if penetration). inline Scalar sphereTriangleDistance(const Sphere& s, const Transform3s& tf1, const TriangleP& tri, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Vec3s& P1 = tf2.transform(tri.a); const Vec3s& P2 = tf2.transform(tri.b); const Vec3s& P3 = tf2.transform(tri.c); Vec3s tri_normal = (P2 - P1).cross(P3 - P1); tri_normal.normalize(); const Vec3s& center = tf1.getTranslation(); // Note: comparing an object with a swept-sphere radius of r1 against another // object with a swept-sphere radius of r2 is equivalent to comparing the // first object with a swept-sphere radius of r1 + r2 against the second // object with a swept-sphere radius of 0. const Scalar& radius = s.radius + s.getSweptSphereRadius() + tri.getSweptSphereRadius(); assert(radius >= 0); assert(s.radius >= 0); Vec3s p1_to_center = center - P1; Scalar distance_from_plane = p1_to_center.dot(tri_normal); Vec3s closest_point( Vec3s::Constant(std::numeric_limits::quiet_NaN())); Scalar min_distance_sqr, distance_sqr; if (distance_from_plane < 0) { distance_from_plane *= -1; tri_normal *= -1; } if (projectInTriangle(P1, P2, P3, tri_normal, center)) { closest_point = center - tri_normal * distance_from_plane; min_distance_sqr = distance_from_plane * distance_from_plane; } else { // Compute distance to each edge and take minimal distance Vec3s nearest_on_edge; min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point); distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); if (distance_sqr < min_distance_sqr) { min_distance_sqr = distance_sqr; closest_point = nearest_on_edge; } distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge); if (distance_sqr < min_distance_sqr) { min_distance_sqr = distance_sqr; closest_point = nearest_on_edge; } } normal = (closest_point - center).normalized(); p1 = center + normal * (s.radius + s.getSweptSphereRadius()); p2 = closest_point - normal * tri.getSweptSphereRadius(); const Scalar distance = std::sqrt(min_distance_sqr) - radius; return distance; } /// @param p1 closest (or most penetrating) point on the Halfspace, /// @param p2 closest (or most penetrating) point on the shape, /// @param normal the halfspace normal. /// @return the distance between the two shapes (negative if penetration). inline Scalar halfspaceDistance(const Halfspace& h, const Transform3s& tf1, const ShapeBase& s, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { // TODO(louis): handle multiple contact points when the halfspace normal is // parallel to the shape's surface (every primitive except sphere and // ellipsoid). // Express halfspace in world frame Halfspace new_h = transform(h, tf1); // Express halfspace normal in shape frame Vec3s n_2(tf2.getRotation().transpose() * new_h.n); // Compute support of shape in direction of halfspace normal int hint = 0; p2.noalias() = getSupport(&s, -n_2, hint); p2 = tf2.transform(p2); const Scalar dist = new_h.signedDistance(p2); p1.noalias() = p2 - dist * new_h.n; normal.noalias() = new_h.n; const Scalar dummy_precision = std::sqrt(Eigen::NumTraits::dummy_precision()); COAL_UNUSED_VARIABLE(dummy_precision); assert(new_h.distance(p1) <= dummy_precision); return dist; } /// @param p1 closest (or most penetrating) point on the Plane, /// @param p2 closest (or most penetrating) point on the shape, /// @param normal the halfspace normal. /// @return the distance between the two shapes (negative if penetration). inline Scalar planeDistance(const Plane& plane, const Transform3s& tf1, const ShapeBase& s, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { // TODO(louis): handle multiple contact points when the plane normal is // parallel to the shape's surface (every primitive except sphere and // ellipsoid). // Express plane as two halfspaces in world frame std::array new_h = transformToHalfspaces(plane, tf1); // Express halfspace normals in shape frame Vec3s n_h1(tf2.getRotation().transpose() * new_h[0].n); Vec3s n_h2(tf2.getRotation().transpose() * new_h[1].n); // Compute support of shape in direction of halfspace normal and its opposite int hint = 0; Vec3s p2h1 = getSupport(&s, -n_h1, hint); p2h1 = tf2.transform(p2h1); hint = 0; Vec3s p2h2 = getSupport(&s, -n_h2, hint); p2h2 = tf2.transform(p2h2); Scalar dist1 = new_h[0].signedDistance(p2h1); Scalar dist2 = new_h[1].signedDistance(p2h2); const Scalar dummy_precision = std::sqrt(Eigen::NumTraits::dummy_precision()); COAL_UNUSED_VARIABLE(dummy_precision); Scalar dist; if (dist1 >= dist2) { dist = dist1; p2.noalias() = p2h1; p1.noalias() = p2 - dist * new_h[0].n; normal.noalias() = new_h[0].n; assert(new_h[0].distance(p1) <= dummy_precision); } else { dist = dist2; p2.noalias() = p2h2; p1.noalias() = p2 - dist * new_h[1].n; normal.noalias() = new_h[1].n; assert(new_h[1].distance(p1) <= dummy_precision); } return dist; } /// Taken from book Real Time Collision Detection, from Christer Ericson /// @param pb the witness point on the box surface /// @param ps the witness point on the sphere. /// @param normal pointing from box to sphere /// @return the distance between the two shapes (negative if penetration). inline Scalar boxSphereDistance(const Box& b, const Transform3s& tfb, const Sphere& s, const Transform3s& tfs, Vec3s& pb, Vec3s& ps, Vec3s& normal) { const Vec3s& os = tfs.getTranslation(); const Vec3s& ob = tfb.getTranslation(); const Matrix3s& Rb = tfb.getRotation(); pb = ob; bool outside = false; const Vec3s os_in_b_frame(Rb.transpose() * (os - ob)); int axis = -1; Scalar min_d = (std::numeric_limits::max)(); for (int i = 0; i < 3; ++i) { Scalar facedist; if (os_in_b_frame(i) < -b.halfSide(i)) { // outside pb.noalias() -= b.halfSide(i) * Rb.col(i); outside = true; } else if (os_in_b_frame(i) > b.halfSide(i)) { // outside pb.noalias() += b.halfSide(i) * Rb.col(i); outside = true; } else { pb.noalias() += os_in_b_frame(i) * Rb.col(i); if (!outside && (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) { axis = i; min_d = facedist; } } } normal = pb - os; Scalar pdist = normal.norm(); Scalar dist; // distance between sphere and box if (outside) { // pb is on the box dist = pdist - s.radius; normal /= -pdist; } else { // pb is inside the box if (os_in_b_frame(axis) >= 0) { normal = Rb.col(axis); } else { normal = -Rb.col(axis); } dist = -min_d - s.radius; } ps = os - s.radius * normal; if (!outside || dist <= 0) { // project point pb onto the box's surface pb = ps - dist * normal; } // Take swept-sphere radius into account const Scalar ssrb = b.getSweptSphereRadius(); const Scalar ssrs = s.getSweptSphereRadius(); if (ssrb > 0 || ssrs > 0) { pb += ssrb * normal; ps -= ssrs * normal; dist -= (ssrb + ssrs); } return dist; } /// @brief return distance between two halfspaces /// @param p1 the witness point on the first halfspace. /// @param p2 the witness point on the second halfspace. /// @param normal pointing from first to second halfspace. /// @return the distance between the two shapes (negative if penetration). /// /// @note If the two halfspaces don't have the same normal (or opposed /// normals), they collide and their distance is set to -infinity as there is no /// translation that can separate them; they have infinite penetration depth. /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. inline Scalar halfspaceHalfspaceDistance(const Halfspace& s1, const Transform3s& tf1, const Halfspace& s2, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { Halfspace new_s1 = transform(s1, tf1); Halfspace new_s2 = transform(s2, tf2); Scalar distance; Vec3s dir = (new_s1.n).cross(new_s2.n); Scalar dir_sq_norm = dir.squaredNorm(); if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { if (new_s1.n.dot(new_s2.n) > 0) { // If the two halfspaces have the same normal, one is inside the other // and they can't be separated. They have inifinte penetration depth. distance = -(std::numeric_limits::max)(); if (new_s1.d <= new_s2.d) { normal = new_s1.n; p1 = normal * distance; p2 = new_s2.n * new_s2.d; assert(new_s2.distance(p2) <= Eigen::NumTraits::dummy_precision()); } else { normal = -new_s1.n; p1 << new_s1.n * new_s1.d; p2 = -(normal * distance); assert(new_s1.distance(p1) <= Eigen::NumTraits::dummy_precision()); } } else { distance = -(new_s1.d + new_s2.d); normal = new_s1.n; p1 = new_s1.n * new_s1.d; p2 = new_s2.n * new_s2.d; } } else { // If the halfspaces are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. normal = dir; p1 = p2 = ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm; // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection // and https://en.wikipedia.org/wiki/Cross_product } // Take swept-sphere radius into account const Scalar ssr1 = s1.getSweptSphereRadius(); const Scalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; distance -= (ssr1 + ssr2); } return distance; } /// @brief return distance between plane and halfspace. /// @param p1 the witness point on the halfspace. /// @param p2 the witness point on the plane. /// @param normal pointing from halfspace to plane. /// @return the distance between the two shapes (negative if penetration). /// /// @note If plane and halfspace don't have the same normal (or opposed /// normals), they collide and their distance is set to -infinity as there is no /// translation that can separate them; they have infinite penetration depth. /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. inline Scalar halfspacePlaneDistance(const Halfspace& s1, const Transform3s& tf1, const Plane& s2, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { Halfspace new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); Scalar distance; Vec3s dir = (new_s1.n).cross(new_s2.n); Scalar dir_sq_norm = dir.squaredNorm(); if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { normal = new_s1.n; distance = new_s1.n.dot(new_s2.n) > 0 ? (new_s2.d - new_s1.d) : -(new_s1.d + new_s2.d); p1 = new_s1.n * new_s1.d; p2 = new_s2.n * new_s2.d; assert(new_s1.distance(p1) <= Eigen::NumTraits::dummy_precision()); assert(new_s2.distance(p2) <= Eigen::NumTraits::dummy_precision()); } else { // If the halfspace and plane are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. normal = dir; p1 = p2 = ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm; // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection // and https://en.wikipedia.org/wiki/Cross_product } // Take swept-sphere radius into account const Scalar ssr1 = s1.getSweptSphereRadius(); const Scalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; distance -= (ssr1 + ssr2); } return distance; } /// @brief return distance between two planes /// @param p1 the witness point on the first plane. /// @param p2 the witness point on the second plane. /// @param normal pointing from first to second plane. /// @return the distance between the two shapes (negative if penetration). /// /// @note If the two planes don't have the same normal (or opposed /// normals), they collide and their distance is set to -infinity as there is no /// translation that can separate them; they have infinite penetration depth. /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. inline Scalar planePlaneDistance(const Plane& s1, const Transform3s& tf1, const Plane& s2, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { Plane new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); Scalar distance; Vec3s dir = (new_s1.n).cross(new_s2.n); Scalar dir_sq_norm = dir.squaredNorm(); if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { p1 = new_s1.n * new_s1.d; p2 = new_s2.n * new_s2.d; assert(new_s1.distance(p1) <= Eigen::NumTraits::dummy_precision()); assert(new_s2.distance(p2) <= Eigen::NumTraits::dummy_precision()); distance = (p1 - p2).norm(); if (distance > Eigen::NumTraits::dummy_precision()) { normal = (p2 - p1).normalized(); } else { normal = new_s1.n; } } else { // If the planes are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. normal = dir; p1 = p2 = ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm; // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection // and https://en.wikipedia.org/wiki/Cross_product } // Take swept-sphere radius into account const Scalar ssr1 = s1.getSweptSphereRadius(); const Scalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; distance -= (ssr1 + ssr2); } return distance; } /// See the prototype below inline Scalar computePenetration(const Vec3s& P1, const Vec3s& P2, const Vec3s& P3, const Vec3s& Q1, const Vec3s& Q2, const Vec3s& Q3, Vec3s& normal) { Vec3s u((P2 - P1).cross(P3 - P1)); normal = u.normalized(); Scalar depth1((P1 - Q1).dot(normal)); Scalar depth2((P1 - Q2).dot(normal)); Scalar depth3((P1 - Q3).dot(normal)); return std::max(depth1, std::max(depth2, depth3)); } // Compute penetration distance and normal of two triangles in collision // Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the // minimal distance (Q1, Q2, Q3) should be translated along the normal so // that the triangles are collision free. // // Note that we compute here an upper bound of the penetration distance, // not the exact value. inline Scalar computePenetration(const Vec3s& P1, const Vec3s& P2, const Vec3s& P3, const Vec3s& Q1, const Vec3s& Q2, const Vec3s& Q3, const Transform3s& tf1, const Transform3s& tf2, Vec3s& normal) { Vec3s globalP1(tf1.transform(P1)); Vec3s globalP2(tf1.transform(P2)); Vec3s globalP3(tf1.transform(P3)); Vec3s globalQ1(tf2.transform(Q1)); Vec3s globalQ2(tf2.transform(Q2)); Vec3s globalQ3(tf2.transform(Q3)); return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2, globalQ3, normal); } } // namespace details } // namespace coal #endif // COAL_SRC_NARROWPHASE_DETAILS_H ================================================ FILE: src/narrowphase/gjk.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/shape/geometric_shapes.h" #include "coal/narrowphase/gjk.h" #include "coal/internal/intersect.h" #include "coal/internal/tools.h" #include "coal/shape/geometric_shapes_traits.h" #include "coal/narrowphase/narrowphase_defaults.h" #include "coal/tracy.hh" namespace coal { namespace details { void GJK::initialize() { distance_upper_bound = (std::numeric_limits::max)(); gjk_variant = GJKVariant::DefaultGJK; convergence_criterion = GJKConvergenceCriterion::Default; convergence_criterion_type = GJKConvergenceCriterionType::Relative; reset(max_iterations, tolerance); } void GJK::reset(size_t max_iterations_, SolverScalar tolerance_) { max_iterations = max_iterations_; tolerance = tolerance_; COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", std::invalid_argument); status = DidNotRun; nfree = 0; simplex = nullptr; iterations = 0; iterations_momentum_stop = 0; } Vec3ps GJK::getGuessFromSimplex() const { return ray; } namespace details { // This function computes the weights associated with projecting the origin // onto the final simplex of GJK or EPA. // The origin is always a convex combination of the supports of the simplex. // The weights are then linearly distributed among the shapes' supports, thanks // to the following property: // if s is a support of the Minkowski difference, then: // w = w.w0 - w.w1, with w.w0 a support of shape0 and w.w1 a support of // shape1. // clang-format off // Suppose the final simplex is of rank 2: // We have 0 = alpha * w[0] + (1 - alpha) * w[1], with alpha the weight of the convex // decomposition, then: // 0 = alpha * (w[0].w0 - w[0].w1) + (1 - alpha) * (w[1].w0 - w[1].w1) // => 0 = alpha * w[0].w0 + (1 - alpha) * w[1].w0 // - alpha * w[0].w1 - (1 - alpha) * w[1].w1 // Therefore we have two witness points: // w0 = alpha * w[0].w0 + (1 - alpha) * w[1].w0 // w1 = alpha * w[0].w1 + (1 - alpha) * w[1].w1 // clang-format on // TODO void getClosestPoints(const GJK::Simplex& simplex, Vec3ps& w0, Vec3ps& w1) { GJK::SimplexV* const* vs = simplex.vertex; for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) { assert(vs[i]->w.isApprox(vs[i]->w0 - vs[i]->w1)); } Project::ProjectResult projection; switch (simplex.rank) { case 1: w0 = vs[0]->w0; w1 = vs[0]->w1; return; case 2: { const Vec3ps &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w, b0 = vs[1]->w0, b1 = vs[1]->w1; SolverScalar la, lb; Vec3ps N(b - a); la = N.dot(-a); if (la <= 0) { assert(false); w0 = a0; w1 = a1; } else { lb = N.squaredNorm(); if (la > lb) { assert(false); w0 = b0; w1 = b1; } else { lb = la / lb; la = 1 - lb; w0 = la * a0 + lb * b0; w1 = la * a1 + lb * b1; } } } return; case 3: // TODO avoid the reprojection projection = Project::projectTriangleOrigin( vs[0]->w, vs[1]->w, vs[2]->w); break; case 4: // We are in collision. projection = Project::projectTetrahedraOrigin( vs[0]->w, vs[1]->w, vs[2]->w, vs[3]->w); break; default: COAL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]", std::logic_error); } w0.setZero(); w1.setZero(); for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) { w0 += projection.parameterization[i] * vs[i]->w0; w1 += projection.parameterization[i] * vs[i]->w1; } return; } /// Inflate the points along a normal. /// The normal is typically the normal of the separating plane found by GJK /// or the normal found by EPA. /// The normal should follow coal convention: it points from shape0 to /// shape1. template void inflate(const MinkowskiDiff& shape, const Vec3ps& normal, Vec3ps& w0, Vec3ps& w1) { #ifndef NDEBUG const SolverScalar dummy_precision = Eigen::NumTraits::dummy_precision(); assert((normal.norm() - 1) < dummy_precision); #endif const Eigen::Array& I( shape.swept_sphere_radius.cast()); Eigen::Array inflate(I > 0); if (!inflate.any()) return; if (inflate[0]) w0 += I[0] * normal; if (inflate[1]) w1 -= I[1] * normal; } } // namespace details void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0, Vec3ps& w1, Vec3ps& normal) const { details::getClosestPoints(*simplex, w0, w1); if ((w1 - w0).norm() > Eigen::NumTraits::dummy_precision()) { normal = (w1 - w0).normalized(); } else { normal = -this->ray.normalized(); } details::inflate(shape, normal, w0, w1); } GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3ps& guess, const support_func_guess_t& supportHint) { COAL_TRACY_ZONE_SCOPED_N("coal::details::GJK::evaluate"); SolverScalar alpha = 0; iterations = 0; const SolverScalar swept_sphere_radius = shape_.swept_sphere_radius.sum(); const SolverScalar upper_bound = distance_upper_bound + swept_sphere_radius; free_v[0] = &store_v[0]; free_v[1] = &store_v[1]; free_v[2] = &store_v[2]; free_v[3] = &store_v[3]; nfree = 4; status = NoCollision; shape = &shape_; distance = 0.0; current = 0; simplices[current].rank = 0; support_hint = supportHint; SolverScalar rl = guess.norm(); if (rl < tolerance) { ray = Vec3ps(-1, 0, 0); rl = 1; } else ray = guess; // Momentum GJKVariant current_gjk_variant = gjk_variant; Vec3ps w = ray; Vec3ps dir = ray; Vec3ps y; SolverScalar momentum; bool normalize_support_direction = shape->normalize_support_direction; do { vertex_id_t next = (vertex_id_t)(1 - current); Simplex& curr_simplex = simplices[current]; Simplex& next_simplex = simplices[next]; // check A: when origin is near the existing simplex, stop if (rl < tolerance) // mean origin is near the face of original simplex, // return touch { // At this point, GJK has converged but we don't know if GJK is enough to // recover penetration information. // EPA needs to be run. // Unless the Minkowski difference is degenerated, EPA will run fine even // if the final simplex of GJK is not a tetrahedron. assert(rl > 0); status = Collision; // GJK is not enough to recover the penetration depth, hence we ignore the // swept-sphere radius for now. // EPA needs to be run to recover the penetration depth. distance = rl; break; } // Compute direction for support call switch (current_gjk_variant) { case DefaultGJK: dir = ray; break; case NesterovAcceleration: // Normalize heuristic for collision pairs involving convex but not // strictly-convex shapes This corresponds to most use cases. if (normalize_support_direction) { momentum = (SolverScalar(iterations) + 2) / (SolverScalar(iterations) + 3); y = momentum * ray + (1 - momentum) * w; SolverScalar y_norm = y.norm(); // ray is the point of the Minkowski difference which currently the // closest to the origin. Therefore, y.norm() > ray.norm() Hence, if // check A above has not stopped the algorithm, we necessarily have // y.norm() > tolerance. The following assert is just a safety check. assert(y_norm > tolerance); dir = momentum * dir / dir.norm() + (1 - momentum) * y / y_norm; } else { momentum = (SolverScalar(iterations) + 1) / (SolverScalar(iterations) + 3); y = momentum * ray + (1 - momentum) * w; dir = momentum * dir + (1 - momentum) * y; } break; case PolyakAcceleration: momentum = 1 / (SolverScalar(iterations) + 1); dir = momentum * dir + (1 - momentum) * ray; break; default: COAL_THROW_PRETTY("Invalid momentum variant.", std::logic_error); } // see below, ray points away from origin appendVertex(curr_simplex, -dir, support_hint); // check removed (by ?): when the new support point is close to previous // support points, stop (as the new simplex is degenerated) w = curr_simplex.vertex[curr_simplex.rank - 1]->w; // check B: no collision if omega > 0 SolverScalar omega = dir.dot(w) / dir.norm(); if (omega > upper_bound) { distance = omega - swept_sphere_radius; status = NoCollisionEarlyStopped; break; } // Check to remove acceleration if (current_gjk_variant != DefaultGJK) { SolverScalar frank_wolfe_duality_gap = 2 * ray.dot(ray - w); if (frank_wolfe_duality_gap - tolerance <= 0) { removeVertex(simplices[current]); current_gjk_variant = DefaultGJK; // move back to classic GJK iterations_momentum_stop = iterations; continue; // continue to next iteration } } // check C: when the new support point is close to the sub-simplex where the // ray point lies, stop (as the new simplex again is degenerated) bool cv_check_passed = checkConvergence(w, rl, alpha, omega); if (iterations > 0 && cv_check_passed) { if (iterations > 0) removeVertex(simplices[current]); if (current_gjk_variant != DefaultGJK) { current_gjk_variant = DefaultGJK; // move back to classic GJK iterations_momentum_stop = iterations; continue; } // At this point, GJK has converged and we know that rl > tolerance (see // check above). Therefore, penetration information can always be // recovered without running EPA. distance = rl - swept_sphere_radius; if (distance < tolerance) { status = CollisionWithPenetrationInformation; } else { status = NoCollision; } break; } // This has been rewritten thanks to the excellent video: // https://youtu.be/Qupqu1xe7Io bool inside; switch (curr_simplex.rank) { case 1: // Only at the first iteration assert(iterations == 0); ray = w; inside = false; next_simplex.rank = 1; next_simplex.vertex[0] = curr_simplex.vertex[0]; break; case 2: inside = projectLineOrigin(curr_simplex, next_simplex); break; case 3: inside = projectTriangleOrigin(curr_simplex, next_simplex); break; case 4: inside = projectTetrahedraOrigin(curr_simplex, next_simplex); break; default: COAL_THROW_PRETTY("Invalid simplex rank", std::logic_error); } assert(nfree + next_simplex.rank == 4); current = next; rl = ray.norm(); if (inside || rl == 0) { status = Collision; // GJK is not enough to recover the penetration depth, hence we ignore the // swept-sphere radius for now. // EPA needs to be run to recover the penetration depth. distance = rl; break; } status = ((++iterations) < max_iterations) ? status : Failed; } while (status == NoCollision); simplex = &simplices[current]; assert(simplex->rank > 0 && simplex->rank < 5); return status; } bool GJK::checkConvergence(const Vec3ps& w, const SolverScalar& rl, SolverScalar& alpha, const SolverScalar& omega) const { // x^* is the optimal solution (projection of origin onto the Minkowski // difference). // x^k is the current iterate (x^k = `ray` in the code). // Each criterion provides a different guarantee on the distance to the // optimal solution. switch (convergence_criterion) { case Default: { // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^*|| - ||x^k|| <= diff const SolverScalar diff = rl - alpha; return ((diff - (tolerance + tolerance * rl)) <= 0); } break; case DualityGap: { // ||x^* - x^k||^2 <= diff const SolverScalar diff = 2 * ray.dot(ray - w); switch (convergence_criterion_type) { case Absolute: return ((diff - tolerance) <= 0); break; case Relative: return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: COAL_THROW_PRETTY("Invalid convergence criterion type.", std::logic_error); } } break; case Hybrid: { // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^* - x^k||^2 <= diff const SolverScalar diff = rl * rl - alpha * alpha; switch (convergence_criterion_type) { case Absolute: return ((diff - tolerance) <= 0); break; case Relative: return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: COAL_THROW_PRETTY("Invalid convergence criterion type.", std::logic_error); } } break; default: COAL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error); } } inline void GJK::removeVertex(Simplex& simplex) { free_v[nfree++] = simplex.vertex[--simplex.rank]; } inline void GJK::appendVertex(Simplex& simplex, const Vec3ps& v, support_func_guess_t& hint) { simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory getSupport(v, *simplex.vertex[simplex.rank++], hint); } bool GJK::encloseOrigin() { Vec3ps axis(Vec3ps::Zero()); support_func_guess_t hint = support_func_guess_t::Zero(); switch (simplex->rank) { case 1: for (int i = 0; i < 3; ++i) { axis[i] = 1; appendVertex(*simplex, axis, hint); if (encloseOrigin()) return true; removeVertex(*simplex); axis[i] = -1; appendVertex(*simplex, -axis, hint); if (encloseOrigin()) return true; removeVertex(*simplex); axis[i] = 0; } break; case 2: { Vec3ps d = simplex->vertex[1]->w - simplex->vertex[0]->w; for (int i = 0; i < 3; ++i) { axis[i] = 1; Vec3ps p = d.cross(axis); if (!p.isZero()) { appendVertex(*simplex, p, hint); if (encloseOrigin()) return true; removeVertex(*simplex); appendVertex(*simplex, -p, hint); if (encloseOrigin()) return true; removeVertex(*simplex); } axis[i] = 0; } } break; case 3: axis.noalias() = (simplex->vertex[1]->w - simplex->vertex[0]->w) .cross(simplex->vertex[2]->w - simplex->vertex[0]->w); if (!axis.isZero()) { appendVertex(*simplex, axis, hint); if (encloseOrigin()) return true; removeVertex(*simplex); appendVertex(*simplex, -axis, hint); if (encloseOrigin()) return true; removeVertex(*simplex); } break; case 4: if (std::abs(triple(simplex->vertex[0]->w - simplex->vertex[3]->w, simplex->vertex[1]->w - simplex->vertex[3]->w, simplex->vertex[2]->w - simplex->vertex[3]->w)) > 0) return true; break; } return false; } inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a, const Vec3ps& A, GJK::Simplex& next, Vec3ps& ray) { // A is the closest to the origin ray = A; next.vertex[0] = current.vertex[a]; next.rank = 1; } inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a, GJK::vertex_id_t b, const Vec3ps& A, const Vec3ps& B, const Vec3ps& AB, const SolverScalar& ABdotAO, GJK::Simplex& next, Vec3ps& ray) { // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B ray = AB.dot(B) * A + ABdotAO * B; next.vertex[0] = current.vertex[b]; next.vertex[1] = current.vertex[a]; next.rank = 2; // To ensure backward compatibility ray /= AB.squaredNorm(); } inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a, GJK::vertex_id_t b, GJK::vertex_id_t c, const Vec3ps& ABC, const SolverScalar& ABCdotAO, GJK::Simplex& next, Vec3ps& ray) { next.rank = 3; next.vertex[2] = current.vertex[a]; if (ABCdotAO == 0) { next.vertex[0] = current.vertex[c]; next.vertex[1] = current.vertex[b]; ray.setZero(); return true; } if (ABCdotAO > 0) { // Above triangle next.vertex[0] = current.vertex[c]; next.vertex[1] = current.vertex[b]; } else { next.vertex[0] = current.vertex[b]; next.vertex[1] = current.vertex[c]; } // To ensure backward compatibility ray = -ABCdotAO / ABC.squaredNorm() * ABC; return false; } bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) { const vertex_id_t a = 1, b = 0; // A is the last point we added. const Vec3ps& A = current.vertex[a]->w; const Vec3ps& B = current.vertex[b]->w; const Vec3ps AB = B - A; const SolverScalar d = AB.dot(-A); assert(d <= AB.squaredNorm()); if (d == 0) { // Two extremely unlikely cases: // - AB is orthogonal to A: should never happen because it means the support // function did not do any progress and GJK should have stopped. // - A == origin // In any case, A is the closest to the origin originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; return A.isZero(); } else if (d < 0) { // A is the closest to the origin originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; } else originToSegment(current, a, b, A, B, AB, d, next, ray); return false; } bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { const vertex_id_t a = 2, b = 1, c = 0; // A is the last point we added. const Vec3ps &A = current.vertex[a]->w, B = current.vertex[b]->w, C = current.vertex[c]->w; const Vec3ps AB = B - A, AC = C - A, ABC = AB.cross(AC); SolverScalar edgeAC2o = ABC.cross(AC).dot(-A); if (edgeAC2o >= 0) { SolverScalar towardsC = AC.dot(-A); if (towardsC >= 0) { // Region 1 originToSegment(current, a, c, A, C, AC, towardsC, next, ray); free_v[nfree++] = current.vertex[b]; } else { // Region 4 or 5 SolverScalar towardsB = AB.dot(-A); if (towardsB < 0) { // Region 5 // A is the closest to the origin originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; } else // Region 4 originToSegment(current, a, b, A, B, AB, towardsB, next, ray); free_v[nfree++] = current.vertex[c]; } } else { SolverScalar edgeAB2o = AB.cross(ABC).dot(-A); if (edgeAB2o >= 0) { // Region 4 or 5 SolverScalar towardsB = AB.dot(-A); if (towardsB < 0) { // Region 5 // A is the closest to the origin originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; } else // Region 4 originToSegment(current, a, b, A, B, AB, towardsB, next, ray); free_v[nfree++] = current.vertex[c]; } else { return originToTriangle(current, a, b, c, ABC, ABC.dot(-A), next, ray); } } return false; } bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { // The code of this function was generated using doc/gjk.py const vertex_id_t a = 3, b = 2, c = 1, d = 0; const Vec3ps& A(current.vertex[a]->w); const Vec3ps& B(current.vertex[b]->w); const Vec3ps& C(current.vertex[c]->w); const Vec3ps& D(current.vertex[d]->w); const SolverScalar aa = A.squaredNorm(); const SolverScalar da = D.dot(A); const SolverScalar db = D.dot(B); const SolverScalar dc = D.dot(C); const SolverScalar dd = D.dot(D); const SolverScalar da_aa = da - aa; const SolverScalar ca = C.dot(A); const SolverScalar cb = C.dot(B); const SolverScalar cc = C.dot(C); const SolverScalar& cd = dc; const SolverScalar ca_aa = ca - aa; const SolverScalar ba = B.dot(A); const SolverScalar bb = B.dot(B); const SolverScalar& bc = cb; const SolverScalar& bd = db; const SolverScalar ba_aa = ba - aa; const SolverScalar ba_ca = ba - ca; const SolverScalar ca_da = ca - da; const SolverScalar da_ba = da - ba; const Vec3ps a_cross_b = A.cross(B); const Vec3ps a_cross_c = A.cross(C); const SolverScalar dummy_precision( 3 * std::sqrt(std::numeric_limits::epsilon())); COAL_UNUSED_VARIABLE(dummy_precision); #define REGION_INSIDE() \ ray.setZero(); \ next.vertex[0] = current.vertex[d]; \ next.vertex[1] = current.vertex[c]; \ next.vertex[2] = current.vertex[b]; \ next.vertex[3] = current.vertex[a]; \ next.rank = 4; \ return true; // clang-format off if (ba_aa <= 0) { // if AB.AO >= 0 / a10 if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / a10.a3 if (ba * da_ba + bd * ba_aa - bb * da_aa <= 0) { // if (ADB ^ AB).AO >= 0 / a10.a3.a9 if (da_aa <= 0) { // if AD.AO >= 0 / a10.a3.a9.a12 assert(da * da_ba + dd * ba_aa - db * da_aa <= dummy_precision); // (ADB ^ AD).AO >= 0 / a10.a3.a9.a12.a8 if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.a4 // Region ABC originToTriangle(current, a, b, c, (B - A).cross(C - A), -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.!a4 // Region AB originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray); free_v[nfree++] = current.vertex[c]; free_v[nfree++] = current.vertex[d]; } // end of (ABC ^ AB).AO >= 0 } else { // not AD.AO >= 0 / a10.a3.a9.!a12 if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.a4 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5 if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.a6 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } else { // not (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.!a6 // Region AC originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; } // end of (ACD ^ AC).AO >= 0 } else { // not (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.!a5 // Region ABC originToTriangle(current, a, b, c, (B - A).cross(C - A), -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; } // end of (ABC ^ AC).AO >= 0 } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.!a4 // Region AB originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray); free_v[nfree++] = current.vertex[c]; free_v[nfree++] = current.vertex[d]; } // end of (ABC ^ AB).AO >= 0 } // end of AD.AO >= 0 } else { // not (ADB ^ AB).AO >= 0 / a10.a3.!a9 if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0 / a10.a3.!a9.a8 // Region ADB originToTriangle(current, a, d, b, (D - A).cross(B - A), D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; } else { // not (ADB ^ AD).AO >= 0 / a10.a3.!a9.!a8 if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.a6 if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.a7 // Region AD originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.!a7 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } // end of (ACD ^ AD).AO >= 0 } else { // not (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.!a6 if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.a7 // Region AD originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.!a7 // Region AC originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; } // end of (ACD ^ AD).AO >= 0 } // end of (ACD ^ AC).AO >= 0 } // end of (ADB ^ AD).AO >= 0 } // end of (ADB ^ AB).AO >= 0 } else { // not ADB.AO >= 0 / a10.!a3 if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / a10.!a3.a1 if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0 / a10.!a3.a1.a4 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.a5 if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.a6 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.!a6 // Region AC originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; } // end of (ACD ^ AC).AO >= 0 } else { // not (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.!a5 // Region ABC originToTriangle(current, a, b, c, (B - A).cross(C - A), -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; } // end of (ABC ^ AC).AO >= 0 } else { // not (ABC ^ AB).AO >= 0 / a10.!a3.a1.!a4 // Region AB originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray); free_v[nfree++] = current.vertex[c]; free_v[nfree++] = current.vertex[d]; } // end of (ABC ^ AB).AO >= 0 } else { // not ABC.AO >= 0 / a10.!a3.!a1 if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / a10.!a3.!a1.a2 if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.a6 if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.a7 // Region AD originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; } else { // not (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.!a7 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } // end of (ACD ^ AD).AO >= 0 } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.!a6 if (ca_aa <= 0) { // if AC.AO >= 0 / a10.!a3.!a1.a2.!a6.a11 // Region AC originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; } else { // not AC.AO >= 0 / a10.!a3.!a1.a2.!a6.!a11 // Region AD originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; } // end of AC.AO >= 0 } // end of (ACD ^ AC).AO >= 0 } else { // not ACD.AO >= 0 / a10.!a3.!a1.!a2 // Region Inside REGION_INSIDE() } // end of ACD.AO >= 0 } // end of ABC.AO >= 0 } // end of ADB.AO >= 0 } else { // not AB.AO >= 0 / !a10 if (ca_aa <= 0) { // if AC.AO >= 0 / !a10.a11 if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.a11.a2 if (da_aa <= 0) { // if AD.AO >= 0 / !a10.a11.a2.a12 if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.a6 if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7 if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.a8 // Region ADB originToTriangle(current, a, d, b, (D - A).cross(B - A), D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.!a8 // Region AD originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; } // end of (ADB ^ AD).AO >= 0 } else { // not (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.!a7 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } // end of (ACD ^ AD).AO >= 0 } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6 assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= -dummy_precision)); // Not (ACD ^ AD).AO >= 0 / // !a10.a11.a2.a12.!a6.!a7 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.a5 // Region AC originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.!a5 // Region ABC originToTriangle(current, a, b, c, (B - A).cross(C - A), -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; } // end of (ABC ^ AC).AO >= 0 } // end of (ACD ^ AC).AO >= 0 } else { // not AD.AO >= 0 / !a10.a11.a2.!a12 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5 if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.a6 assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= -dummy_precision)); // Not (ACD ^ AD).AO >= 0 / // !a10.a11.a2.!a12.a5.a6.!a7 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.!a6 // Region AC originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; } // end of (ACD ^ AC).AO >= 0 } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.!a5 if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.a1 assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <= dummy_precision); // (ABC ^ AB).AO >= 0 / // !a10.a11.a2.!a12.!a5.a1.a4 // Region ABC originToTriangle(current, a, b, c, (B - A).cross(C - A), -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; } else { // not ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.!a1 assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= -dummy_precision)); // Not (ACD ^ AD).AO >= 0 / // !a10.a11.a2.!a12.!a5.!a1.!a7 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } // end of ABC.AO >= 0 } // end of (ABC ^ AC).AO >= 0 } // end of AD.AO >= 0 } else { // not ACD.AO >= 0 / !a10.a11.!a2 if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.a11.!a2.a1 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.a5 // Region AC originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.!a5 assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <= dummy_precision); // (ABC ^ AB).AO >= 0 / // !a10.a11.!a2.a1.!a5.a4 // Region ABC originToTriangle(current, a, b, c, (B - A).cross(C - A), -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; } // end of (ABC ^ AC).AO >= 0 } else { // not ABC.AO >= 0 / !a10.a11.!a2.!a1 if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.a11.!a2.!a1.a3 if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.a8 // Region ADB originToTriangle(current, a, d, b, (D - A).cross(B - A), D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.!a8 // Region AD originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; } // end of (ADB ^ AD).AO >= 0 } else { // not ADB.AO >= 0 / !a10.a11.!a2.!a1.!a3 // Region Inside REGION_INSIDE() } // end of ADB.AO >= 0 } // end of ABC.AO >= 0 } // end of ACD.AO >= 0 } else { // not AC.AO >= 0 / !a10.!a11 if (da_aa <= 0) { // if AD.AO >= 0 / !a10.!a11.a12 if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.!a11.a12.a3 if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7 if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.a8 assert(!(ba * da_ba + bd * ba_aa - bb * da_aa <= -dummy_precision)); // Not (ADB ^ AB).AO >= 0 / // !a10.!a11.a12.a3.a7.a8.!a9 // Region ADB originToTriangle(current, a, d, b, (D - A).cross(B - A), D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; } else { // not (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.!a8 // Region AD originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; } // end of (ADB ^ AD).AO >= 0 } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.!a7 if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.a2 assert(ca * ca_da + cc * da_aa - cd * ca_aa <= dummy_precision); // (ACD ^ AC).AO >= 0 / // !a10.!a11.a12.a3.!a7.a2.a6 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } else { // not ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2 if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.a1 assert(!(ba * ba_ca + bb * ca_aa - bc * ba_aa <= -dummy_precision)); // Not (ABC ^ AB).AO >= 0 / // !a10.!a11.a12.a3.!a7.!a2.a1.!a4 // Region ADB originToTriangle(current, a, d, b, (D - A).cross(B - A), D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; } else { // not ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.!a1 // Region ADB originToTriangle(current, a, d, b, (D - A).cross(B - A), D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; } // end of ABC.AO >= 0 } // end of ACD.AO >= 0 } // end of (ACD ^ AD).AO >= 0 } else { // not ADB.AO >= 0 / !a10.!a11.a12.!a3 if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.!a11.a12.!a3.a2 if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.a7 // Region AD originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.!a7 assert(ca * ca_da + cc * da_aa - cd * ca_aa <= dummy_precision); // (ACD ^ AC).AO >= 0 / // !a10.!a11.a12.!a3.a2.!a7.a6 // Region ACD originToTriangle(current, a, c, d, (C - A).cross(D - A), -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; } // end of (ACD ^ AD).AO >= 0 } else { // not ACD.AO >= 0 / !a10.!a11.a12.!a3.!a2 // Region Inside REGION_INSIDE() } // end of ACD.AO >= 0 } // end of ADB.AO >= 0 } else { // not AD.AO >= 0 / !a10.!a11.!a12 // Region A originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; free_v[nfree++] = current.vertex[d]; } // end of AD.AO >= 0 } // end of AC.AO >= 0 } // end of AB.AO >= 0 // clang-format on #undef REGION_INSIDE return false; } void EPA::initialize() { reset(max_iterations, tolerance); } void EPA::reset(size_t max_iterations_, SolverScalar tolerance_) { max_iterations = max_iterations_; tolerance = tolerance_; // EPA creates only 2 faces and 1 vertex per iteration. // (+ the 4 (or 8 in the future) faces at the beginning // + the 4 vertices (or 6 in the future) at the beginning) sv_store.resize(max_iterations + 4); fc_store.resize(2 * max_iterations + 4); status = DidNotRun; normal.setZero(); support_hint.setZero(); depth = 0; closest_face = nullptr; result.reset(); hull.reset(); num_vertices = 0; stock.reset(); // The stock is initialized with the faces in reverse order so that the // hull and the stock do not overlap (the stock will shring as the hull will // grow). for (size_t i = 0; i < fc_store.size(); ++i) stock.append(&fc_store[fc_store.size() - i - 1]); iterations = 0; } bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a, const SimplexVertex& b, SolverScalar& dist) { Vec3ps ab = b.w - a.w; Vec3ps n_ab = ab.cross(face->n); SolverScalar a_dot_nab = a.w.dot(n_ab); if (a_dot_nab < 0) // the origin is on the outside part of ab { // following is similar to projectOrigin for two points // however, as we dont need to compute the parameterization, dont need to // compute 0 or 1 SolverScalar a_dot_ab = a.w.dot(ab); SolverScalar b_dot_ab = b.w.dot(ab); if (a_dot_ab > 0) dist = a.w.norm(); else if (b_dot_ab < 0) dist = b.w.norm(); else { dist = std::sqrt( std::max(a.w.squaredNorm() - a_dot_ab * a_dot_ab / ab.squaredNorm(), SolverScalar(0))); } return true; } return false; } EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c, bool force) { if (stock.root != nullptr) { SimplexFace* face = stock.root; stock.remove(face); hull.append(face); face->pass = 0; face->vertex_id[0] = id_a; face->vertex_id[1] = id_b; face->vertex_id[2] = id_c; const SimplexVertex& a = sv_store[id_a]; const SimplexVertex& b = sv_store[id_b]; const SimplexVertex& c = sv_store[id_c]; face->n = (b.w - a.w).cross(c.w - a.w); if (face->n.norm() > Eigen::NumTraits::epsilon()) { face->n.normalize(); // If the origin projects outside the face, skip it in the // `findClosestFace` method. // The origin always projects inside the closest face. SolverScalar a_dot_nab = a.w.dot((b.w - a.w).cross(face->n)); SolverScalar b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n)); SolverScalar c_dot_nca = c.w.dot((a.w - c.w).cross(face->n)); if (a_dot_nab >= -tolerance && // b_dot_nbc >= -tolerance && // c_dot_nca >= -tolerance) { face->d = a.w.dot(face->n); face->ignore = false; } else { // We will never check this face, so we don't care about // its true distance to the origin. face->d = std::numeric_limits::max(); face->ignore = true; } // For the initialization of EPA, we need to force the addition of the // face. This is because the origin can lie outside the initial // tetrahedron. This is expected. GJK can converge in two different ways: // either by enclosing the origin with a simplex, or by converging // sufficiently close to the origin. // The thing is, "sufficiently close to the origin" depends on the // tolerance of GJK. So in this case, GJK **cannot** guarantee that the // shapes are indeed in collision. If it turns out they are not in // collision, the origin will lie outside of the Minkowski difference and // thus outside the initial tetrahedron. But EPA is ultimately a // projection algorithm, so it will work fine! // // Actually, the `NonConvex` status is badly named. There should not be // such a status! Again, if the origin lies outside the Minkowski // difference, EPA will work fine, and will find the right (signed) // distance between the shapes as well as the right normal. This is a // very nice mathematical property, making GJK + EPA a **very** robust set // of algorithms. :) // TODO(louis): remove the `NonConvex` status. if (face->d >= -tolerance || force) return face; else status = NonConvex; } else status = Degenerated; hull.remove(face); stock.append(face); return nullptr; } assert(hull.count >= fc_store.size() && "EPA: should not be out of faces."); status = OutOfFaces; return nullptr; } /** @brief Find the best polytope face to split */ EPA::SimplexFace* EPA::findClosestFace() { SimplexFace* minf = hull.root; SolverScalar mind = std::numeric_limits::max(); for (SimplexFace* f = minf; f; f = f->next_face) { if (f->ignore) continue; SolverScalar sqd = f->d * f->d; if (sqd < mind) { minf = f; mind = sqd; } } assert(minf && !(minf->ignore) && "EPA: minf should not be flagged ignored."); return minf; } EPA::Status EPA::evaluate(GJK& gjk, const Vec3ps& guess) { COAL_TRACY_ZONE_SCOPED_N("coal::details::EPA::evaluate"); GJK::Simplex& simplex = *gjk.getSimplex(); support_hint = gjk.support_hint; // TODO(louis): we might want to start with a hexahedron if the // simplex given by GJK is of rank <= 3. bool enclosed_origin = gjk.encloseOrigin(); if ((simplex.rank > 1) && enclosed_origin) { assert(simplex.rank == 4 && "When starting EPA, simplex should be of rank 4."); while (hull.root) { SimplexFace* f = hull.root; hull.remove(f); stock.append(f); } assert(hull.count == 0); assert(stock.count == fc_store.size()); status = Valid; num_vertices = 0; // Make sure the tetrahedron has its normals pointing outside. if ((simplex.vertex[0]->w - simplex.vertex[3]->w) .dot((simplex.vertex[1]->w - simplex.vertex[3]->w) .cross(simplex.vertex[2]->w - simplex.vertex[3]->w)) < 0) { SimplexVertex* tmp = simplex.vertex[0]; simplex.vertex[0] = simplex.vertex[1]; simplex.vertex[1] = tmp; } // Add the 4 vertices to sv_store for (size_t i = 0; i < 4; ++i) { sv_store[num_vertices++] = *simplex.vertex[i]; } SimplexFace* tetrahedron[] = {newFace(0, 1, 2, true), // newFace(1, 0, 3, true), // newFace(2, 1, 3, true), // newFace(0, 2, 3, true)}; if (hull.count == 4) { // set the face connectivity bind(tetrahedron[0], 0, tetrahedron[1], 0); bind(tetrahedron[0], 1, tetrahedron[2], 0); bind(tetrahedron[0], 2, tetrahedron[3], 0); bind(tetrahedron[1], 1, tetrahedron[3], 2); bind(tetrahedron[1], 2, tetrahedron[2], 1); bind(tetrahedron[2], 2, tetrahedron[3], 1); closest_face = findClosestFace(); // find the best face (the face with the // minimum distance to origin) to split SimplexFace outer = *closest_face; status = Valid; iterations = 0; size_t pass = 0; for (; iterations < max_iterations; ++iterations) { if (num_vertices >= sv_store.size()) { status = OutOfVertices; break; } // Step 1: find the support point in the direction of the closest_face // normal. // -------------------------------------------------------------------------- SimplexHorizon horizon; SimplexVertex& w = sv_store[num_vertices++]; bool valid = true; closest_face->pass = ++pass; // At the moment, SimplexF.n is always normalized. This could be revised // in the future... gjk.getSupport(closest_face->n, w, support_hint); // Step 2: check for convergence. // ------------------------------ // Preambule to understand the convergence criterion of EPA: // the support we just added is in the direction of the normal of // the closest_face. Therefore, the support point will **always** // lie "after" the closest_face, i.e closest_face.n.dot(w.w) > 0. if (iterations > 0) { assert(closest_face->n.dot(w.w) > -tolerance && "The support is not in the right direction."); } // // 1) First check: `fdist` (see below) is an upper bound of how much // more penetration depth we can expect to "gain" by adding `w` to EPA's // polytope. This first check, as any convergence check, should be both // absolute and relative. This allows to adapt the tolerance to the // scale of the objects. const SimplexVertex& vf1 = sv_store[closest_face->vertex_id[0]]; const SimplexVertex& vf2 = sv_store[closest_face->vertex_id[1]]; const SimplexVertex& vf3 = sv_store[closest_face->vertex_id[2]]; SolverScalar fdist = closest_face->n.dot(w.w - vf1.w); SolverScalar wnorm = w.w.norm(); // TODO(louis): we might want to use tol_abs and tol_rel; this might // obfuscate the code for the user though. if (fdist <= tolerance + tolerance * wnorm) { status = AccuracyReached; break; } // 2) Second check: the expand function **assumes** that the support we // just computed is not a vertex of the face. We make sure that this // is the case: // TODO(louis): should we use squaredNorm everywhere instead of norm? if ((w.w - vf1.w).norm() <= tolerance + tolerance * wnorm || (w.w - vf2.w).norm() <= tolerance + tolerance * wnorm || (w.w - vf3.w).norm() <= tolerance + tolerance * wnorm) { status = AccuracyReached; break; } // Step 3: expand the polytope // --------------------------- for (size_t j = 0; (j < 3) && valid; ++j) valid &= expand(pass, w, closest_face->adjacent_faces[j], closest_face->adjacent_edge[j], horizon); if (!valid || horizon.num_faces < 3) { // The status has already been set by the expand function. assert(!(status & Valid)); break; } // need to add the edge connectivity between first and last faces bind(horizon.first_face, 2, horizon.current_face, 1); hull.remove(closest_face); stock.append(closest_face); closest_face = findClosestFace(); outer = *closest_face; } status = ((iterations) < max_iterations) ? status : Failed; normal = outer.n; depth = outer.d + gjk.shape->swept_sphere_radius.sum(); result.rank = 3; result.vertex[0] = &sv_store[outer.vertex_id[0]]; result.vertex[1] = &sv_store[outer.vertex_id[1]]; result.vertex[2] = &sv_store[outer.vertex_id[2]]; return status; } assert(false && "The tetrahedron with which EPA started is degenerated."); } // FallBack when the simplex given by GJK is of rank 1. // Since the simplex only contains support points which convex // combination describe the origin, the point in the simplex is actually // the origin. status = FallBack; // TODO: define a better normal assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance())); normal = -guess; SolverScalar nl = normal.norm(); if (nl > 0) normal /= nl; else normal = Vec3ps(1, 0, 0); depth = 0; result.rank = 1; result.vertex[0] = simplex.vertex[0]; return status; } // Use this function to debug `EPA::expand` if needed. // void EPA::PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w) { // std::cout << "Vertices:\n"; // for (size_t i = 0; i < num_vertices; ++i) { // std::cout << "["; // std::cout << sv_store[i].w(0) << ", "; // std::cout << sv_store[i].w(1) << ", "; // std::cout << sv_store[i].w(2) << "]\n"; // } // // // std::cout << "\nTriangles:\n"; // SimplexFace* face = hull.root; // for (size_t i = 0; i < hull.count; ++i) { // std::cout << "["; // std::cout << face->vertex_id[0] << ", "; // std::cout << face->vertex_id[1] << ", "; // std::cout << face->vertex_id[2] << "]\n"; // face = face->next_face; // } // // // std::cout << "\nNormals:\n"; // face = hull.root; // for (size_t i = 0; i < hull.count; ++i) { // std::cout << "["; // std::cout << face->n(0) << ", "; // std::cout << face->n(1) << ", "; // std::cout << face->n(2) << "]\n"; // face = face->next_face; // } // // // std::cout << "\nClosest face:\n"; // face = hull.root; // for (size_t i = 0; i < hull.count; ++i) { // if (face == closest_face) { // std::cout << i << "\n"; // } // face = face->next_face; // } // std::cout << "\nSupport point:\n"; // std::cout << "[" << w.w(0) << ", " << w.w(1) << ", " << w.w(2) << "]\n"; // } /** @brief the goal is to add a face connecting vertex w and face edge f[e] */ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, SimplexHorizon& horizon) { static const size_t nexti[] = {1, 2, 0}; static const size_t previ[] = {2, 0, 1}; const size_t id_w = num_vertices - 1; // w is always the last vertex added to sv_store // Check if we loop through expand indefinitely. if (f->pass == pass) { // Uncomment the following line and the associated EPA method // to debug the infinite loop if needed. // EPAPrintExpandLooping(this, f); assert(f != closest_face && "EPA is looping indefinitely."); status = InvalidHull; return false; } const size_t e1 = nexti[e]; // Preambule: when expanding the polytope, the `closest_face` is always // deleted. This is handled in EPA::evaluate after calling the expand // function. This function handles how the neighboring face `f` of the // `closest_face` is connected to the new support point. (Note: because // `expand` is recursive, `f` can also denote a face of a face of the // `closest_face`, and so on. But the reasoning is the same.) // // EPA can handle `f` in two ways, depending on where the new support point // is located: // 1) If it is "below" `f`, then `f` is preserved. A new face is created // and connects to the edge `e` of `f`. This new face is made of the // two points of the edge `e` of `f` and the new support point `w`. // Geometrically, this corresponds to the case where the projection of // the origin on the `closest_face` is **inside** the triangle defined by // the `closest_face`. // 2) If it is "above" `f`, then `f` has to be deleted, simply because the // edge `e` of `f` is not part of the convex hull anymore. // The two faces adjacent to `f` are thus expanded following // either 1) or 2). // Geometrically, this corresponds to the case where the projection of // the origin on the `closest_face` is on an edge of the triangle defined // by the `closest_face`. The projection of the origin cannot lie on a // vertex of the `closest_face` because EPA should have exited before // reaching this point. // // The following checks for these two cases. // This check is however subject to numerical precision and due to the // recursive nature of `expand`, it is safer to go through the first case. // This is because `expand` can potentially loop indefinitly if the // Minkowski difference is very flat (hence the check above). const SolverScalar dummy_precision( 3 * std::sqrt(std::numeric_limits::epsilon())); const SimplexVertex& vf = sv_store[f->vertex_id[e]]; if (f->n.dot(w.w - vf.w) < dummy_precision) { // case 1: the support point is "below" `f`. SimplexFace* new_face = newFace(f->vertex_id[e1], f->vertex_id[e], id_w); if (new_face != nullptr) { // add face-face connectivity bind(new_face, 0, f, e); // if there is last face in the horizon, then need to add another // connectivity, i.e. the edge connecting the current new add edge and the // last new add edge. This does not finish all the connectivities because // the final face need to connect with the first face, this will be // handled in the evaluate function. Notice the face is anti-clockwise, so // the edges are 0 (bottom), 1 (right), 2 (left) if (horizon.current_face != nullptr) { bind(new_face, 2, horizon.current_face, 1); } else { horizon.first_face = new_face; } horizon.current_face = new_face; ++horizon.num_faces; return true; } return false; } // case 2: the support point is "above" `f`. const size_t e2 = previ[e]; f->pass = pass; if (expand(pass, w, f->adjacent_faces[e1], f->adjacent_edge[e1], horizon) && expand(pass, w, f->adjacent_faces[e2], f->adjacent_edge[e2], horizon)) { hull.remove(f); stock.append(f); return true; } return false; } void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0, Vec3ps& w1, Vec3ps& normal) const { details::getClosestPoints(result, w0, w1); if ((w0 - w1).norm() > Eigen::NumTraits::dummy_precision()) { if (this->depth >= 0) { // The shapes are in collision. normal = (w0 - w1).normalized(); } else { // The shapes are not in collision. normal = (w1 - w0).normalized(); } } else { normal = this->normal; } details::inflate(shape, normal, w0, w1); } } // namespace details template void ConvexBaseTpl::buildSupportWarmStart() { typedef ConvexBaseTpl ConvexBaseType; if (this->points->size() < ConvexBaseType::num_vertices_large_convex_threshold) { return; } this->support_warm_starts.points.reserve( ConvexBaseType::num_support_warm_starts); this->support_warm_starts.indices.reserve( ConvexBaseType::num_support_warm_starts); Vec3s axiis(0, 0, 0); details::ShapeSupportData support_data; int support_hint = 0; for (int i = 0; i < 3; ++i) { axiis(i) = 1; { Vec3s support; coal::details::getShapeSupport( this, axiis, support, support_hint, support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } axiis(i) = -1; { Vec3s support; coal::details::getShapeSupport( this, axiis, support, support_hint, support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } axiis(i) = 0; } std::array eis = {Vec3s(1, 1, 1), // Vec3s(-1, 1, 1), // Vec3s(-1, -1, 1), // Vec3s(1, -1, 1)}; for (size_t ei_index = 0; ei_index < 4; ++ei_index) { { Vec3s support; coal::details::getShapeSupport( this, eis[ei_index], support, support_hint, support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } { Vec3s support; coal::details::getShapeSupport( this, -eis[ei_index], support, support_hint, support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } } if (this->support_warm_starts.points.size() != ConvexBaseType::num_support_warm_starts || this->support_warm_starts.indices.size() != ConvexBaseType::num_support_warm_starts) { COAL_THROW_PRETTY("Wrong number of support warm starts.", std::runtime_error); } } template void COAL_DLLAPI ConvexBaseTpl::buildSupportWarmStart(); template void COAL_DLLAPI ConvexBaseTpl::buildSupportWarmStart(); } // namespace coal ================================================ FILE: src/narrowphase/minkowski_difference.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ #include "coal/narrowphase/minkowski_difference.h" #include "coal/shape/geometric_shapes_traits.h" namespace coal { namespace details { // ============================================================================ template void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3s& oR1, const Vec3s& ot1, const Vec3s& dir, Vec3s& support0, Vec3s& support1, support_func_guess_t& hint, ShapeSupportData data[2]) { assert(dir.norm() > Eigen::NumTraits::epsilon()); getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]); if (TransformIsIdentity) { getShapeSupport<_SupportOptions>(s1, -dir, support1, hint[1], data[1]); } else { getShapeSupport<_SupportOptions>(s1, -oR1.transpose() * dir, support1, hint[1], data[1]); support1 = oR1 * support1 + ot1; } } // ============================================================================ template void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3s& dir, Vec3s& support0, Vec3s& support1, support_func_guess_t& hint, ShapeSupportData data[2]) { getSupportTpl( static_cast(md.shapes[0]), static_cast(md.shapes[1]), md.oR1, md.ot1, dir, support0, support1, hint, data); } // ============================================================================ template MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( const ShapeBase* s1, bool identity, Eigen::Array& swept_sphere_radius, ShapeSupportData data[2]) { if (_SupportOptions == SupportOptions::WithSweptSphere) { // No need to store the information of swept sphere radius swept_sphere_radius[1] = 0; } else { // We store the information of swept sphere radius. // GJK and EPA will use this information to correct the solution they find. swept_sphere_radius[1] = s1->getSweptSphereRadius(); } switch (s1->getNodeType()) { case GEOM_TRIANGLE: if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; case GEOM_BOX: if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; case GEOM_SPHERE: if (_SupportOptions == SupportOptions::NoSweptSphere) { // Sphere can be considered a swept-sphere point. swept_sphere_radius[1] += static_cast(s1)->radius; } if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; case GEOM_ELLIPSOID: if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; case GEOM_CAPSULE: if (_SupportOptions == SupportOptions::NoSweptSphere) { // Sphere can be considered as a swept-sphere segment. swept_sphere_radius[1] += static_cast(s1)->radius; } if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; case GEOM_CONE: if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; case GEOM_CYLINDER: if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; case GEOM_CONVEX16: { const ConvexBase16* convex1 = static_cast(s1); if (static_cast(convex1->num_points) > ConvexBase16::num_vertices_large_convex_threshold) { data[1].visited.assign(convex1->num_points, false); data[1].last_dir.setZero(); if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; } else { if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; } } case GEOM_CONVEX32: { const ConvexBase32* convex1 = static_cast(s1); if (static_cast(convex1->num_points) > ConvexBase32::num_vertices_large_convex_threshold) { data[1].visited.assign(convex1->num_points, false); data[1].last_dir.setZero(); if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; } else { if (identity) return getSupportFuncTpl; else return getSupportFuncTpl; } } default: COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); } } // ============================================================================ template MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( const ShapeBase* s0, const ShapeBase* s1, bool identity, Eigen::Array& swept_sphere_radius, ShapeSupportData data[2]) { if (_SupportOptions == SupportOptions::WithSweptSphere) { // No need to store the information of swept sphere radius swept_sphere_radius[0] = 0; } else { // We store the information of swept sphere radius. // GJK and EPA will use this information to correct the solution they find. swept_sphere_radius[0] = s0->getSweptSphereRadius(); } switch (s0->getNodeType()) { case GEOM_TRIANGLE: return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; case GEOM_BOX: return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; case GEOM_SPHERE: if (_SupportOptions == SupportOptions::NoSweptSphere) { // Sphere can always be considered as a swept-sphere point. swept_sphere_radius[0] += static_cast(s0)->radius; } return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; case GEOM_ELLIPSOID: return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; case GEOM_CAPSULE: if (_SupportOptions == SupportOptions::NoSweptSphere) { // Capsule can always be considered as a swept-sphere segment. swept_sphere_radius[0] += static_cast(s0)->radius; } return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; case GEOM_CONE: return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; case GEOM_CYLINDER: return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; case GEOM_CONVEX16: { const ConvexBase16* convex0 = static_cast(s0); if (static_cast(convex0->num_points) > ConvexBase16::num_vertices_large_convex_threshold) { data[0].visited.assign(convex0->num_points, false); data[0].last_dir.setZero(); return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); } else return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; } case GEOM_CONVEX32: { const ConvexBase32* convex0 = static_cast(s0); if (static_cast(convex0->num_points) > ConvexBase32::num_vertices_large_convex_threshold) { data[0].visited.assign(convex0->num_points, false); data[0].last_dir.setZero(); return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); } else return makeGetSupportFunction1( s1, identity, swept_sphere_radius, data); break; } default: COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); } } // ============================================================================ bool getNormalizeSupportDirection(const ShapeBase* shape) { switch (shape->getNodeType()) { case GEOM_TRIANGLE: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; case GEOM_BOX: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; case GEOM_SPHERE: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; case GEOM_ELLIPSOID: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; case GEOM_CAPSULE: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; case GEOM_CONE: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; case GEOM_CYLINDER: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; case GEOM_CONVEX16: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; case GEOM_CONVEX32: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; default: COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); } } // ============================================================================ void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0, const ShapeBase* shape1, bool& normalize_support_direction) { normalize_support_direction = getNormalizeSupportDirection(shape0) && getNormalizeSupportDirection(shape1); } // ============================================================================ template void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, const Transform3s& tf0, const Transform3s& tf1) { shapes[0] = shape0; shapes[1] = shape1; getNormalizeSupportDirectionFromShapes(shape0, shape1, normalize_support_direction); oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation(); ot1.noalias() = tf0.getRotation().transpose() * (tf1.getTranslation() - tf0.getTranslation()); bool identity = (oR1.isIdentity() && ot1.isZero()); getSupportFunc = makeGetSupportFunction0<_SupportOptions>( shape0, shape1, identity, swept_sphere_radius, data); } // clang-format off template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); // clang-format on // ============================================================================ template void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { shapes[0] = shape0; shapes[1] = shape1; getNormalizeSupportDirectionFromShapes(shape0, shape1, normalize_support_direction); oR1.setIdentity(); ot1.setZero(); getSupportFunc = makeGetSupportFunction0<_SupportOptions>( shape0, shape1, true, swept_sphere_radius, data); } // clang-format off template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&); template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&); // clang-format on // ============================================================================ // clang-format off template Vec3s COAL_DLLAPI MinkowskiDiff::support0(const Vec3s&, int&) const; template Vec3s COAL_DLLAPI MinkowskiDiff::support0(const Vec3s&, int&) const; template Vec3s COAL_DLLAPI MinkowskiDiff::support1(const Vec3s&, int&) const; template Vec3s COAL_DLLAPI MinkowskiDiff::support1(const Vec3s&, int&) const; // clang-format on } // namespace details } // namespace coal ================================================ FILE: src/narrowphase/support_functions.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2021-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ #include "coal/narrowphase/support_functions.h" #include "coal/contact_patch/polygon_convex_hull.h" namespace coal { namespace details { // ============================================================================ #define CALL_GET_SHAPE_SUPPORT(ShapeType) \ getShapeSupport<_SupportOptions>(static_cast(shape), dir, \ support, hint, support_data) template Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) { Vec3s support; ShapeSupportData support_data; switch (shape->getNodeType()) { case GEOM_TRIANGLE: CALL_GET_SHAPE_SUPPORT(TriangleP); break; case GEOM_BOX: CALL_GET_SHAPE_SUPPORT(Box); break; case GEOM_SPHERE: CALL_GET_SHAPE_SUPPORT(Sphere); break; case GEOM_ELLIPSOID: CALL_GET_SHAPE_SUPPORT(Ellipsoid); break; case GEOM_CAPSULE: CALL_GET_SHAPE_SUPPORT(Capsule); break; case GEOM_CONE: CALL_GET_SHAPE_SUPPORT(Cone); break; case GEOM_CYLINDER: CALL_GET_SHAPE_SUPPORT(Cylinder); break; case GEOM_CONVEX16: CALL_GET_SHAPE_SUPPORT(ConvexBaseTpl); break; case GEOM_CONVEX32: CALL_GET_SHAPE_SUPPORT(ConvexBaseTpl); break; case GEOM_PLANE: case GEOM_HALFSPACE: default: support.setZero(); ; // nothing } return support; } #undef CALL_GET_SHAPE_SUPPORT // Explicit instantiation // clang-format off template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&); template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&); // clang-format on // ============================================================================ #define getShapeSupportTplInstantiation(ShapeType) \ template COAL_DLLAPI void getShapeSupport( \ const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \ ShapeSupportData& support_data); \ \ template COAL_DLLAPI void getShapeSupport( \ const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \ ShapeSupportData& support_data); // ============================================================================ template void getShapeSupport(const TriangleP* triangle, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { Scalar dota = dir.dot(triangle->a); Scalar dotb = dir.dot(triangle->b); Scalar dotc = dir.dot(triangle->c); if (dota > dotb) { if (dotc > dota) { support = triangle->c; } else { support = triangle->a; } } else { if (dotc > dotb) { support = triangle->c; } else { support = triangle->b; } } if (_SupportOptions == SupportOptions::WithSweptSphere) { support += triangle->getSweptSphereRadius() * dir.normalized(); } } getShapeSupportTplInstantiation(TriangleP); // ============================================================================ template void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { // The inflate value is simply to make the specialized functions with box // have a preferred side for edge cases. static const Scalar inflate = (dir.array() == 0).any() ? 1 + Scalar(1e-10) : 1; static const Scalar dummy_precision = Eigen::NumTraits::dummy_precision(); Vec3s support1 = (dir.array() > dummy_precision).select(box->halfSide, 0); Vec3s support2 = (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0); support.noalias() = support1 + support2; if (_SupportOptions == SupportOptions::WithSweptSphere) { support += box->getSweptSphereRadius() * dir.normalized(); } } getShapeSupportTplInstantiation(Box); // ============================================================================ template void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { if (_SupportOptions == SupportOptions::WithSweptSphere) { support.noalias() = (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized(); } else { support.setZero(); } COAL_UNUSED_VARIABLE(sphere); COAL_UNUSED_VARIABLE(dir); } getShapeSupportTplInstantiation(Sphere); // ============================================================================ template void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { Scalar a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; Scalar b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; Scalar c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; Vec3s v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); Scalar d = std::sqrt(v.dot(dir)); support = v / d; if (_SupportOptions == SupportOptions::WithSweptSphere) { support += ellipsoid->getSweptSphereRadius() * dir.normalized(); } } getShapeSupportTplInstantiation(Ellipsoid); // ============================================================================ template void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { static const Scalar dummy_precision = Eigen::NumTraits::dummy_precision(); support.setZero(); if (dir[2] > dummy_precision) { support[2] = capsule->halfLength; } else if (dir[2] < -dummy_precision) { support[2] = -capsule->halfLength; } if (_SupportOptions == SupportOptions::WithSweptSphere) { support += (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized(); } } getShapeSupportTplInstantiation(Capsule); // ============================================================================ template void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { static const Scalar dummy_precision = Eigen::NumTraits::dummy_precision(); // The cone radius is, for -h < z < h, (h - z) * r / (2*h) // The inflate value is simply to make the specialized functions with cone // have a preferred side for edge cases. static const Scalar inflate = 1 + Scalar(1e-10); Scalar h = cone->halfLength; Scalar r = cone->radius; if (dir.head<2>().isZero(dummy_precision)) { support.head<2>().setZero(); if (dir[2] > dummy_precision) { support[2] = h; } else { support[2] = -inflate * h; } } else { Scalar zdist = dir[0] * dir[0] + dir[1] * dir[1]; Scalar len = zdist + dir[2] * dir[2]; zdist = std::sqrt(zdist); if (dir[2] <= 0) { Scalar rad = r / zdist; support.head<2>() = rad * dir.head<2>(); support[2] = -h; } else { len = std::sqrt(len); Scalar sin_a = r / std::sqrt(r * r + 4 * h * h); if (dir[2] > len * sin_a) support << 0, 0, h; else { Scalar rad = r / zdist; support.head<2>() = rad * dir.head<2>(); support[2] = -h; } } } if (_SupportOptions == SupportOptions::WithSweptSphere) { support += cone->getSweptSphereRadius() * dir.normalized(); } } getShapeSupportTplInstantiation(Cone); // ============================================================================ template void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { static const Scalar dummy_precision = Eigen::NumTraits::dummy_precision(); // The inflate value is simply to make the specialized functions with cylinder // have a preferred side for edge cases. static const Scalar inflate = 1 + Scalar(1e-10); Scalar half_h = cylinder->halfLength; Scalar r = cylinder->radius; const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision); if (dir_is_aligned_with_z) half_h *= inflate; if (dir[2] > dummy_precision) { support[2] = half_h; } else if (dir[2] < -dummy_precision) { support[2] = -half_h; } else { support[2] = 0; r *= inflate; } if (dir_is_aligned_with_z) { support.head<2>().setZero(); } else { support.head<2>() = dir.head<2>().normalized() * r; } assert(fabs(support[0] * dir[1] - support[1] * dir[0]) < sqrt(std::numeric_limits::epsilon())); if (_SupportOptions == SupportOptions::WithSweptSphere) { support += cylinder->getSweptSphereRadius() * dir.normalized(); } } getShapeSupportTplInstantiation(Cylinder); // ============================================================================ template void getShapeSupportLog(const ConvexBaseTpl* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& support_data) { assert(convex->neighbors != nullptr && "Convex has no neighbors."); // Use warm start if current support direction is distant from last support // direction. const Scalar use_warm_start_threshold = Scalar(0.9); Vec3s dir_normalized = dir.normalized(); if (!support_data.last_dir.isZero() && !convex->support_warm_starts.points.empty() && support_data.last_dir.dot(dir_normalized) < use_warm_start_threshold) { // Change hint if last dir is too far from current dir. Scalar maxdot = convex->support_warm_starts.points[0].dot(dir); hint = int(convex->support_warm_starts.indices[0]); for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) { Scalar dot = convex->support_warm_starts.points[i].dot(dir); if (dot > maxdot) { maxdot = dot; hint = int(convex->support_warm_starts.indices[i]); } } } support_data.last_dir = dir_normalized; const std::vector& pts = *(convex->points); typedef typename ConvexBaseTpl::Neighbors Neighbors; const std::vector& nn = *(convex->neighbors); if (hint < 0 || hint >= (int)convex->num_points) { hint = 0; } Scalar maxdot = pts[static_cast(hint)].dot(dir); std::vector& visited = support_data.visited; if (support_data.visited.size() == convex->num_points) { std::fill(visited.begin(), visited.end(), false); } else { // std::vector::assign not only assigns the values of the vector but also // resizes the vector. So if `visited` has not been set up yet, this makes // sure the size convex's points and visited are identical. support_data.visited.assign(convex->num_points, false); } visited[static_cast(hint)] = true; // When the first face is orthogonal to dir, all the dot products will be // equal. Yet, the neighbors must be visited. bool found = true; bool loose_check = true; while (found) { const Neighbors& n = nn[static_cast(hint)]; found = false; IndexType current_vertex_idx = IndexType(hint); for (IndexType in = 0; in < n.count; ++in) { const IndexType ip = convex->neighbor(current_vertex_idx, in); if (visited[ip]) continue; visited[ip] = true; const Scalar dot = pts[ip].dot(dir); bool better = false; if (dot > maxdot) { better = true; loose_check = false; } else if (loose_check && dot == maxdot) better = true; if (better) { maxdot = dot; hint = static_cast(ip); found = true; } } } support = pts[static_cast(hint)]; if (_SupportOptions == SupportOptions::WithSweptSphere) { support += convex->getSweptSphereRadius() * dir.normalized(); } } // ============================================================================ template void getShapeSupportLinear(const ConvexBaseTpl* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& /*unused*/) { const std::vector& pts = *(convex->points); hint = 0; Scalar maxdot = pts[0].dot(dir); for (int i = 1; i < (int)convex->num_points; ++i) { Scalar dot = pts[static_cast(i)].dot(dir); if (dot > maxdot) { maxdot = dot; hint = i; } } support = pts[static_cast(hint)]; if (_SupportOptions == SupportOptions::WithSweptSphere) { support += convex->getSweptSphereRadius() * dir.normalized(); } } // ============================================================================ template void getShapeSupport(const ConvexBaseTpl* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& support_data) { // TODO add benchmark to set a proper value for switching between linear and // logarithmic. if (convex->num_points > ConvexBaseTpl::num_vertices_large_convex_threshold && convex->neighbors != nullptr) { getShapeSupportLog<_SupportOptions>(convex, dir, support, hint, support_data); } else { getShapeSupportLinear<_SupportOptions>(convex, dir, support, hint, support_data); } } getShapeSupportTplInstantiation(ConvexBaseTpl); getShapeSupportTplInstantiation(ConvexBaseTpl); // ============================================================================ template void getShapeSupport(const SmallConvex* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& support_data) { getShapeSupportLinear<_SupportOptions>( reinterpret_cast*>(convex), dir, support, hint, support_data); } getShapeSupportTplInstantiation(SmallConvex); getShapeSupportTplInstantiation(SmallConvex); // ============================================================================ template void getShapeSupport(const LargeConvex* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& support_data) { getShapeSupportLog<_SupportOptions>( reinterpret_cast*>(convex), dir, support, hint, support_data); } getShapeSupportTplInstantiation(LargeConvex); getShapeSupportTplInstantiation(LargeConvex); // ============================================================================ #define CALL_GET_SHAPE_SUPPORT_SET(ShapeType) \ getShapeSupportSet<_SupportOptions>(static_cast(shape), \ support_set, hint, support_data, \ max_num_supports, tol) template void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, size_t max_num_supports, Scalar tol) { ShapeSupportData support_data; switch (shape->getNodeType()) { case GEOM_TRIANGLE: CALL_GET_SHAPE_SUPPORT_SET(TriangleP); break; case GEOM_BOX: CALL_GET_SHAPE_SUPPORT_SET(Box); break; case GEOM_SPHERE: CALL_GET_SHAPE_SUPPORT_SET(Sphere); break; case GEOM_ELLIPSOID: CALL_GET_SHAPE_SUPPORT_SET(Ellipsoid); break; case GEOM_CAPSULE: CALL_GET_SHAPE_SUPPORT_SET(Capsule); break; case GEOM_CONE: CALL_GET_SHAPE_SUPPORT_SET(Cone); break; case GEOM_CYLINDER: CALL_GET_SHAPE_SUPPORT_SET(Cylinder); break; case GEOM_CONVEX16: CALL_GET_SHAPE_SUPPORT_SET(ConvexBaseTpl); break; case GEOM_CONVEX32: CALL_GET_SHAPE_SUPPORT_SET(ConvexBaseTpl); break; case GEOM_PLANE: case GEOM_HALFSPACE: default:; // nothing } } #undef CALL_GET_SHAPE_SUPPORT // Explicit instantiation // clang-format off template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, Scalar); template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, Scalar); // clang-format on // ============================================================================ #define getShapeSupportSetTplInstantiation(ShapeType) \ template COAL_DLLAPI void getShapeSupportSet( \ const ShapeType* shape_, SupportSet& support_set, int& hint, \ ShapeSupportData& data, size_t num_sampled_supports, Scalar tol); \ \ template COAL_DLLAPI void \ getShapeSupportSet( \ const ShapeType* shape_, SupportSet& support_set, int& hint, \ ShapeSupportData& data, size_t num_sampled_supports, Scalar tol); // ============================================================================ template void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t /*unused*/, Scalar tol) { assert(tol > 0); support_set.clear(); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); // We simply want to compute the support value, no need to take the // swept-sphere radius into account. getShapeSupport(triangle, support_dir, support, hint, support_data); const Scalar support_value = support.dot(support_dir); if (support_value - support_dir.dot(triangle->a) < tol) { // Note: at the moment, it's useless to take into account the // swept-sphere radius, but in the future we might want to store the // offsets to the plane in `SupportSet`. if (_SupportOptions == SupportOptions::WithSweptSphere) { support_set.addPoint(triangle->a + triangle->getSweptSphereRadius() * support_dir); } else { support_set.addPoint(triangle->a); } } if (support_value - support_dir.dot(triangle->b) < tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { support_set.addPoint(triangle->b + triangle->getSweptSphereRadius() * support_dir); } else { support_set.addPoint(triangle->b); } } if (support_value - support_dir.dot(triangle->c) < tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { support_set.addPoint(triangle->c + triangle->getSweptSphereRadius() * support_dir); } else { support_set.addPoint(triangle->c); } } } getShapeSupportSetTplInstantiation(TriangleP); // ============================================================================ template void getShapeSupportSet(const Box* box, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data, size_t /*unused*/, Scalar tol) { assert(tol > 0); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(box, support_dir, support, hint, support_data); const Scalar support_value = support.dot(support_dir); const Scalar x = box->halfSide[0]; const Scalar y = box->halfSide[1]; const Scalar z = box->halfSide[2]; const std::array corners = { Vec3s(x, y, z), Vec3s(-x, y, z), Vec3s(-x, -y, z), Vec3s(x, -y, z), Vec3s(x, y, -z), Vec3s(-x, y, -z), Vec3s(-x, -y, -z), Vec3s(x, -y, -z), }; std::vector& polygon = support_data.polygon; polygon.clear(); const Transform3s& tf = support_set.tf; for (const Vec3s& corner : corners) { const Scalar val = corner.dot(support_dir); if (support_value - val < tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { const Vec2s p = tf.inverseTransform(corner + box->getSweptSphereRadius() * support_dir) .template head<2>(); polygon.emplace_back(p); } else { const Vec2s p = tf.inverseTransform(corner).template head<2>(); polygon.emplace_back(p); } } } computeSupportSetConvexHull(polygon, support_set.points()); } getShapeSupportSetTplInstantiation(Box); // ============================================================================ template void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t /*unused*/, Scalar /*unused*/) { support_set.points().clear(); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); getShapeSupport<_SupportOptions>(sphere, support_dir, support, hint, support_data); support_set.addPoint(support); } getShapeSupportSetTplInstantiation(Sphere); // ============================================================================ template void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, int& hint, ShapeSupportData& support_data /*unused*/, size_t /*unused*/, Scalar /*unused*/) { support_set.points().clear(); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); getShapeSupport<_SupportOptions>(ellipsoid, support_dir, support, hint, support_data); support_set.addPoint(support); } getShapeSupportSetTplInstantiation(Ellipsoid); // ============================================================================ template void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t /*unused*/, Scalar tol) { // clang-format on assert(tol > 0); support_set.points().clear(); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(capsule, support_dir, support, hint, support_data); const Scalar support_value = support_dir.dot(support + capsule->radius * support_dir); // The support set of a capsule has either 2 points or 1 point. // The two candidate points lie at the frontier between the cylinder and // sphere parts of the capsule. const Scalar h = capsule->halfLength; const Scalar r = capsule->radius; const Vec3s p1(r * support_dir[0], r * support_dir[1], h); const Vec3s p2(r * support_dir[0], r * support_dir[1], -h); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { if (_SupportOptions == SupportOptions::WithSweptSphere) { const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius(); support_set.addPoint(p1 + ssr_vec); support_set.addPoint(p2 + ssr_vec); } else { support_set.addPoint(p1); support_set.addPoint(p2); } } else { if (_SupportOptions == SupportOptions::WithSweptSphere) { const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius(); support_set.addPoint(support + ssr_vec); } else { support_set.addPoint(support); } } } getShapeSupportSetTplInstantiation(Capsule); // ============================================================================ template void getShapeSupportSet(const Cone* cone, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t num_sampled_supports, Scalar tol) { assert(tol > 0); support_set.points().clear(); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(cone, support_dir, support, hint, support_data); const Scalar support_value = support.dot(support_dir); // If the support direction is perpendicular to the cone's basis, there is an // infinite amount of support points; otherwise there are up to two support // points (two if direction is perpendicular to the side of the cone and one // otherwise). // // To check this condition, we look at two points on the cone's basis; // these two points are symmetrical w.r.t the center of the circle. If // both these points are tol away from the support plane, then all the // points of the circle are tol away from the support plane. const Scalar r = cone->radius; const Scalar z = -cone->halfLength; const Vec3s p1(r * support_dir[0], r * support_dir[1], z); const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { // If this check passed, support direction is considered perpendicular to // the basis of the cone. We sample `num_sampled_supports` points on the // base of the cone. We are guaranteed that these points like at a distance // tol of the support plane. const Scalar angle_increment = Scalar(2 * EIGEN_PI) / (Scalar(num_sampled_supports)); for (size_t i = 0; i < num_sampled_supports; ++i) { const Scalar theta = (Scalar)(i)*angle_increment; Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol); if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_cone_base += cone->getSweptSphereRadius() * support_dir; } support_set.addPoint(point_on_cone_base); } } else { // There are two potential supports to add: the tip of the cone and a point // on the basis of the cone. We compare each of these points to the support // value. Vec3s cone_tip(0, 0, cone->halfLength); if (support_value - support_dir.dot(cone_tip) <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { cone_tip += cone->getSweptSphereRadius() * support_dir; } support_set.addPoint(cone_tip); } Vec3s point_on_cone_base = Vec3s(cone->radius * support_dir[0], // cone->radius * support_dir[1], // z); if (support_value - support_dir.dot(point_on_cone_base) <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_cone_base += cone->getSweptSphereRadius() * support_dir; } support_set.addPoint(point_on_cone_base); } } } getShapeSupportSetTplInstantiation(Cone); // ============================================================================ template void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t num_sampled_supports, Scalar tol) { assert(tol > 0); support_set.points().clear(); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(cylinder, support_dir, support, hint, support_data); const Scalar support_value = support.dot(support_dir); // The following is very similar to what is done for Cone's support set // computation. const Scalar r = cylinder->radius; const Scalar z = support_dir[2] <= 0 ? -cylinder->halfLength : cylinder->halfLength; const Vec3s p1(r * support_dir[0], r * support_dir[1], z); const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { const Scalar angle_increment = Scalar(2 * EIGEN_PI) / (Scalar(num_sampled_supports)); for (size_t i = 0; i < num_sampled_supports; ++i) { const Scalar theta = (Scalar)(i)*angle_increment; Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol); if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_cone_base += cylinder->getSweptSphereRadius() * support_dir; } support_set.addPoint(point_on_cone_base); } } else { // There are two potential supports to add: one on each circle bases of the // cylinder. Vec3s point_on_lower_circle = Vec3s(cylinder->radius * support_dir[0], // cylinder->radius * support_dir[1], // -cylinder->halfLength); if (support_value - support_dir.dot(point_on_lower_circle) <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_lower_circle += cylinder->getSweptSphereRadius() * support_dir; } support_set.addPoint(point_on_lower_circle); } Vec3s point_on_upper_circle = Vec3s(cylinder->radius * support_dir[0], // cylinder->radius * support_dir[1], // cylinder->halfLength); if (support_value - support_dir.dot(point_on_upper_circle) <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_upper_circle += cylinder->getSweptSphereRadius() * support_dir; } support_set.addPoint(point_on_upper_circle); } } } getShapeSupportSetTplInstantiation(Cylinder); // ============================================================================ template void getShapeSupportSetLinear(const ConvexBaseTpl* convex, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data, size_t /*unused*/, Scalar tol) { assert(tol > 0); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(convex, support_dir, support, hint, support_data); const Scalar support_value = support_dir.dot(support); const std::vector& points = *(convex->points); std::vector& polygon = support_data.polygon; polygon.clear(); const Transform3s& tf = support_set.tf; for (const Vec3s& point : points) { const Scalar dot = support_dir.dot(point); if (support_value - dot <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { const Vec2s p = tf.inverseTransform(point + convex->getSweptSphereRadius() * support_dir) .template head<2>(); polygon.emplace_back(p); } else { const Vec2s p = tf.inverseTransform(point).template head<2>(); polygon.emplace_back(p); } } } computeSupportSetConvexHull(polygon, support_set.points()); } // ============================================================================ template void convexSupportSetRecurse(const ConvexBaseTpl* convex, const size_t vertex_idx, const Vec3s& support_dir, const Scalar support_value, const Transform3s& tf, std::vector& visited, std::vector& polygon, Scalar tol) { if (visited[vertex_idx]) { return; } visited[vertex_idx] = true; const std::vector& points = *(convex->points); const Vec3s& point = points[vertex_idx]; const Scalar val = point.dot(support_dir); Scalar swept_sphere_radius = convex->getSweptSphereRadius(); if (support_value - val <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { const Vec2s p = tf.inverseTransform(point + swept_sphere_radius * support_dir) .template head<2>(); polygon.emplace_back(p); } else { const Vec2s p = tf.inverseTransform(point).template head<2>(); polygon.emplace_back(p); } typedef typename ConvexBaseTpl::Neighbors Neighbors; const std::vector& neighbors = *(convex->neighbors); const Neighbors& point_neighbors = neighbors[vertex_idx]; for (IndexType i = 0; i < point_neighbors.count; ++i) { const IndexType neighbor_index = convex->neighbor(IndexType(vertex_idx), i); convexSupportSetRecurse<_SupportOptions, IndexType>( convex, neighbor_index, support_dir, support_value, tf, visited, polygon, tol); } } } // ============================================================================ template void getShapeSupportSetLog(const ConvexBaseTpl* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t /*unused*/, Scalar tol) { assert(tol > 0); Vec3s support; const Vec3s& support_dir = support_set.getNormal(); getShapeSupportLog( convex, support_dir, support, hint, support_data); const Scalar support_value = support.dot(support_dir); std::vector& visited = support_data.visited; // `visited` is guaranteed to be of right size due to previous call to convex // log support function. std::fill(support_data.visited.begin(), support_data.visited.end(), false); std::vector& polygon = support_data.polygon; polygon.clear(); const Transform3s& tf = support_set.tf; const size_t vertex_idx = (size_t)(hint); convexSupportSetRecurse<_SupportOptions, IndexType>( convex, vertex_idx, support_dir, support_value, tf, visited, polygon, tol); computeSupportSetConvexHull(polygon, support_set.points()); } // ============================================================================ template void getShapeSupportSet(const ConvexBaseTpl* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t num_sampled_supports /*unused*/, Scalar tol) { if (convex->num_points > ConvexBaseTpl::num_vertices_large_convex_threshold && convex->neighbors != nullptr) { getShapeSupportSetLog<_SupportOptions>( convex, support_set, hint, support_data, num_sampled_supports, tol); } else { getShapeSupportSetLinear<_SupportOptions>( convex, support_set, hint, support_data, num_sampled_supports, tol); } } getShapeSupportSetTplInstantiation(ConvexBaseTpl); getShapeSupportSetTplInstantiation(ConvexBaseTpl); // ============================================================================ template void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t num_sampled_supports /*unused*/, Scalar tol) { getShapeSupportSetLinear<_SupportOptions>( reinterpret_cast*>(convex), support_set, hint, support_data, num_sampled_supports, tol); } getShapeSupportSetTplInstantiation(SmallConvex); getShapeSupportSetTplInstantiation(SmallConvex); // ============================================================================ template void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t num_sampled_supports /*unused*/, Scalar tol) { getShapeSupportSetLog<_SupportOptions>( reinterpret_cast*>(convex), support_set, hint, support_data, num_sampled_supports, tol); } getShapeSupportSetTplInstantiation(LargeConvex); getShapeSupportSetTplInstantiation(LargeConvex); // ============================================================================ COAL_DLLAPI void computeSupportSetConvexHull(const std::vector& cloud, std::vector& cvx_hull) { computePolygonConvexHull(cloud, cvx_hull); } } // namespace details } // namespace coal ================================================ FILE: src/octree.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2020-2023, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "coal/octree.h" #include namespace coal { namespace internal { struct Neighbors { char value; Neighbors() : value(0) {} bool minusX() const { return value & 0x1; } bool plusX() const { return value & 0x2; } bool minusY() const { return value & 0x4; } bool plusY() const { return value & 0x8; } bool minusZ() const { return value & 0x10; } bool plusZ() const { return value & 0x20; } void hasNeighboordMinusX() { value |= 0x1; } void hasNeighboordPlusX() { value |= 0x2; } void hasNeighboordMinusY() { value |= 0x4; } void hasNeighboordPlusY() { value |= 0x8; } void hasNeighboordMinusZ() { value |= 0x10; } void hasNeighboordPlusZ() { value |= 0x20; } }; // struct neighbors void computeNeighbors(const std::vector& boxes, std::vector& neighbors) { typedef std::vector VectorVec6s; Scalar fixedSize = -1; Scalar e = Scalar(1e-8); for (std::size_t i = 0; i < boxes.size(); ++i) { const Vec6s& box(boxes[i]); Neighbors& n(neighbors[i]); Scalar x(box[0]); Scalar y(box[1]); Scalar z(box[2]); Scalar s(box[3]); if (fixedSize == -1) fixedSize = s; else assert(s == fixedSize); for (VectorVec6s::const_iterator it = boxes.begin(); it != boxes.end(); ++it) { const Vec6s& otherBox = *it; Scalar xo(otherBox[0]); Scalar yo(otherBox[1]); Scalar zo(otherBox[2]); // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){ // continue; // } if ((fabs(x - xo - s) < e) && (fabs(y - yo) < e) && (fabs(z - zo) < e)) { n.hasNeighboordMinusX(); } else if ((fabs(x - xo + s) < e) && (fabs(y - yo) < e) && (fabs(z - zo) < e)) { n.hasNeighboordPlusX(); } else if ((fabs(x - xo) < e) && (fabs(y - yo - s) < e) && (fabs(z - zo) < e)) { n.hasNeighboordMinusY(); } else if ((fabs(x - xo) < e) && (fabs(y - yo + s) < e) && (fabs(z - zo) < e)) { n.hasNeighboordPlusY(); } else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) && (fabs(z - zo - s) < e)) { n.hasNeighboordMinusZ(); } else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) && (fabs(z - zo + s) < e)) { n.hasNeighboordPlusZ(); } } } } } // namespace internal void OcTree::exportAsObjFile(const std::string& filename) const { std::vector boxes(this->toBoxes()); std::vector neighbors(boxes.size()); internal::computeNeighbors(boxes, neighbors); // compute list of vertices and faces typedef std::vector VectorVec3s; std::vector vertices; typedef std::array Array4; typedef std::vector VectorArray4; std::vector faces; for (std::size_t i = 0; i < boxes.size(); ++i) { const Vec6s& box(boxes[i]); internal::Neighbors& n(neighbors[i]); Scalar x(box[0]); Scalar y(box[1]); Scalar z(box[2]); Scalar size(box[3]); const Scalar half = Scalar(0.5); vertices.push_back( Vec3s(x - half * size, y - half * size, z - half * size)); vertices.push_back( Vec3s(x + half * size, y - half * size, z - half * size)); vertices.push_back( Vec3s(x - half * size, y + half * size, z - half * size)); vertices.push_back( Vec3s(x + half * size, y + half * size, z - half * size)); vertices.push_back( Vec3s(x - half * size, y - half * size, z + half * size)); vertices.push_back( Vec3s(x + half * size, y - half * size, z + half * size)); vertices.push_back( Vec3s(x - half * size, y + half * size, z + half * size)); vertices.push_back( Vec3s(x + half * size, y + half * size, z + half * size)); // Add face only if box has no neighbor with the same face if (!n.minusX()) { Array4 a = {{8 * i + 1, 8 * i + 5, 8 * i + 7, 8 * i + 3}}; faces.push_back(a); } if (!n.plusX()) { Array4 a = {{8 * i + 2, 8 * i + 4, 8 * i + 8, 8 * i + 6}}; faces.push_back(a); } if (!n.minusY()) { Array4 a = {{8 * i + 1, 8 * i + 2, 8 * i + 6, 8 * i + 5}}; faces.push_back(a); } if (!n.plusY()) { Array4 a = {{8 * i + 4, 8 * i + 3, 8 * i + 7, 8 * i + 8}}; faces.push_back(a); } if (!n.minusZ()) { Array4 a = {{8 * i + 1, 8 * i + 2, 8 * i + 4, 8 * i + 3}}; faces.push_back(a); } if (!n.plusZ()) { Array4 a = {{8 * i + 5, 8 * i + 6, 8 * i + 8, 8 * i + 7}}; faces.push_back(a); } } // write obj in a file std::ofstream os; os.open(filename); if (!os.is_open()) COAL_THROW_PRETTY( (std::string("failed to open file \"") + filename + std::string("\"")) .c_str(), std::runtime_error); // write vertices os << "# list of vertices\n"; for (VectorVec3s::const_iterator it = vertices.begin(); it != vertices.end(); ++it) { const Vec3s& v = *it; os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n'; } os << "\n# list of faces\n"; for (VectorArray4::const_iterator it = faces.begin(); it != faces.end(); ++it) { const Array4& f = *it; os << "f " << f[0] << " " << f[1] << " " << f[2] << " " << f[3] << '\n'; } } OcTreePtr_t makeOctree( const Eigen::Matrix& point_cloud, const Scalar resolution) { typedef Eigen::Matrix InputType; typedef InputType::ConstRowXpr RowType; shared_ptr octree(new octomap::OcTree(resolution)); for (Eigen::DenseIndex row_id = 0; row_id < point_cloud.rows(); ++row_id) { RowType row = point_cloud.row(row_id); octree->updateNode(row[0], row[1], row[2], true, true); } octree->updateInnerOccupancy(); return OcTreePtr_t(new OcTree(octree)); } } // namespace coal ================================================ FILE: src/serialization/serialization.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Justin Carpentier */ #include "coal/serialization/fwd.h" using namespace coal; #include "coal/serialization/transform.h" #include "coal/serialization/collision_data.h" #include "coal/serialization/geometric_shapes.h" #include "coal/serialization/convex.h" #include "coal/serialization/hfield.h" #include "coal/serialization/BVH_model.h" #ifdef COAL_HAS_OCTOMAP #include "coal/serialization/octree.h" #endif COAL_SERIALIZATION_DEFINE_EXPORT(CollisionRequest) COAL_SERIALIZATION_DEFINE_EXPORT(CollisionResult) COAL_SERIALIZATION_DEFINE_EXPORT(DistanceRequest) COAL_SERIALIZATION_DEFINE_EXPORT(DistanceResult) COAL_SERIALIZATION_DEFINE_EXPORT(ShapeBase) COAL_SERIALIZATION_DEFINE_EXPORT(CollisionGeometry) COAL_SERIALIZATION_DEFINE_EXPORT(TriangleP) COAL_SERIALIZATION_DEFINE_EXPORT(Box) COAL_SERIALIZATION_DEFINE_EXPORT(Sphere) COAL_SERIALIZATION_DEFINE_EXPORT(Ellipsoid) COAL_SERIALIZATION_DEFINE_EXPORT(Capsule) COAL_SERIALIZATION_DEFINE_EXPORT(Cone) COAL_SERIALIZATION_DEFINE_EXPORT(Cylinder) COAL_SERIALIZATION_DEFINE_EXPORT(Halfspace) COAL_SERIALIZATION_DEFINE_EXPORT(Plane) #define EXPORT_AND_CAST(Derived, Base) \ COAL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ COAL_SERIALIZATION_DEFINE_EXPORT(Derived) \ /**/ COAL_SERIALIZATION_CAST_REGISTER(ConvexBase16, CollisionGeometry) COAL_SERIALIZATION_CAST_REGISTER(ConvexBase32, CollisionGeometry) EXPORT_AND_CAST(ConvexTpl, ConvexBase16) EXPORT_AND_CAST(ConvexTpl, ConvexBase32) EXPORT_AND_CAST(ConvexTpl, ConvexBase16) EXPORT_AND_CAST(ConvexTpl, ConvexBase32) COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) COAL_SERIALIZATION_CAST_REGISTER(BVHModelBase, CollisionGeometry) EXPORT_AND_CAST(BVHModel, BVHModelBase) EXPORT_AND_CAST(BVHModel, BVHModelBase) EXPORT_AND_CAST(BVHModel, BVHModelBase) EXPORT_AND_CAST(BVHModel, BVHModelBase) EXPORT_AND_CAST(BVHModel, BVHModelBase) EXPORT_AND_CAST(BVHModel>, BVHModelBase) EXPORT_AND_CAST(BVHModel>, BVHModelBase) EXPORT_AND_CAST(BVHModel>, BVHModelBase) #ifdef COAL_HAS_OCTOMAP COAL_SERIALIZATION_DEFINE_EXPORT(OcTree) #endif ================================================ FILE: src/shape/geometric_shapes.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/shape/geometric_shapes.h" #include "coal/shape/geometric_shapes_utility.h" #ifdef COAL_HAS_QHULL #include #include #include #include #include #include #include using orgQhull::Qhull; using orgQhull::QhullFacet; using orgQhull::QhullPoint; using orgQhull::QhullRidgeSet; using orgQhull::QhullVertexList; using orgQhull::QhullVertexSet; #endif namespace coal { template ConvexBaseTpl* ConvexBaseTpl::convexHull( std::shared_ptr>& pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand) { COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS return ConvexBaseTpl::convexHull(pts->data(), num_points, keepTriangles, qhullCommand); COAL_COMPILER_DIAGNOSTIC_POP } template COAL_DLLAPI ConvexBaseTpl* ConvexBaseTpl::convexHull( std::shared_ptr>& pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand); template COAL_DLLAPI ConvexBaseTpl* ConvexBaseTpl::convexHull( std::shared_ptr>& pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand); template ConvexBaseTpl* ConvexBaseTpl::convexHull( const Vec3s* pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand) { #ifdef COAL_HAS_QHULL if (num_points <= 3) { COAL_THROW_PRETTY( "You shouldn't use this function with less than" " 4 points.", std::invalid_argument); } assert(pts[0].data() + 3 == pts[1].data()); Qhull qh; const char* command = qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : ""); // TODO: add a ifdef not double precision here using Vec3d = Eigen::Vector3d; std::vector qhull_pts; qhull_pts.reserve(num_points); for (size_t i = 0; i < num_points; ++i) { qhull_pts.push_back(pts[i].template cast()); } qh.runQhull("", 3, static_cast(num_points), qhull_pts[0].data(), command); if (qh.qhullStatus() != qh_ERRnone) { if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; COAL_THROW_PRETTY("Qhull failed", std::logic_error); } typedef int size_type; // Map index in pts to index in vertices. -1 means not used std::vector pts_to_vertices(num_points, -1); // Initialize the vertices size_t nvertex = static_cast(qh.vertexCount()); if (nvertex >= size_t(std::numeric_limits::max())) { COAL_THROW_PRETTY("nvertex >= std::numeric_limits::max()", std::runtime_error); } std::shared_ptr> vertices( new std::vector(size_t(nvertex))); QhullVertexList vertexList(qh.vertexList()); size_t i_vertex = 0; for (QhullVertexList::const_iterator v = vertexList.begin(); v != vertexList.end(); ++v) { QhullPoint pt((*v).point()); pts_to_vertices[(size_t)pt.id()] = (int)i_vertex; (*vertices)[i_vertex] = Vec3s(Scalar(pt[0]), Scalar(pt[1]), Scalar(pt[2])); ++i_vertex; } assert(i_vertex == nvertex); ConvexTpl>* convex_tri(NULL); ConvexBaseTpl* convex(NULL); if (keepTriangles) convex = convex_tri = new ConvexTpl>(); else convex = new ConvexBaseTpl; convex->initialize(vertices, static_cast(nvertex)); // Build the neighbors convex->neighbors.reset(new std::vector(size_t(nvertex))); std::vector> nneighbors(static_cast(nvertex)); if (keepTriangles) { convex_tri->num_polygons = static_cast(qh.facetCount()); convex_tri->polygons.reset( new std::vector>(convex_tri->num_polygons)); convex_tri->computeCenter(); } unsigned int c_nneighbors = 0; unsigned int i_polygon = 0; // TODO: make sure number of vertices < size of IndexType // Compute the neighbors from the edges of the faces. for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); facet = facet.next()) { if (facet.isSimplicial()) { // In 3D, simplicial faces have 3 vertices. We mark them as neighbors. QhullVertexSet f_vertices(facet.vertices()); IndexType n = static_cast(f_vertices.count()); assert(n == 3); TriangleTpl tri( static_cast( pts_to_vertices[static_cast(f_vertices[0].point().id())]), static_cast( pts_to_vertices[static_cast(f_vertices[1].point().id())]), static_cast(pts_to_vertices[static_cast( f_vertices[2].point().id())])); if (keepTriangles) { reorderTriangle(convex_tri, tri); (*convex_tri->polygons)[i_polygon++] = tri; } for (IndexType j = 0; j < static_cast(n); ++j) { IndexType i = (j == 0) ? n - 1 : j - 1; IndexType k = (j == n - 1) ? 0 : j + 1; // Update neighbors of pj; if (nneighbors[tri[j]].insert(tri[i]).second) c_nneighbors++; if (nneighbors[tri[j]].insert(tri[k]).second) c_nneighbors++; } } else { if (keepTriangles) { // TODO I think there is a memory leak here. COAL_THROW_PRETTY( "You requested to keep triangles so you " "must pass option \"Qt\" to qhull via the qhull command argument.", std::invalid_argument); } // Non-simplicial faces have more than 3 vertices and contains a list of // rigdes. Ridges are (3-1)D simplex (i.e. one edge). We mark the two // vertices of each ridge as neighbors. QhullRidgeSet f_ridges(facet.ridges()); for (size_type j = 0; j < f_ridges.count(); ++j) { assert(f_ridges[j].vertices().count() == 2); int pi = pts_to_vertices[static_cast( f_ridges[j].vertices()[0].point().id())], pj = pts_to_vertices[static_cast( f_ridges[j].vertices()[1].point().id())]; // Update neighbors of pi and pj; if (nneighbors[static_cast(pj)] .insert(static_cast(pi)) .second) c_nneighbors++; if (nneighbors[static_cast(pi)] .insert(static_cast(pj)) .second) c_nneighbors++; } } } assert(!keepTriangles || static_cast(i_polygon) == qh.facetCount()); // Build the double representation (free in this case because qhull has // alreday run) convex->buildDoubleDescriptionFromQHullResult(qh); // Fill the neighbor attribute of the returned object. convex->nneighbors_.reset(new std::vector(c_nneighbors)); std::vector& neighbors_ = *(convex->neighbors); std::vector& nneighbors_ = *(convex->nneighbors_); IndexType begin_id = 0; for (size_t i = 0; i < static_cast(nvertex); ++i) { Neighbors& n = neighbors_[i]; n.count = static_cast(nneighbors[i].size()); n.begin_id = begin_id; IndexType j = 0; for (IndexType idx : nneighbors[i]) { nneighbors_[n.begin_id + j] = idx; j++; } begin_id += n.count; } // Now that the neighbors are computed, we can call the // `buildSupportWarmStart` function. convex->buildSupportWarmStart(); return convex; #else COAL_THROW_PRETTY( "Library built without qhull. Cannot build object of this type.", std::logic_error); COAL_UNUSED_VARIABLE(pts); COAL_UNUSED_VARIABLE(num_points); COAL_UNUSED_VARIABLE(keepTriangles); COAL_UNUSED_VARIABLE(qhullCommand); #endif } template COAL_DLLAPI ConvexBaseTpl* ConvexBaseTpl::convexHull(const Vec3s* pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand); template COAL_DLLAPI ConvexBaseTpl* ConvexBaseTpl::convexHull(const Vec3s* pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand); #ifdef COAL_HAS_QHULL template void ConvexBaseTpl::buildDoubleDescription() { if (num_points <= 3) { COAL_THROW_PRETTY( "You shouldn't use this function with a convex less than" " 4 points.", std::invalid_argument); } Qhull qh; const char* command = "Qt"; using Vec3d = Eigen::Vector3d; std::vector qhull_pts; qhull_pts.reserve(num_points); for (size_t i = 0; i < num_points; ++i) { qhull_pts.push_back((*points)[i].template cast()); } qh.runQhull("", 3, static_cast(num_points), qhull_pts[0].data(), command); if (qh.qhullStatus() != qh_ERRnone) { if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; COAL_THROW_PRETTY("Qhull failed", std::logic_error); } buildDoubleDescriptionFromQHullResult(qh); } template void COAL_DLLAPI ConvexBaseTpl::buildDoubleDescription(); template void COAL_DLLAPI ConvexBaseTpl::buildDoubleDescription(); template void ConvexBaseTpl::buildDoubleDescriptionFromQHullResult( const Qhull& qh) { num_normals_and_offsets = static_cast(qh.facetCount()); normals.reset(new std::vector(num_normals_and_offsets)); std::vector& normals_ = *normals; offsets.reset(new std::vector(num_normals_and_offsets)); std::vector& offsets_ = *offsets; unsigned int i_normal = 0; for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); facet = facet.next()) { const orgQhull::QhullHyperplane& plane = facet.hyperplane(); normals_[i_normal] = Vec3s(Scalar(plane.coordinates()[0]), Scalar(plane.coordinates()[1]), Scalar(plane.coordinates()[2])); offsets_[i_normal] = Scalar(plane.offset()); i_normal++; } assert(static_cast(i_normal) == qh.facetCount()); } template void COAL_DLLAPI ConvexBaseTpl::buildDoubleDescriptionFromQHullResult( const Qhull& qh); template void COAL_DLLAPI ConvexBaseTpl::buildDoubleDescriptionFromQHullResult( const Qhull& qh); #endif void Halfspace::unitNormalTest() { Scalar l = n.norm(); if (l > 0) { Scalar inv_l = Scalar(1) / l; n *= inv_l; d *= inv_l; } else { n << 1, 0, 0; d = 0; } } void Plane::unitNormalTest() { Scalar l = n.norm(); if (l > 0) { Scalar inv_l = Scalar(1) / l; n *= inv_l; d *= inv_l; } else { n << 1, 0, 0; d = 0; } } void Box::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Sphere::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = radius; } void Ellipsoid::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Capsule::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Cone::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Cylinder::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Halfspace::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void Plane::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } void TriangleP::computeLocalAABB() { computeBV(*this, Transform3s(), aabb_local); const Scalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } } // namespace coal ================================================ FILE: src/shape/geometric_shapes_utility.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/shape/geometric_shapes_utility.h" #include "coal/internal/BV_fitter.h" #include "coal/internal/tools.h" namespace coal { namespace details { std::vector getBoundVertices(const Box& box, const Transform3s& tf) { std::vector result(8); Scalar a = box.halfSide[0]; Scalar b = box.halfSide[1]; Scalar c = box.halfSide[2]; result[0] = tf.transform(Vec3s(a, b, c)); result[1] = tf.transform(Vec3s(a, b, -c)); result[2] = tf.transform(Vec3s(a, -b, c)); result[3] = tf.transform(Vec3s(a, -b, -c)); result[4] = tf.transform(Vec3s(-a, b, c)); result[5] = tf.transform(Vec3s(-a, b, -c)); result[6] = tf.transform(Vec3s(-a, -b, c)); result[7] = tf.transform(Vec3s(-a, -b, -c)); return result; } // we use icosahedron to bound the sphere std::vector getBoundVertices(const Sphere& sphere, const Transform3s& tf) { std::vector result(12); const Scalar m = (1 + sqrt(Scalar(5))) / Scalar(2); Scalar edge_size = sphere.radius * 6 / (sqrt(Scalar(27)) + sqrt(Scalar(15))); Scalar a = edge_size; Scalar b = m * edge_size; result[0] = tf.transform(Vec3s(0, a, b)); result[1] = tf.transform(Vec3s(0, -a, b)); result[2] = tf.transform(Vec3s(0, a, -b)); result[3] = tf.transform(Vec3s(0, -a, -b)); result[4] = tf.transform(Vec3s(a, b, 0)); result[5] = tf.transform(Vec3s(-a, b, 0)); result[6] = tf.transform(Vec3s(a, -b, 0)); result[7] = tf.transform(Vec3s(-a, -b, 0)); result[8] = tf.transform(Vec3s(b, 0, a)); result[9] = tf.transform(Vec3s(b, 0, -a)); result[10] = tf.transform(Vec3s(-b, 0, a)); result[11] = tf.transform(Vec3s(-b, 0, -a)); return result; } // we use scaled icosahedron to bound the ellipsoid std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3s& tf) { std::vector result(12); const Scalar phi = (1 + sqrt(Scalar(5))) / Scalar(2); const Scalar a = sqrt(Scalar(3)) / (phi * phi); const Scalar b = phi * a; const Scalar& A = ellipsoid.radii[0]; const Scalar& B = ellipsoid.radii[1]; const Scalar& C = ellipsoid.radii[2]; Scalar Aa = A * a; Scalar Ab = A * b; Scalar Ba = B * a; Scalar Bb = B * b; Scalar Ca = C * a; Scalar Cb = C * b; result[0] = tf.transform(Vec3s(0, Ba, Cb)); result[1] = tf.transform(Vec3s(0, -Ba, Cb)); result[2] = tf.transform(Vec3s(0, Ba, -Cb)); result[3] = tf.transform(Vec3s(0, -Ba, -Cb)); result[4] = tf.transform(Vec3s(Aa, Bb, 0)); result[5] = tf.transform(Vec3s(-Aa, Bb, 0)); result[6] = tf.transform(Vec3s(Aa, -Bb, 0)); result[7] = tf.transform(Vec3s(-Aa, -Bb, 0)); result[8] = tf.transform(Vec3s(Ab, 0, Ca)); result[9] = tf.transform(Vec3s(Ab, 0, -Ca)); result[10] = tf.transform(Vec3s(-Ab, 0, Ca)); result[11] = tf.transform(Vec3s(-Ab, 0, -Ca)); return result; } std::vector getBoundVertices(const Capsule& capsule, const Transform3s& tf) { std::vector result(36); const Scalar m = (1 + sqrt(Scalar(5))) / Scalar(2); Scalar hl = capsule.halfLength; Scalar edge_size = capsule.radius * 6 / (sqrt(Scalar(27)) + sqrt(Scalar(15))); Scalar a = edge_size; Scalar b = m * edge_size; Scalar r2 = capsule.radius * 2 / sqrt(Scalar(3)); result[0] = tf.transform(Vec3s(0, a, b + hl)); result[1] = tf.transform(Vec3s(0, -a, b + hl)); result[2] = tf.transform(Vec3s(0, a, -b + hl)); result[3] = tf.transform(Vec3s(0, -a, -b + hl)); result[4] = tf.transform(Vec3s(a, b, hl)); result[5] = tf.transform(Vec3s(-a, b, hl)); result[6] = tf.transform(Vec3s(a, -b, hl)); result[7] = tf.transform(Vec3s(-a, -b, hl)); result[8] = tf.transform(Vec3s(b, 0, a + hl)); result[9] = tf.transform(Vec3s(b, 0, -a + hl)); result[10] = tf.transform(Vec3s(-b, 0, a + hl)); result[11] = tf.transform(Vec3s(-b, 0, -a + hl)); result[12] = tf.transform(Vec3s(0, a, b - hl)); result[13] = tf.transform(Vec3s(0, -a, b - hl)); result[14] = tf.transform(Vec3s(0, a, -b - hl)); result[15] = tf.transform(Vec3s(0, -a, -b - hl)); result[16] = tf.transform(Vec3s(a, b, -hl)); result[17] = tf.transform(Vec3s(-a, b, -hl)); result[18] = tf.transform(Vec3s(a, -b, -hl)); result[19] = tf.transform(Vec3s(-a, -b, -hl)); result[20] = tf.transform(Vec3s(b, 0, a - hl)); result[21] = tf.transform(Vec3s(b, 0, -a - hl)); result[22] = tf.transform(Vec3s(-b, 0, a - hl)); result[23] = tf.transform(Vec3s(-b, 0, -a - hl)); Scalar c = Scalar(0.5) * r2; Scalar d = capsule.radius; result[24] = tf.transform(Vec3s(r2, 0, hl)); result[25] = tf.transform(Vec3s(c, d, hl)); result[26] = tf.transform(Vec3s(-c, d, hl)); result[27] = tf.transform(Vec3s(-r2, 0, hl)); result[28] = tf.transform(Vec3s(-c, -d, hl)); result[29] = tf.transform(Vec3s(c, -d, hl)); result[30] = tf.transform(Vec3s(r2, 0, -hl)); result[31] = tf.transform(Vec3s(c, d, -hl)); result[32] = tf.transform(Vec3s(-c, d, -hl)); result[33] = tf.transform(Vec3s(-r2, 0, -hl)); result[34] = tf.transform(Vec3s(-c, -d, -hl)); result[35] = tf.transform(Vec3s(c, -d, -hl)); return result; } std::vector getBoundVertices(const Cone& cone, const Transform3s& tf) { std::vector result(7); Scalar hl = cone.halfLength; Scalar r2 = cone.radius * 2 / sqrt(Scalar(3)); Scalar a = Scalar(0.5) * r2; Scalar b = cone.radius; result[0] = tf.transform(Vec3s(r2, 0, -hl)); result[1] = tf.transform(Vec3s(a, b, -hl)); result[2] = tf.transform(Vec3s(-a, b, -hl)); result[3] = tf.transform(Vec3s(-r2, 0, -hl)); result[4] = tf.transform(Vec3s(-a, -b, -hl)); result[5] = tf.transform(Vec3s(a, -b, -hl)); result[6] = tf.transform(Vec3s(0, 0, hl)); return result; } std::vector getBoundVertices(const Cylinder& cylinder, const Transform3s& tf) { std::vector result(12); Scalar hl = cylinder.halfLength; Scalar r2 = cylinder.radius * 2 / sqrt(Scalar(3)); Scalar a = Scalar(0.5) * r2; Scalar b = cylinder.radius; result[0] = tf.transform(Vec3s(r2, 0, -hl)); result[1] = tf.transform(Vec3s(a, b, -hl)); result[2] = tf.transform(Vec3s(-a, b, -hl)); result[3] = tf.transform(Vec3s(-r2, 0, -hl)); result[4] = tf.transform(Vec3s(-a, -b, -hl)); result[5] = tf.transform(Vec3s(a, -b, -hl)); result[6] = tf.transform(Vec3s(r2, 0, hl)); result[7] = tf.transform(Vec3s(a, b, hl)); result[8] = tf.transform(Vec3s(-a, b, hl)); result[9] = tf.transform(Vec3s(-r2, 0, hl)); result[10] = tf.transform(Vec3s(-a, -b, hl)); result[11] = tf.transform(Vec3s(a, -b, hl)); return result; } std::vector getBoundVertices(const TriangleP& triangle, const Transform3s& tf) { std::vector result(3); result[0] = tf.transform(triangle.a); result[1] = tf.transform(triangle.b); result[2] = tf.transform(triangle.c); return result; } } // namespace details Halfspace transform(const Halfspace& a, const Transform3s& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' /// where n' = R * n /// and d' = d + n' * T Vec3s n = tf.getRotation() * a.n; Scalar d = a.d + n.dot(tf.getTranslation()); Halfspace result(n, d); result.setSweptSphereRadius(a.getSweptSphereRadius()); return result; } Plane transform(const Plane& a, const Transform3s& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' /// where n' = R * n /// and d' = d + n' * T Vec3s n = tf.getRotation() * a.n; Scalar d = a.d + n.dot(tf.getTranslation()); Plane result(n, d); result.setSweptSphereRadius(a.getSweptSphereRadius()); return result; } std::array transformToHalfspaces(const Plane& a, const Transform3s& tf) { // A plane can be represented by two halfspaces Vec3s n = tf.getRotation() * a.n; Scalar d = a.d + n.dot(tf.getTranslation()); std::array result = {Halfspace(n, d), Halfspace(-n, -d)}; result[0].setSweptSphereRadius(a.getSweptSphereRadius()); result[1].setSweptSphereRadius(a.getSweptSphereRadius()); return result; } template <> void computeBV(const Box& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); Vec3s v_delta(R.cwiseAbs() * s.halfSide); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> void computeBV(const Sphere& s, const Transform3s& tf, AABB& bv) { const Vec3s& T = tf.getTranslation(); Vec3s v_delta(Vec3s::Constant(s.radius)); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> void computeBV(const Ellipsoid& e, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); Vec3s v_delta = R * e.radii; bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> void computeBV(const Capsule& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); Vec3s v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3s::Constant(s.radius)); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> void computeBV(const Cone& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); Scalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength); Scalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength); Scalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength); Vec3s v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> void computeBV(const Cylinder& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); Scalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength); Scalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength); Scalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength); Vec3s v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template void computeAABBConvex(const ConvexBaseTpl& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); AABB bv_; const std::vector& points_ = *(s.points); for (std::size_t i = 0; i < s.num_points; ++i) { Vec3s new_p = R * points_[i] + T; bv_ += new_p; } bv = bv_; } template <> void computeBV(const ConvexBase32& s, const Transform3s& tf, AABB& bv) { computeAABBConvex(s, tf, bv); } template <> void computeBV(const ConvexBase16& s, const Transform3s& tf, AABB& bv) { computeAABBConvex(s, tf, bv); } template <> void computeBV(const TriangleP& s, const Transform3s& tf, AABB& bv) { bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c)); } template <> void computeBV(const Halfspace& s, const Transform3s& tf, AABB& bv) { Halfspace new_s = transform(s, tf); const Vec3s& n = new_s.n; const Scalar& d = new_s.d; AABB bv_; bv_.min_ = Vec3s::Constant(-(std::numeric_limits::max)()); bv_.max_ = Vec3s::Constant((std::numeric_limits::max)()); if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) { // normal aligned with x axis if (n[0] < 0) bv_.min_[0] = -d; else if (n[0] > 0) bv_.max_[0] = d; } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) { // normal aligned with y axis if (n[1] < 0) bv_.min_[1] = -d; else if (n[1] > 0) bv_.max_[1] = d; } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) { // normal aligned with z axis if (n[2] < 0) bv_.min_[2] = -d; else if (n[2] > 0) bv_.max_[2] = d; } bv = bv_; } template <> void computeBV(const Plane& s, const Transform3s& tf, AABB& bv) { Plane new_s = transform(s, tf); const Vec3s& n = new_s.n; const Scalar& d = new_s.d; AABB bv_; bv_.min_ = Vec3s::Constant(-(std::numeric_limits::max)()); bv_.max_ = Vec3s::Constant((std::numeric_limits::max)()); if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) { // normal aligned with x axis if (n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } else if (n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) { // normal aligned with y axis if (n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } else if (n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) { // normal aligned with z axis if (n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; } else if (n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; } } bv = bv_; } template <> void computeBV(const Box& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); bv.To = T; bv.axes = R; bv.extent = s.halfSide; } template <> void computeBV(const Sphere& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.setIdentity(); bv.extent.setConstant(s.radius); } template <> void computeBV(const Capsule& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; bv.extent << s.radius, s.radius, s.halfLength + s.radius; } template <> void computeBV(const Cone& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; bv.extent << s.radius, s.radius, s.halfLength; } template <> void computeBV(const Cylinder& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; bv.extent << s.radius, s.radius, s.halfLength; } template void computeOBBConvex(const ConvexBaseTpl& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); fit(s.points->data(), s.num_points, bv); bv.axes.applyOnTheLeft(R); bv.To = R * bv.To + T; } template <> void computeBV(const ConvexBase32& s, const Transform3s& tf, OBB& bv) { computeOBBConvex(s, tf, bv); } template <> void computeBV(const ConvexBase16& s, const Transform3s& tf, OBB& bv) { computeOBBConvex(s, tf, bv); } template <> void computeBV(const Halfspace& s, const Transform3s&, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } /// Half space can only have very rough OBB bv.axes.setIdentity(); bv.To.setZero(); bv.extent.setConstant(((std::numeric_limits::max)())); } template <> void computeBV(const Halfspace& s, const Transform3s&, RSS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } /// Half space can only have very rough RSS bv.axes.setIdentity(); bv.Tr.setZero(); bv.length[0] = bv.length[1] = bv.radius = (std::numeric_limits::max)(); } template <> void computeBV(const Halfspace& s, const Transform3s& tf, OBBRSS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); } template <> void computeBV(const Halfspace& s, const Transform3s& tf, kIOS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } bv.num_spheres = 1; computeBV(s, tf, bv.obb); bv.spheres[0].o = Vec3s(); bv.spheres[0].r = (std::numeric_limits::max)(); } template <> void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<16>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } Halfspace new_s = transform(s, tf); const Vec3s& n = new_s.n; const Scalar& d = new_s.d; const short D = 8; for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; } } template <> void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<18>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } Halfspace new_s = transform(s, tf); const Vec3s& n = new_s.n; const Scalar& d = new_s.d; const short D = 9; for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; else bv.dist(8) = n[1] * d * 2; } } template <> void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<24>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } Halfspace new_s = transform(s, tf); const Vec3s& n = new_s.n; const Scalar& d = new_s.d; const short D = 12; for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; else bv.dist(8) = n[1] * d * 2; } else if (n[0] + n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; else bv.dist(9) = n[0] * d * 3; } else if (n[0] + n[1] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; else bv.dist(10) = n[0] * d * 3; } else if (n[0] + n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; else bv.dist(11) = n[1] * d * 3; } } template <> void computeBV(const Plane& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } Vec3s n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; bv.extent << 0, (std::numeric_limits::max)(), (std::numeric_limits::max)(); Vec3s p = s.n * s.d; bv.To = tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T } template <> void computeBV(const Plane& s, const Transform3s& tf, RSS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } Vec3s n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; bv.length[0] = (std::numeric_limits::max)(); bv.length[1] = (std::numeric_limits::max)(); bv.radius = 0; Vec3s p = s.n * s.d; bv.Tr = tf.transform(p); } template <> void computeBV(const Plane& s, const Transform3s& tf, OBBRSS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); } template <> void computeBV(const Plane& s, const Transform3s& tf, kIOS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } bv.num_spheres = 1; computeBV(s, tf, bv.obb); bv.spheres[0].o = Vec3s(); bv.spheres[0].r = (std::numeric_limits::max)(); } template <> void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<16>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } Plane new_s = transform(s, tf); const Vec3s& n = new_s.n; const Scalar& d = new_s.d; const short D = 8; for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) { bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } } template <> void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<18>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } Plane new_s = transform(s, tf); const Vec3s& n = new_s.n; const Scalar& d = new_s.d; const short D = 9; for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; } } template <> void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<24>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } Plane new_s = transform(s, tf); const Vec3s& n = new_s.n; const Scalar& d = new_s.d; const short D = 12; for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } else if (n[0] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; } else if (n[0] + n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) { bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; } else if (n[0] + n[1] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) { bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; } else if (n[0] + n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) { bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; } } void constructBox(const AABB& bv, Box& box, Transform3s& tf) { box = Box(bv.max_ - bv.min_); tf = Transform3s(bv.center()); } void constructBox(const OBB& bv, Box& box, Transform3s& tf) { box = Box(bv.extent * 2); tf = Transform3s(bv.axes, bv.To); } void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf) { box = Box(bv.obb.extent * 2); tf = Transform3s(bv.obb.axes, bv.obb.To); } void constructBox(const kIOS& bv, Box& box, Transform3s& tf) { box = Box(bv.obb.extent * 2); tf = Transform3s(bv.obb.axes, bv.obb.To); } void constructBox(const RSS& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3s(bv.axes, bv.Tr); } void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3s(bv.center()); } void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3s(bv.center()); } void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3s(bv.center()); } void constructBox(const AABB& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf) { box = Box(bv.max_ - bv.min_); tf = tf_bv * Transform3s(bv.center()); } void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf) { box = Box(bv.extent * 2); tf = tf_bv * Transform3s(bv.axes, bv.To); } void constructBox(const OBBRSS& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf) { box = Box(bv.obb.extent * 2); tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To); } void constructBox(const kIOS& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf) { box = Box(bv.obb.extent * 2); tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To); } void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3s(bv.axes, bv.Tr); } void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3s(bv.center()); } void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3s(bv.center()); } void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3s(bv.center()); } } // namespace coal ================================================ FILE: src/traits_traversal.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2019, CNRS-LAAS * All rights reserved. */ /** \author Lucile Remigy, Joseph Mirabel */ #include "coal/collision_func_matrix.h" #include "coal/narrowphase/narrowphase.h" #include <../src/collision_node.h> #include "coal/internal/traversal_node_setup.h" #include "coal/internal/shape_shape_func.h" namespace coal { // TraversalTraitsCollision for collision_func_matrix.cpp template struct COAL_LOCAL TraversalTraitsCollision {}; #ifdef COAL_HAS_OCTOMAP template struct COAL_LOCAL TraversalTraitsCollision { typedef ShapeOcTreeCollisionTraversalNode CollisionTraversal_t; }; template struct COAL_LOCAL TraversalTraitsCollision { typedef OcTreeShapeCollisionTraversalNode CollisionTraversal_t; }; template <> struct COAL_LOCAL TraversalTraitsCollision { typedef OcTreeCollisionTraversalNode CollisionTraversal_t; }; template struct COAL_LOCAL TraversalTraitsCollision > { typedef OcTreeMeshCollisionTraversalNode CollisionTraversal_t; }; template struct COAL_LOCAL TraversalTraitsCollision, OcTree> { typedef MeshOcTreeCollisionTraversalNode CollisionTraversal_t; }; template struct COAL_LOCAL TraversalTraitsCollision > { typedef OcTreeHeightFieldCollisionTraversalNode CollisionTraversal_t; }; template struct COAL_LOCAL TraversalTraitsCollision, OcTree> { typedef HeightFieldOcTreeCollisionTraversalNode CollisionTraversal_t; }; #endif // TraversalTraitsDistance for distance_func_matrix.cpp template struct COAL_LOCAL TraversalTraitsDistance {}; #ifdef COAL_HAS_OCTOMAP template struct COAL_LOCAL TraversalTraitsDistance { typedef ShapeOcTreeDistanceTraversalNode CollisionTraversal_t; }; template struct COAL_LOCAL TraversalTraitsDistance { typedef OcTreeShapeDistanceTraversalNode CollisionTraversal_t; }; template <> struct COAL_LOCAL TraversalTraitsDistance { typedef OcTreeDistanceTraversalNode CollisionTraversal_t; }; template struct COAL_LOCAL TraversalTraitsDistance > { typedef OcTreeMeshDistanceTraversalNode CollisionTraversal_t; }; template struct COAL_LOCAL TraversalTraitsDistance, OcTree> { typedef MeshOcTreeDistanceTraversalNode CollisionTraversal_t; }; #endif } // namespace coal ================================================ FILE: src/traversal/traversal_recurse.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #include "coal/internal/traversal_recurse.h" #include namespace coal { void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, Scalar& sqrDistLowerBound) { Scalar sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0; bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if (l1 && l2) { updateFrontList(front_list, b1, b2); // if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return; node->leafCollides(b1, b2, sqrDistLowerBound); return; } if (node->BVDisjoints(b1, b2, sqrDistLowerBound)) { updateFrontList(front_list, b1, b2); return; } if (node->firstOverSecond(b1, b2)) { unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1); unsigned int c2 = (unsigned int)node->getFirstRightChild(b1); collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1); // early stop is disabled is front_list is used if (node->canStop() && !front_list) return; collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2); sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2); } else { unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2); unsigned int c2 = (unsigned int)node->getSecondRightChild(b2); collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1); // early stop is disabled is front_list is used if (node->canStop() && !front_list) return; collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2); sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2); } } void collisionNonRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list, Scalar& sqrDistLowerBound) { typedef std::pair BVPair_t; // typedef std::stack > Stack_t; typedef std::vector Stack_t; Stack_t pairs; pairs.reserve(1000); sqrDistLowerBound = std::numeric_limits::infinity(); Scalar sdlb = std::numeric_limits::infinity(); pairs.push_back(BVPair_t(0, 0)); while (!pairs.empty()) { unsigned int a = pairs.back().first, b = pairs.back().second; pairs.pop_back(); bool la = node->isFirstNodeLeaf(a); bool lb = node->isSecondNodeLeaf(b); // Leaf / Leaf case if (la && lb) { updateFrontList(front_list, a, b); // TODO should we test the BVs ? // if(node->BVDijsoints(a, b, sdlb)) { // if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb; // continue; //} node->leafCollides(a, b, sdlb); if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb; if (node->canStop() && !front_list) return; continue; } // TODO shouldn't we test the leaf triangle against BV is la != lb // if (la && !lb) { // leaf triangle 1 against BV 2 // } else if (!la && lb) { // BV 1 against leaf triangle 2 // } // Check the BV if (node->BVDisjoints(a, b, sdlb)) { if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb; updateFrontList(front_list, a, b); continue; } if (node->firstOverSecond(a, b)) { unsigned int c1 = (unsigned int)node->getFirstLeftChild(a); unsigned int c2 = (unsigned int)node->getFirstRightChild(a); pairs.push_back(BVPair_t(c2, b)); pairs.push_back(BVPair_t(c1, b)); } else { unsigned int c1 = (unsigned int)node->getSecondLeftChild(b); unsigned int c2 = (unsigned int)node->getSecondRightChild(b); pairs.push_back(BVPair_t(a, c2)); pairs.push_back(BVPair_t(a, c1)); } } } /** Recurse function for self collision * Make sure node is set correctly so that the first and second tree are the * same */ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if (l1 && l2) { updateFrontList(front_list, b1, b2); node->leafComputeDistance(b1, b2); return; } unsigned int a1, a2, c1, c2; if (node->firstOverSecond(b1, b2)) { a1 = (unsigned int)node->getFirstLeftChild(b1); a2 = b2; c1 = (unsigned int)node->getFirstRightChild(b1); c2 = b2; } else { a1 = b1; a2 = (unsigned int)node->getSecondLeftChild(b2); c1 = b1; c2 = (unsigned int)node->getSecondRightChild(b2); } Scalar d1 = node->BVDistanceLowerBound(a1, a2); Scalar d2 = node->BVDistanceLowerBound(c1, c2); if (d2 < d1) { if (!node->canStop(d2)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); if (!node->canStop(d1)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); } else { if (!node->canStop(d1)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); if (!node->canStop(d2)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); } } /** @brief Bounding volume test structure */ struct COAL_LOCAL BVT { /** @brief distance between bvs */ Scalar d; /** @brief bv indices for a pair of bvs in two models */ unsigned int b1, b2; }; /** @brief Comparer between two BVT */ struct COAL_LOCAL BVT_Comparer { bool operator()(const BVT& lhs, const BVT& rhs) const { return lhs.d > rhs.d; } // namespace coal }; struct COAL_LOCAL BVTQ { BVTQ() : qsize(2) {} bool empty() const { return pq.empty(); } size_t size() const { return pq.size(); } const BVT& top() const { return pq.top(); } void push(const BVT& x) { pq.push(x); } void pop() { pq.pop(); } bool full() const { return (pq.size() + 1 >= qsize); } std::priority_queue, BVT_Comparer> pq; /** @brief Queue size */ unsigned int qsize; }; void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, unsigned int qsize) { BVTQ bvtq; bvtq.qsize = qsize; BVT min_test; min_test.b1 = b1; min_test.b2 = b2; while (1) { bool l1 = node->isFirstNodeLeaf(min_test.b1); bool l2 = node->isSecondNodeLeaf(min_test.b2); if (l1 && l2) { updateFrontList(front_list, min_test.b1, min_test.b2); node->leafComputeDistance(min_test.b1, min_test.b2); } else if (bvtq.full()) { // queue should not get two more tests, recur distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize); } else { // queue capacity is not full yet BVT bvt1, bvt2; if (node->firstOverSecond(min_test.b1, min_test.b2)) { unsigned int c1 = (unsigned int)node->getFirstLeftChild(min_test.b1); unsigned int c2 = (unsigned int)node->getFirstRightChild(min_test.b1); bvt1.b1 = c1; bvt1.b2 = min_test.b2; bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2); bvt2.b1 = c2; bvt2.b2 = min_test.b2; bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2); } else { unsigned int c1 = (unsigned int)node->getSecondLeftChild(min_test.b2); unsigned int c2 = (unsigned int)node->getSecondRightChild(min_test.b2); bvt1.b1 = min_test.b1; bvt1.b2 = c1; bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2); bvt2.b1 = min_test.b1; bvt2.b2 = c2; bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2); } bvtq.push(bvt1); bvtq.push(bvt2); } if (bvtq.empty()) break; else { min_test = bvtq.top(); bvtq.pop(); if (node->canStop(min_test.d)) { updateFrontList(front_list, min_test.b1, min_test.b2); break; } } } } void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, const CollisionRequest& /*request*/, CollisionResult& result, BVHFrontList* front_list) { Scalar sqrDistLowerBound = -1, sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0; BVHFrontList::iterator front_iter; BVHFrontList append; for (front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter) { unsigned int b1 = front_iter->left; unsigned int b2 = front_iter->right; bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if (l1 & l2) { front_iter->valid = false; // the front node is no longer valid, in // collideRecurse will add again. collisionRecurse(node, b1, b2, &append, sqrDistLowerBound); } else { if (!node->BVDisjoints(b1, b2, sqrDistLowerBound)) { front_iter->valid = false; if (node->firstOverSecond(b1, b2)) { unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1); unsigned int c2 = (unsigned int)node->getFirstRightChild(b1); collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1); collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2); sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2); } else { unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2); unsigned int c2 = (unsigned int)node->getSecondRightChild(b2); collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1); collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2); sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2); } } } result.updateDistanceLowerBound(sqrt(sqrDistLowerBound)); } // clean the old front list (remove invalid node) for (front_iter = front_list->begin(); front_iter != front_list->end();) { if (!front_iter->valid) front_iter = front_list->erase(front_iter); else ++front_iter; } for (front_iter = append.begin(); front_iter != append.end(); ++front_iter) { front_list->push_back(*front_iter); } } } // namespace coal ================================================ FILE: test/CMakeLists.txt ================================================ # Find Boost.UnitTestFramework if(NOT BUILD_STANDALONE_PYTHON_INTERFACE) find_package(Boost REQUIRED COMPONENTS unit_test_framework filesystem) CONFIG_FILES(fcl_resources/config.h) function(add_coal_test test_name source) set(target_name ${PROJECT_NAME}-${test_name}) ADD_UNIT_TEST(${target_name} ${source}) set_standard_output_directory(${target_name}) target_link_libraries( ${target_name} PUBLIC ${LIBRARY_NAME} Boost::filesystem ${utility_target} ) CXX_FLAGS_BY_COMPILER_FRONTEND( GNU "-Wno-c99-extensions" OUTPUT PRIVATE_OPTIONS ) target_compile_options(${target_name} PRIVATE ${PRIVATE_OPTIONS}) if(COAL_HAS_QHULL) target_compile_definitions(${target_name} PRIVATE COAL_HAS_QHULL) endif() endfunction() include_directories(${CMAKE_CURRENT_BINARY_DIR}) set(utility_target ${PROJECT_NAME}-utility) add_library(${utility_target} STATIC utility.cpp) set_standard_output_directory(${utility_target}) target_link_libraries(${utility_target} PUBLIC ${PROJECT_NAME}) add_coal_test(math math.cpp) add_coal_test(collision collision.cpp) add_coal_test(contact_patch contact_patch.cpp) add_coal_test(distance distance.cpp) add_coal_test(swept_sphere_radius swept_sphere_radius.cpp) add_coal_test(normal_and_nearest_points normal_and_nearest_points.cpp) add_coal_test(distance_lower_bound distance_lower_bound.cpp) add_coal_test(security_margin security_margin.cpp) add_coal_test(geometric_shapes geometric_shapes.cpp) add_coal_test(shape_inflation shape_inflation.cpp) add_coal_test(polygon_convex_hull polygon_convex_hull.cpp) add_coal_test(patch_simplifier patch_simplifier.cpp) #add_coal_test(shape_mesh_consistency shape_mesh_consistency.cpp) add_coal_test(gjk_asserts gjk_asserts.cpp) add_coal_test(frontlist frontlist.cpp) set_tests_properties(${PROJECT_NAME}-frontlist PROPERTIES TIMEOUT 7200) # add_coal_test(sphere_capsule sphere_capsule.cpp) add_coal_test(capsule_capsule capsule_capsule.cpp) add_coal_test(box_box_distance box_box_distance.cpp) add_coal_test(box_box_collision box_box_collision.cpp) add_coal_test(simple simple.cpp) add_coal_test(capsule_box_1 capsule_box_1.cpp) add_coal_test(capsule_box_2 capsule_box_2.cpp) add_coal_test(obb obb.cpp) add_coal_test(convex convex.cpp) add_coal_test(bvh_models bvh_models.cpp) add_coal_test(collision_node_asserts collision_node_asserts.cpp) add_coal_test(hfields hfields.cpp) add_coal_test(profiling profiling.cpp) add_coal_test(gjk gjk.cpp) add_coal_test(accelerated_gjk accelerated_gjk.cpp) add_coal_test(gjk_convergence_criterion gjk_convergence_criterion.cpp) if(COAL_HAS_OCTOMAP) add_coal_test(octree octree.cpp) endif(COAL_HAS_OCTOMAP) add_coal_test(serialization serialization.cpp) add_coal_test(alloca alloca.cpp) # Broadphase add_coal_test(broadphase broadphase.cpp) set_tests_properties(${PROJECT_NAME}-broadphase PROPERTIES WILL_FAIL TRUE) add_coal_test(broadphase_dynamic_AABB_tree broadphase_dynamic_AABB_tree.cpp) add_coal_test(broadphase_collision_1 broadphase_collision_1.cpp) add_coal_test(broadphase_collision_2 broadphase_collision_2.cpp) ## Benchmark set(test_benchmark_target ${PROJECT_NAME}-test-benchmark) add_executable(${test_benchmark_target} benchmark.cpp) set_standard_output_directory(${test_benchmark_target}) target_link_libraries( ${test_benchmark_target} PUBLIC ${utility_target} Boost::filesystem ${PROJECT_NAME} ) endif() ## Python tests if(BUILD_PYTHON_INTERFACE) add_subdirectory(python_unit) endif(BUILD_PYTHON_INTERFACE) ================================================ FILE: test/accelerated_gjk.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2022, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #define BOOST_TEST_MODULE COAL_NESTEROV_GJK #include #include #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/tools.h" #include "utility.h" using coal::Box; using coal::Capsule; using coal::constructPolytopeFromEllipsoid; using coal::ConvexTpl; using coal::Ellipsoid; using coal::GJKSolver; using coal::GJKVariant; using coal::Scalar; using coal::ShapeBase; using coal::SolverScalar; using coal::support_func_guess_t; using coal::Transform3s; using coal::Triangle32; using coal::Vec3ps; using coal::Vec3s; using coal::details::GJK; using coal::details::MinkowskiDiff; using coal::details::SupportOptions; using std::size_t; BOOST_AUTO_TEST_CASE(set_gjk_variant) { GJKSolver solver; GJK gjk(128, Scalar(1e-6)); MinkowskiDiff shape; // Checking defaults BOOST_CHECK(solver.gjk.gjk_variant == GJKVariant::DefaultGJK); BOOST_CHECK(gjk.gjk_variant == GJKVariant::DefaultGJK); BOOST_CHECK(shape.normalize_support_direction == false); // Checking set solver.gjk.gjk_variant = GJKVariant::NesterovAcceleration; gjk.gjk_variant = GJKVariant::NesterovAcceleration; BOOST_CHECK(solver.gjk.gjk_variant == GJKVariant::NesterovAcceleration); BOOST_CHECK(gjk.gjk_variant == GJKVariant::NesterovAcceleration); solver.gjk.gjk_variant = GJKVariant::PolyakAcceleration; gjk.gjk_variant = GJKVariant::PolyakAcceleration; BOOST_CHECK(solver.gjk.gjk_variant == GJKVariant::PolyakAcceleration); BOOST_CHECK(gjk.gjk_variant == GJKVariant::PolyakAcceleration); } BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) { Ellipsoid ellipsoid = Ellipsoid(1, 1, 1); Box box = Box(1, 1, 1); ConvexTpl cvx; MinkowskiDiff mink_diff1; mink_diff1.set(&ellipsoid, &ellipsoid); BOOST_CHECK(mink_diff1.normalize_support_direction == false); MinkowskiDiff mink_diff2; mink_diff2.set(&ellipsoid, &box); BOOST_CHECK(mink_diff2.normalize_support_direction == false); MinkowskiDiff mink_diff3; mink_diff3.set(&cvx, &cvx); BOOST_CHECK(mink_diff3.normalize_support_direction == true); } void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // Solvers unsigned int max_iterations = 128; Scalar tolerance = Scalar(1e-6); GJK gjk(max_iterations, tolerance); GJK gjk_nesterov(max_iterations, tolerance); gjk_nesterov.gjk_variant = GJKVariant::NesterovAcceleration; GJK gjk_polyak(max_iterations, tolerance); gjk_polyak.gjk_variant = GJKVariant::PolyakAcceleration; // Minkowski difference MinkowskiDiff mink_diff; // Generate random transforms size_t n = 1000; Scalar extents[] = {-3., -3., 0, 3., 3., 3.}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Transform3s identity = Transform3s::Identity(); // Same init for both solvers Vec3s init_guess = Vec3s(1, 0, 0); support_func_guess_t init_support_guess; init_support_guess.setZero(); for (size_t i = 0; i < n; ++i) { // No need to take into account swept-sphere radius in supports computation // when using GJK/EPA; after they have converged, these algos will correctly // handle the swept-sphere radius of the shapes. mink_diff.set(&shape0, &shape1, identity, transforms[i]); // Evaluate both solvers twice, make sure they give the same solution GJK::Status res_gjk_1 = gjk.evaluate( mink_diff, init_guess.cast(), init_support_guess); Vec3ps ray_gjk = gjk.ray; GJK::Status res_gjk_2 = gjk.evaluate( mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(res_gjk_1 == res_gjk_2); EIGEN_VECTOR_IS_APPROX(ray_gjk, gjk.ray, SolverScalar(1e-8)); // -------------- // -- Nesterov -- // -------------- GJK::Status res_nesterov_gjk_1 = gjk_nesterov.evaluate( mink_diff, init_guess.cast(), init_support_guess); Vec3ps ray_nesterov = gjk_nesterov.ray; GJK::Status res_nesterov_gjk_2 = gjk_nesterov.evaluate( mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(res_nesterov_gjk_1 == res_nesterov_gjk_2); EIGEN_VECTOR_IS_APPROX(ray_nesterov, gjk_nesterov.ray, SolverScalar(1e-8)); // Make sure GJK and Nesterov accelerated GJK find the same distance between // the shapes BOOST_CHECK(res_nesterov_gjk_1 == res_gjk_1); BOOST_CHECK_SMALL(fabs(ray_gjk.norm() - ray_nesterov.norm()), SolverScalar(1e-4)); // Make sure GJK and Nesterov accelerated GJK converges in a reasonable // amount of iterations BOOST_CHECK(gjk.getNumIterations() < max_iterations); BOOST_CHECK(gjk_nesterov.getNumIterations() < max_iterations); // ------------ // -- Polyak -- // ------------ GJK::Status res_polyak_gjk_1 = gjk_polyak.evaluate( mink_diff, init_guess.cast(), init_support_guess); Vec3ps ray_polyak = gjk_polyak.ray; GJK::Status res_polyak_gjk_2 = gjk_polyak.evaluate( mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(res_polyak_gjk_1 == res_polyak_gjk_2); EIGEN_VECTOR_IS_APPROX(ray_polyak, gjk_polyak.ray, SolverScalar(1e-8)); // Make sure GJK and Polyak accelerated GJK find the same distance between // the shapes BOOST_CHECK(res_polyak_gjk_1 == res_gjk_1); BOOST_CHECK_SMALL(fabs(ray_gjk.norm() - ray_polyak.norm()), SolverScalar(1e-4)); // Make sure GJK and Polyak accelerated GJK converges in a reasonable // amount of iterations BOOST_CHECK(gjk.getNumIterations() < max_iterations); BOOST_CHECK(gjk_polyak.getNumIterations() < max_iterations); } } BOOST_AUTO_TEST_CASE(ellipsoid_ellipsoid) { Ellipsoid ellipsoid0 = Ellipsoid(Scalar(0.3), Scalar(0.4), Scalar(0.5)); Ellipsoid ellipsoid1 = Ellipsoid(Scalar(1.5), Scalar(1.4), Scalar(1.3)); test_accelerated_gjk(ellipsoid0, ellipsoid1); test_accelerated_gjk(ellipsoid0, ellipsoid1); } BOOST_AUTO_TEST_CASE(ellipsoid_capsule) { Ellipsoid ellipsoid0 = Ellipsoid(Scalar(0.5), Scalar(0.4), Scalar(0.3)); Ellipsoid ellipsoid1 = Ellipsoid(Scalar(1.5), Scalar(1.4), Scalar(1.3)); Capsule capsule0 = Capsule(Scalar(0.1), Scalar(0.3)); Capsule capsule1 = Capsule(Scalar(1.1), Scalar(1.3)); test_accelerated_gjk(ellipsoid0, capsule0); test_accelerated_gjk(ellipsoid0, capsule1); test_accelerated_gjk(ellipsoid1, capsule0); test_accelerated_gjk(ellipsoid1, capsule1); } BOOST_AUTO_TEST_CASE(ellipsoid_box) { Ellipsoid ellipsoid0 = Ellipsoid(Scalar(0.5), Scalar(0.4), Scalar(0.3)); Ellipsoid ellipsoid1 = Ellipsoid(Scalar(1.5), Scalar(1.4), Scalar(1.3)); Box box0 = Box(Scalar(0.1), Scalar(0.2), Scalar(0.3)); Box box1 = Box(Scalar(1.1), Scalar(1.2), Scalar(1.3)); test_accelerated_gjk(ellipsoid0, box0); test_accelerated_gjk(ellipsoid0, box1); test_accelerated_gjk(ellipsoid1, box0); test_accelerated_gjk(ellipsoid1, box1); } BOOST_AUTO_TEST_CASE(ellipsoid_mesh) { Ellipsoid ellipsoid0 = Ellipsoid(Scalar(0.5), Scalar(0.4), Scalar(0.3)); Ellipsoid ellipsoid1 = Ellipsoid(Scalar(1.5), Scalar(1.4), Scalar(1.3)); ConvexTpl cvx0 = constructPolytopeFromEllipsoid(ellipsoid0); ConvexTpl cvx1 = constructPolytopeFromEllipsoid(ellipsoid1); test_accelerated_gjk(ellipsoid0, cvx0); test_accelerated_gjk(ellipsoid0, cvx1); test_accelerated_gjk(ellipsoid1, cvx0); test_accelerated_gjk(ellipsoid1, cvx1); } BOOST_AUTO_TEST_CASE(capsule_mesh) { Ellipsoid ellipsoid0 = Ellipsoid(Scalar(0.5), Scalar(0.4), Scalar(0.3)); Ellipsoid ellipsoid1 = Ellipsoid(Scalar(1.5), Scalar(1.4), Scalar(1.3)); ConvexTpl cvx0 = constructPolytopeFromEllipsoid(ellipsoid0); ConvexTpl cvx1 = constructPolytopeFromEllipsoid(ellipsoid1); Capsule capsule0 = Capsule(Scalar(0.1), Scalar(0.3)); Capsule capsule1 = Capsule(Scalar(1.1), Scalar(1.3)); test_accelerated_gjk(capsule0, cvx0); test_accelerated_gjk(capsule0, cvx1); test_accelerated_gjk(capsule1, cvx0); test_accelerated_gjk(capsule1, cvx1); } BOOST_AUTO_TEST_CASE(capsule_capsule) { Capsule capsule0 = Capsule(Scalar(0.1), Scalar(0.3)); Capsule capsule1 = Capsule(Scalar(1.1), Scalar(1.3)); test_accelerated_gjk(capsule0, capsule0); test_accelerated_gjk(capsule1, capsule1); test_accelerated_gjk(capsule0, capsule1); } BOOST_AUTO_TEST_CASE(box_box) { Box box0 = Box(Scalar(0.1), Scalar(0.2), Scalar(0.3)); Box box1 = Box(Scalar(1.1), Scalar(1.2), Scalar(1.3)); test_accelerated_gjk(box0, box0); test_accelerated_gjk(box0, box1); test_accelerated_gjk(box1, box1); } BOOST_AUTO_TEST_CASE(box_mesh) { Box box0 = Box(Scalar(0.1), Scalar(0.2), Scalar(0.3)); Box box1 = Box(Scalar(1.1), Scalar(1.2), Scalar(1.3)); Ellipsoid ellipsoid0 = Ellipsoid(Scalar(0.5), Scalar(0.4), Scalar(0.3)); Ellipsoid ellipsoid1 = Ellipsoid(Scalar(1.5), Scalar(1.4), Scalar(1.3)); ConvexTpl cvx0 = constructPolytopeFromEllipsoid(ellipsoid0); ConvexTpl cvx1 = constructPolytopeFromEllipsoid(ellipsoid1); test_accelerated_gjk(box0, cvx0); test_accelerated_gjk(box0, cvx1); test_accelerated_gjk(box1, cvx0); test_accelerated_gjk(box1, cvx1); } BOOST_AUTO_TEST_CASE(mesh_mesh) { Ellipsoid ellipsoid0 = Ellipsoid(Scalar(0.5), Scalar(0.4), Scalar(0.3)); Ellipsoid ellipsoid1 = Ellipsoid(Scalar(1.5), Scalar(1.4), Scalar(1.3)); ConvexTpl cvx0 = constructPolytopeFromEllipsoid(ellipsoid0); ConvexTpl cvx1 = constructPolytopeFromEllipsoid(ellipsoid1); test_accelerated_gjk(cvx0, cvx0); test_accelerated_gjk(cvx0, cvx1); test_accelerated_gjk(cvx1, cvx1); } ================================================ FILE: test/alloca.cpp ================================================ // // Copyright (c) 2026 INRIA // #define BOOST_TEST_MODULE COAL_ALLOCA #include #include "coal/data_types.h" #include "coal/alloca.h" using namespace coal; BOOST_AUTO_TEST_CASE(copy_std_vector) { for (std::size_t i = 0; i < 10; ++i) { std::vector vec; for (std::size_t i = 0; i < 20; ++i) { vec.emplace_back(Vec2s::Random()); } // temporary alloca copy Vec2s* tmp_vec_copy = COAL_ALLOCA_TYPED_PTR(Vec2s, vec.size()); for (std::size_t i = 0; i < vec.size(); ++i) { tmp_vec_copy[i] = vec[i]; } // copy from alloca tmp std::vector vec_copy; for (std::size_t i = 0; i < vec.size(); ++i) { vec_copy.emplace_back(tmp_vec_copy[i]); } // check original and copy match BOOST_CHECK(vec_copy.size() == vec.size()); for (std::size_t i = 0; i < vec_copy.size(); ++i) { BOOST_CHECK(vec[i] == vec_copy[i]); } } } ================================================ FILE: test/benchmark/test_fcl_gjk.output ================================================ Result after running gjk in Release mode. nCol = 2840 nDiff = 0 Total time gjk: 20020 -- Collisions ------------------------- Total time gjk: 5554 -- No collisions ------------------------- Total time gjk: 14466 ================================================ FILE: test/benchmark.cpp ================================================ // Copyright (c) 2016, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // // This file is part of Coal. // Coal 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 // 3 of the License, or (at your option) any later version. // // Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // Coal. If not, see . #include #include "coal/internal/traversal_node_setup.h" #include "coal/internal/traversal_node_bvhs.h" #include "../src/collision_node.h" #include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" #define RUN_CASE(BV, tf, models, split) \ run(tf, models, split, #BV " - " #split ":\t") using namespace coal; bool verbose = false; Scalar DELTA = Scalar(0.001); template void makeModel(const std::vector& vertices, const std::vector& triangles, SplitMethodType split_method, BVHModel& model); template double distance(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose); template double collide(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose); template double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* sm_name); template struct traits {}; template <> struct traits { typedef MeshCollisionTraversalNodeRSS CollisionTraversalNode; typedef MeshDistanceTraversalNodeRSS DistanceTraversalNode; }; template <> struct traits { typedef MeshCollisionTraversalNodekIOS CollisionTraversalNode; typedef MeshDistanceTraversalNodekIOS DistanceTraversalNode; }; template <> struct traits { typedef MeshCollisionTraversalNodeOBB CollisionTraversalNode; // typedef MeshDistanceTraversalNodeOBB DistanceTraversalNode; }; template <> struct traits { typedef MeshCollisionTraversalNodeOBBRSS CollisionTraversalNode; typedef MeshDistanceTraversalNodeOBBRSS DistanceTraversalNode; }; template void makeModel(const std::vector& vertices, const std::vector& triangles, SplitMethodType split_method, BVHModel& model) { model.bv_splitter.reset(new BVSplitter(split_method)); model.bv_splitter.reset(new BVSplitter(split_method)); model.beginModel(); model.addSubModel(vertices, triangles); model.endModel(); } template double distance(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose) { Transform3s pose2; DistanceResult local_result; DistanceRequest request(true); TraversalNode node; node.enable_statistics = verbose; BenchTimer timer; timer.start(); for (std::size_t i = 0; i < tf.size(); ++i) { if (!initialize(node, m1, tf[i], m2, pose2, request, local_result)) std::cout << "initialize error" << std::endl; distance(&node, NULL); } return timer.getElapsedTimeInMicroSec(); } template double collide(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose) { Transform3s pose2; CollisionResult local_result; CollisionRequest request; TraversalNode node(request); node.enable_statistics = verbose; BenchTimer timer; timer.start(); for (std::size_t i = 0; i < tf.size(); ++i) { bool success(initialize(node, m1, tf[i], m2, pose2, local_result)); (void)success; assert(success); CollisionResult result; collide(&node, request, result); } return timer.getElapsedTimeInMicroSec(); } template double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* prefix) { double col = collide::CollisionTraversalNode>( tf, models[0][split_method], models[1][split_method], verbose); double dist = distance::DistanceTraversalNode>( tf, models[0][split_method], models[1][split_method], verbose); std::cout << prefix << " (" << col << ", " << dist << ")\n"; return col + dist; } template <> double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* prefix) { double col = collide::CollisionTraversalNode>( tf, models[0][split_method], models[1][split_method], verbose); double dist = 0; std::cout << prefix << " (\t" << col << ", \tNaN)\n"; return col + dist; } int main(int, char*[]) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); // Make models BVHModel ms_rss[2][3]; makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_rss[0][SPLIT_METHOD_MEAN]); makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_rss[0][SPLIT_METHOD_BV_CENTER]); makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_rss[0][SPLIT_METHOD_MEDIAN]); makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_rss[1][SPLIT_METHOD_MEAN]); makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_rss[1][SPLIT_METHOD_BV_CENTER]); makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_rss[1][SPLIT_METHOD_MEDIAN]); BVHModel ms_kios[2][3]; makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_kios[0][SPLIT_METHOD_MEAN]); makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_kios[0][SPLIT_METHOD_BV_CENTER]); makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_kios[0][SPLIT_METHOD_MEDIAN]); makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_kios[1][SPLIT_METHOD_MEAN]); makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_kios[1][SPLIT_METHOD_BV_CENTER]); makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_kios[1][SPLIT_METHOD_MEDIAN]); BVHModel ms_obb[2][3]; makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obb[0][SPLIT_METHOD_MEAN]); makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_obb[0][SPLIT_METHOD_BV_CENTER]); makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obb[0][SPLIT_METHOD_MEDIAN]); makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obb[1][SPLIT_METHOD_MEAN]); makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_obb[1][SPLIT_METHOD_BV_CENTER]); makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obb[1][SPLIT_METHOD_MEDIAN]); BVHModel ms_obbrss[2][3]; makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obbrss[0][SPLIT_METHOD_MEAN]); makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_obbrss[0][SPLIT_METHOD_BV_CENTER]); makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obbrss[0][SPLIT_METHOD_MEDIAN]); makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obbrss[1][SPLIT_METHOD_MEAN]); makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_obbrss[1][SPLIT_METHOD_BV_CENTER]); makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]); std::vector transforms; // t0 Scalar extents[] = {-3000, -3000, -3000, 3000, 3000, 3000}; std::size_t n = 10000; generateRandomTransforms(extents, transforms, n); double total_time = 0; total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_MEAN); total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_BV_CENTER); total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_MEDIAN); total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_MEAN); total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_BV_CENTER); total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_MEDIAN); total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_MEAN); total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_BV_CENTER); total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_MEDIAN); total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_MEAN); total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_BV_CENTER); total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_MEDIAN); std::cout << "\n\nTotal time: " << total_time << std::endl; } ================================================ FILE: test/box_box_collision.cpp ================================================ #define BOOST_TEST_MODULE COAL_BOX_BOX_COLLISION #include #include #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/tools.h" #include "utility.h" using coal::Box; using coal::collide; using coal::CollisionRequest; using coal::CollisionResult; using coal::ComputeCollision; using coal::Scalar; using coal::Transform3s; using coal::Vec3s; BOOST_AUTO_TEST_CASE(box_box_collision) { // Define boxes Box shape1(1, 1, 1); Box shape2(1, 1, 1); // Define transforms Transform3s T1 = Transform3s::Identity(); Transform3s T2 = Transform3s::Identity(); // Compute collision CollisionRequest req; req.enable_cached_gjk_guess = true; req.distance_upper_bound = Scalar(1e-6); CollisionResult res; ComputeCollision collide_functor(&shape1, &shape2); T1.setTranslation(Vec3s(0, 0, 0)); res.clear(); BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == true); res.clear(); BOOST_CHECK(collide_functor(T1, T2, req, res) == true); T1.setTranslation(Vec3s(2, 0, 0)); res.clear(); BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == false); res.clear(); BOOST_CHECK(collide_functor(T1, T2, req, res) == false); } ================================================ FILE: test/box_box_distance.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2014-2016, CNRS-LAAS. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #define _USE_MATH_DEFINES #include #define BOOST_TEST_MODULE COAL_BOX_BOX #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include #include #include "coal/distance.h" #include "coal/math/transform.h" #include "coal/collision.h" #include "coal/collision_object.h" #include "coal/shape/geometric_shapes.h" #include "utility.h" using coal::CollisionGeometryPtr_t; using coal::CollisionObject; using coal::DistanceRequest; using coal::DistanceResult; using coal::Scalar; using coal::Transform3s; using coal::Vec3s; BOOST_AUTO_TEST_CASE(distance_box_box_1) { CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); Transform3s tf1; Transform3s tf2(Vec3s(25, 20, 5)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); // Enable computation of nearest points DistanceRequest distanceRequest(true, true, 0, 0); DistanceResult distanceResult; coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl << " R1 = " << tf1.getRotation() << std::endl << " T2 = " << tf2.getTranslation() << std::endl << " R2 = " << tf2.getRotation() << std::endl; std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; Scalar dx = 25 - 3 - 1; Scalar dy = 20 - 5 - 1; Scalar dz = 5 - 1 - 1; const Vec3s& p1 = distanceResult.nearest_points[0]; const Vec3s& p2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, sqrt(dx * dx + dy * dy + dz * dz), 1e-4); BOOST_CHECK_CLOSE(p1[0], 3, 1e-6); BOOST_CHECK_CLOSE(p1[1], 5, 1e-6); BOOST_CHECK_CLOSE(p1[2], 1, 1e-6); BOOST_CHECK_CLOSE(p2[0], 24, 1e-6); BOOST_CHECK_CLOSE(p2[1], 19, 1e-6); BOOST_CHECK_CLOSE(p2[2], 4, 1e-6); } BOOST_AUTO_TEST_CASE(distance_box_box_2) { CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); static Scalar pi = Scalar(M_PI); Transform3s tf1; Transform3s tf2( coal::makeQuat(cos(Scalar(pi / 8)), sin(Scalar(pi / 8)) / sqrt(Scalar(3)), sin(Scalar(pi / 8)) / sqrt(Scalar(3)), sin(Scalar(pi / 8)) / sqrt(Scalar(3))), Vec3s(0, 0, 10)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); // Enable computation of nearest points DistanceRequest distanceRequest(true, true, 0, 0); DistanceResult distanceResult; coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl << " R1 = " << tf1.getRotation() << std::endl << " T2 = " << tf2.getTranslation() << std::endl << " R2 = " << tf2.getRotation() << std::endl; std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; const Vec3s& p1 = distanceResult.nearest_points[0]; const Vec3s& p2 = distanceResult.nearest_points[1]; Scalar distance = Scalar(-1.62123444 + 10 - 1); BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); BOOST_CHECK_CLOSE(p1[0], 0.60947571, 1e-4); BOOST_CHECK_CLOSE(p1[1], 0.01175873, 1e-4); BOOST_CHECK_CLOSE(p1[2], 1, 1e-6); BOOST_CHECK_CLOSE(p2[0], 0.60947571, 1e-4); BOOST_CHECK_CLOSE(p2[1], 0.01175873, 1e-4); BOOST_CHECK_CLOSE(p2[2], -1.62123444 + 10, 1e-4); } BOOST_AUTO_TEST_CASE(distance_box_box_3) { CollisionGeometryPtr_t s1(new coal::Box(1, 1, 1)); CollisionGeometryPtr_t s2(new coal::Box(1, 1, 1)); static Scalar pi = Scalar(M_PI); Transform3s tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), Vec3s(-2, 1, .5)); Transform3s tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), Vec3s(2, .5, .5)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); // Enable computation of nearest points DistanceRequest distanceRequest(true, 0, 0); DistanceResult distanceResult; coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl << " R1 = " << tf1.getRotation() << std::endl << " T2 = " << tf2.getTranslation() << std::endl << " R2 = " << tf2.getRotation() << std::endl; std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; const Vec3s& p1 = distanceResult.nearest_points[0]; const Vec3s& p2 = distanceResult.nearest_points[1]; Scalar distance = Scalar(4 - sqrt(2)); BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); const Vec3s p1Ref(sqrt(Scalar(2)) / 2 - 2, 1, .5); const Vec3s p2Ref(2 - sqrt(Scalar(2)) / 2, 1, .5); BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4); BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4); BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4); BOOST_CHECK_CLOSE(p2[0], p2Ref[0], 1e-4); BOOST_CHECK_CLOSE(p2[1], p2Ref[1], 1e-4); BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4); // Apply the same global transform to both objects and recompute Transform3s tf3( coal::makeQuat(Scalar(0.435952844074), Scalar(-0.718287018243), Scalar(0.310622451066), Scalar(0.444435113443)), Vec3s(4, 5, 6)); tf1 = tf3 * tf1; tf2 = tf3 * tf2; o1 = CollisionObject(s1, tf1); o2 = CollisionObject(s2, tf2); distanceResult.clear(); coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl << " R1 = " << tf1.getRotation() << std::endl << " T2 = " << tf2.getTranslation() << std::endl << " R2 = " << tf2.getRotation() << std::endl; std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); const Vec3s p1Moved = tf3.transform(p1Ref); const Vec3s p2Moved = tf3.transform(p2Ref); BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4); BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4); BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4); BOOST_CHECK_CLOSE(p2[0], p2Moved[0], 1e-4); BOOST_CHECK_CLOSE(p2[1], p2Moved[1], 1e-4); BOOST_CHECK_CLOSE(p2[2], p2Moved[2], 1e-4); } BOOST_AUTO_TEST_CASE(distance_box_box_4) { coal::Box s1(1, 1, 1); coal::Box s2(1, 1, 1); // Enable computation of nearest points DistanceRequest distanceRequest(true, true, 0, 0); DistanceResult distanceResult; Scalar distance; Transform3s tf1(Vec3s(2, 0, 0)); Transform3s tf2; coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 1.; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); tf1.setTranslation(Vec3s(Scalar(1.01), 0, 0)); distanceResult.clear(); coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = Scalar(0.01); BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); tf1.setTranslation(Vec3s(Scalar(0.99), 0, 0)); distanceResult.clear(); coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = Scalar(-0.01); BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); tf1.setTranslation(Vec3s(0, 0, 0)); distanceResult.clear(); coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = -1; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); } ================================================ FILE: test/broadphase.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE COAL_BROADPHASE #include #include "coal/config.hh" #include "coal/broadphase/broadphase.h" #include "coal/shape/geometric_shape_to_BVH_model.h" #include "coal/math/transform.h" #include "utility.h" #if USE_GOOGLEHASH #include #include #include #endif #include #include #include using namespace coal; using namespace coal::detail; /// @brief Generate environment with 3 * n objects for self distance, so we try /// to make sure none of them collide with each other. void generateSelfDistanceEnvironments(std::vector& env, Scalar env_scale, std::size_t n); /// @brief Generate environment with 3 * n objects for self distance, but all in /// meshes. void generateSelfDistanceEnvironmentsMesh(std::vector& env, Scalar env_scale, std::size_t n); /// @brief test for broad phase distance void broad_phase_distance_test(Scalar env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh = false); /// @brief test for broad phase self distance void broad_phase_self_distance_test(Scalar env_scale, std::size_t env_size, bool use_mesh = false); Scalar DELTA = Scalar(0.01); #if USE_GOOGLEHASH template struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; template struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > { GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() { this->set_empty_key(NULL); } }; #endif // TODO(jcarpent): fix this test // - test_core_bf_broad_phase_distance /// check broad phase distance BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance) { #ifndef NDEBUG broad_phase_distance_test(200, 100, 10); broad_phase_distance_test(200, 1000, 10); broad_phase_distance_test(2000, 100, 10); broad_phase_distance_test(2000, 1000, 10); #else broad_phase_distance_test(200, 100, 100); broad_phase_distance_test(200, 1000, 100); broad_phase_distance_test(2000, 100, 100); broad_phase_distance_test(2000, 1000, 100); #endif } /// check broad phase self distance BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance) { broad_phase_self_distance_test(200, 512); broad_phase_self_distance_test(200, 1000); broad_phase_self_distance_test(200, 5000); } /// check broad phase distance BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh) { #ifndef NDEBUG broad_phase_distance_test(200, 10, 10, true); broad_phase_distance_test(200, 100, 10, true); broad_phase_distance_test(2000, 10, 10, true); broad_phase_distance_test(2000, 100, 10, true); #else broad_phase_distance_test(200, 100, 100, true); broad_phase_distance_test(200, 1000, 100, true); broad_phase_distance_test(2000, 100, 100, true); broad_phase_distance_test(2000, 1000, 100, true); #endif } /// check broad phase self distance BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_self_distance_mesh) { broad_phase_self_distance_test(200, 512, true); broad_phase_self_distance_test(200, 1000, true); broad_phase_self_distance_test(200, 5000, true); } void generateSelfDistanceEnvironments(std::vector& env, Scalar env_scale, std::size_t n) { int n_edge = static_cast(std::floor(std::pow(n, 1 / 3.0))); Scalar step_size = env_scale * 2 / Scalar(n_edge); Scalar delta_size = step_size * Scalar(0.05); Scalar single_size = step_size - 2 * delta_size; int i = 0; for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Box* box = new Box(single_size, single_size, single_size); const Scalar half = Scalar(0.5); env.push_back(new CollisionObject( shared_ptr(box), Transform3s(Vec3s( Scalar(x) * step_size + delta_size + half * single_size - env_scale, Scalar(y) * step_size + delta_size + half * single_size - env_scale, Scalar(z) * step_size + delta_size + half * single_size - env_scale)))); env.back()->collisionGeometry()->computeLocalAABB(); } for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Sphere* sphere = new Sphere(single_size / 2); const Scalar half = Scalar(0.5); env.push_back(new CollisionObject( shared_ptr(sphere), Transform3s(Vec3s( Scalar(x) * step_size + delta_size + half * single_size - env_scale, Scalar(y) * step_size + delta_size + half * single_size - env_scale, Scalar(z) * step_size + delta_size + half * single_size - env_scale)))); env.back()->collisionGeometry()->computeLocalAABB(); } for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cylinder* cylinder = new Cylinder(single_size / 2, single_size); const Scalar half = Scalar(0.5); env.push_back(new CollisionObject( shared_ptr(cylinder), Transform3s(Vec3s( Scalar(x) * step_size + delta_size + half * single_size - env_scale, Scalar(y) * step_size + delta_size + half * single_size - env_scale, Scalar(z) * step_size + delta_size + half * single_size - env_scale)))); env.back()->collisionGeometry()->computeLocalAABB(); } for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cone* cone = new Cone(single_size / 2, single_size); const Scalar half = Scalar(0.5); env.push_back(new CollisionObject( shared_ptr(cone), Transform3s(Vec3s( Scalar(x) * step_size + delta_size + half * single_size - env_scale, Scalar(y) * step_size + delta_size + half * single_size - env_scale, Scalar(z) * step_size + delta_size + half * single_size - env_scale)))); env.back()->collisionGeometry()->computeLocalAABB(); } } void generateSelfDistanceEnvironmentsMesh(std::vector& env, Scalar env_scale, std::size_t n) { int n_edge = static_cast(std::floor(std::pow(n, 1 / 3.0))); Scalar step_size = env_scale * 2 / Scalar(n_edge); Scalar delta_size = step_size * Scalar(0.05); Scalar single_size = step_size - 2 * delta_size; int i = 0; for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Box box(single_size, single_size, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3s()); const Scalar half = Scalar(0.5); env.push_back(new CollisionObject( shared_ptr(model), Transform3s(Vec3s( Scalar(x) * step_size + delta_size + half * single_size - env_scale, Scalar(y) * step_size + delta_size + half * single_size - env_scale, Scalar(z) * step_size + delta_size + half * single_size - env_scale)))); env.back()->collisionGeometry()->computeLocalAABB(); } for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Sphere sphere(single_size / 2); BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3s(), 16, 16); const Scalar half = Scalar(0.5); env.push_back(new CollisionObject( shared_ptr(model), Transform3s(Vec3s( Scalar(x) * step_size + delta_size + half * single_size - env_scale, Scalar(y) * step_size + delta_size + half * single_size - env_scale, Scalar(z) * step_size + delta_size + half * single_size - env_scale)))); env.back()->collisionGeometry()->computeLocalAABB(); } for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cylinder cylinder(single_size / 2, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3s(), 16, 16); const Scalar half = Scalar(0.5); env.push_back(new CollisionObject( shared_ptr(model), Transform3s(Vec3s( Scalar(x) * step_size + delta_size + half * single_size - env_scale, Scalar(y) * step_size + delta_size + half * single_size - env_scale, Scalar(z) * step_size + delta_size + half * single_size - env_scale)))); env.back()->collisionGeometry()->computeLocalAABB(); } for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cone cone(single_size / 2, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, cone, Transform3s(), 16, 16); const Scalar half = Scalar(0.5); env.push_back(new CollisionObject( shared_ptr(model), Transform3s(Vec3s( Scalar(x) * step_size + delta_size + half * single_size - env_scale, Scalar(y) * step_size + delta_size + half * single_size - env_scale, Scalar(z) * step_size + delta_size + half * single_size - env_scale)))); env.back()->collisionGeometry()->computeLocalAABB(); } } void broad_phase_self_distance_test(Scalar env_scale, std::size_t env_size, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; if (use_mesh) generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size); else generateSelfDistanceEnvironments(env, env_scale, env_size); std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, (upper_limit[1] - lower_limit[1]) / 5), (upper_limit[2] - lower_limit[2]) / 5); // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, // lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager< SparseHashTable >( cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back( new SpatialHashingCollisionManager >( cell_size, lower_limit, upper_limit)); managers.push_back( new SpatialHashingCollisionManager >( cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeArrayCollisionManager()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeArrayCollisionManager* m = new DynamicAABBTreeArrayCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } std::vector self_callbacks(managers.size()); for (size_t i = 0; i < self_callbacks.size(); ++i) { timers[i].start(); managers[i]->distance(&self_callbacks[i]); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); // std::cout << self_data[i].result.min_distance << " "; } // std::cout << std::endl; for (size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(fabs(self_callbacks[0].data.result.min_distance - self_callbacks[i].data.result.min_distance) < DELTA || fabs(self_callbacks[0].data.result.min_distance - self_callbacks[i].data.result.min_distance) / fabs(self_callbacks[0].data.result.min_distance) < DELTA); for (size_t i = 0; i < env.size(); ++i) delete env[i]; for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); int w = 7; std::cout << "self distance timing summary" << std::endl; std::cout << env.size() << " objs" << std::endl; std::cout << "register time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "self distance time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "overall time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } void broad_phase_distance_test(Scalar env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; if (use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; BroadPhaseCollisionManager* manager = new NaiveCollisionManager(); for (std::size_t i = 0; i < env.size(); ++i) manager->registerObject(env[i]); manager->setup(); while (1) { std::vector candidates; if (use_mesh) generateEnvironmentsMesh(candidates, env_scale, query_size); else generateEnvironments(candidates, env_scale, query_size); for (std::size_t i = 0; i < candidates.size(); ++i) { CollisionCallBackDefault callback; manager->collide(candidates[i], &callback); if (callback.data.result.numContacts() == 0) query.push_back(candidates[i]); else delete candidates[i]; if (query.size() == query_size) break; } if (query.size() == query_size) break; } delete manager; std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, // lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager< SparseHashTable >( cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back( new SpatialHashingCollisionManager >( cell_size, lower_limit, upper_limit)); managers.push_back( new SpatialHashingCollisionManager >( cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeArrayCollisionManager()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeArrayCollisionManager* m = new DynamicAABBTreeArrayCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (size_t i = 0; i < query.size(); ++i) { std::vector query_callbacks(managers.size()); for (size_t j = 0; j < managers.size(); ++j) { timers[j].start(); managers[j]->distance(query[i], &query_callbacks[j]); const auto elapsed_s = timers[j].getElapsedTime(); ts[j].push_back(elapsed_s); std::cout << query_callbacks[j].data.result.min_distance << " "; } std::cout << std::endl; for (size_t j = 1; j < managers.size(); ++j) { bool test = fabs(query_callbacks[0].data.result.min_distance - query_callbacks[j].data.result.min_distance) < DELTA || fabs(query_callbacks[0].data.result.min_distance - query_callbacks[j].data.result.min_distance) / fabs(query_callbacks[0].data.result.min_distance) < DELTA; BOOST_CHECK(test); if (!test) { const BroadPhaseCollisionManager& self = *managers[j]; std::cout << "j: " << typeid(self).name() << std::endl; std::cout << "query_callbacks[0].data.result.min_distance: " << query_callbacks[0].data.result.min_distance << std::endl; std::cout << "query_callbacks[j].data.result.min_distance: " << query_callbacks[j].data.result.min_distance << std::endl; } } } for (std::size_t i = 0; i < env.size(); ++i) delete env[i]; for (std::size_t i = 0; i < query.size(); ++i) delete query[i]; for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); int w = 7; std::cout << "distance timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "distance time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { double tmp = 0; for (size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } ================================================ FILE: test/broadphase_collision_1.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * Copyright (c) 2022, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_1 #include #include "coal/broadphase/broadphase_bruteforce.h" #include "coal/broadphase/broadphase_spatialhash.h" #include "coal/broadphase/broadphase_SaP.h" #include "coal/broadphase/broadphase_SSaP.h" #include "coal/broadphase/broadphase_interval_tree.h" #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #include "coal/broadphase/default_broadphase_callbacks.h" #include "coal/broadphase/detail/sparse_hash_table.h" #include "coal/broadphase/detail/spatial_hash.h" #include "utility.h" #include #if USE_GOOGLEHASH #include #include #include #endif #include #include using namespace coal; /// @brief make sure if broadphase algorithms doesn't check twice for the same /// collision object pair void broad_phase_duplicate_check_test(Scalar env_scale, std::size_t env_size, bool verbose = false); /// @brief test for broad phase update void broad_phase_update_collision_test(Scalar env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); #if USE_GOOGLEHASH template struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to> {}; template struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to> { GoogleDenseHashTable() : google::dense_hash_map, std::equal_to>() { this->set_empty_key(nullptr); } }; #endif /// make sure if broadphase algorithms doesn't check twice for the same /// collision object pair BOOST_AUTO_TEST_CASE(test_broad_phase_dont_duplicate_check) { #ifdef NDEBUG broad_phase_duplicate_check_test(2000, 1000); #else broad_phase_duplicate_check_test(2000, 100); #endif } /// check the update, only return collision or not BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_binary) { #ifdef NDEBUG broad_phase_update_collision_test(2000, 100, 1000, 1, false); broad_phase_update_collision_test(2000, 1000, 1000, 1, false); #else broad_phase_update_collision_test(2000, 10, 100, 1, false); broad_phase_update_collision_test(2000, 100, 100, 1, false); #endif } /// check the update, return 10 contacts BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision) { #ifdef NDEBUG broad_phase_update_collision_test(2000, 100, 1000, 10, false); broad_phase_update_collision_test(2000, 1000, 1000, 10, false); #else broad_phase_update_collision_test(2000, 10, 100, 10, false); broad_phase_update_collision_test(2000, 100, 100, 10, false); #endif } /// check the update, exhaustive BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_exhaustive) { #ifdef NDEBUG broad_phase_update_collision_test(2000, 100, 1000, 1, true); broad_phase_update_collision_test(2000, 1000, 1000, 1, true); #else broad_phase_update_collision_test(2000, 10, 100, 1, true); broad_phase_update_collision_test(2000, 100, 100, 1, true); #endif } /// check broad phase update, in mesh, only return collision or not BOOST_AUTO_TEST_CASE( test_core_mesh_bf_broad_phase_update_collision_mesh_binary) { #ifdef NDEBUG broad_phase_update_collision_test(2000, 100, 1000, 1, false, true); broad_phase_update_collision_test(2000, 1000, 1000, 1, false, true); #else broad_phase_update_collision_test(2000, 2, 4, 1, false, true); broad_phase_update_collision_test(2000, 4, 4, 1, false, true); #endif } /// check broad phase update, in mesh, return 10 contacts BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh) { #ifdef NDEBUG broad_phase_update_collision_test(2000, 100, 1000, 10, false, true); broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true); #else broad_phase_update_collision_test(200, 2, 4, 10, false, true); broad_phase_update_collision_test(200, 4, 4, 10, false, true); #endif } /// check broad phase update, in mesh, exhaustive BOOST_AUTO_TEST_CASE( test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) { #ifdef NDEBUG broad_phase_update_collision_test(2000, 100, 1000, 1, true, true); broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true); #else broad_phase_update_collision_test(2000, 2, 4, 1, true, true); broad_phase_update_collision_test(2000, 4, 4, 1, true, true); #endif } //============================================================================== struct CollisionDataForUniquenessChecking { std::set> checkedPairs; bool checkUniquenessAndAddPair(CollisionObject* o1, CollisionObject* o2) { auto search = checkedPairs.find(std::make_pair(o1, o2)); if (search != checkedPairs.end()) return false; checkedPairs.emplace(o1, o2); return true; } }; //============================================================================== struct CollisionFunctionForUniquenessChecking : CollisionCallBackBase { bool collide(CollisionObject* o1, CollisionObject* o2) { BOOST_CHECK(data.checkUniquenessAndAddPair(o1, o2)); return false; } CollisionDataForUniquenessChecking data; }; //============================================================================== void broad_phase_duplicate_check_test(Scalar env_scale, std::size_t env_size, bool verbose) { std::vector ts; std::vector timers; std::vector env; generateEnvironments(env, env_scale, env_size); std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); managers.push_back( new SpatialHashingCollisionManager< detail::SparseHashTable>( cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back( new SpatialHashingCollisionManager>( cell_size, lower_limit, upper_limit)); managers.push_back( new SpatialHashingCollisionManager>( cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeArrayCollisionManager()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeArrayCollisionManager* m = new DynamicAABBTreeArrayCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } // update the environment Scalar delta_angle_max = Scalar(10) / Scalar(360 * 2) * boost::math::constants::pi(); Scalar delta_trans_max = Scalar(0.01) * env_scale; const Scalar half = Scalar(0.5); for (size_t i = 0; i < env.size(); ++i) { Scalar rand_angle_x = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max; Scalar rand_trans_x = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max; Scalar rand_angle_y = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max; Scalar rand_trans_y = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max; Scalar rand_angle_z = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max; Scalar rand_trans_z = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max; using AngleAxis = Eigen::AngleAxis; Matrix3s dR(AngleAxis(rand_angle_x, Vec3s::UnitX()) * AngleAxis(rand_angle_y, Vec3s::UnitY()) * AngleAxis(rand_angle_z, Vec3s::UnitZ())); Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z); Matrix3s R = env[i]->getRotation(); Vec3s T = env[i]->getTranslation(); env[i]->setTransform(dR * R, dR * T + dT); env[i]->computeAABB(); } for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->update(); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } std::vector self_data(managers.size()); for (size_t i = 0; i < managers.size(); ++i) { CollisionFunctionForUniquenessChecking callback; timers[i].start(); managers[i]->collide(&callback); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (auto obj : env) delete obj; if (!verbose) return; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); int w = 7; std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs" << std::endl; std::cout << "register time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "update time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[3] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { double tmp = 0; for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } void broad_phase_update_collision_test(Scalar env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; if (use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; if (use_mesh) generateEnvironmentsMesh(query, env_scale, query_size); else generateEnvironments(query, env_scale, query_size); std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); // managers.push_back(new SpatialHashingCollisionManager(cell_size, // lower_limit, upper_limit)); managers.push_back( new SpatialHashingCollisionManager< detail::SparseHashTable>( cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back( new SpatialHashingCollisionManager>( cell_size, lower_limit, upper_limit)); managers.push_back( new SpatialHashingCollisionManager>( cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeArrayCollisionManager()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeArrayCollisionManager* m = new DynamicAABBTreeArrayCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } // update the environment Scalar delta_angle_max = Scalar(10) / Scalar(360 * 2) * boost::math::constants::pi(); Scalar delta_trans_max = Scalar(0.01) * env_scale; const Scalar half = Scalar(0.5); for (size_t i = 0; i < env.size(); ++i) { Scalar rand_angle_x = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max; Scalar rand_trans_x = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max; Scalar rand_angle_y = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max; Scalar rand_trans_y = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max; Scalar rand_angle_z = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_angle_max; Scalar rand_trans_z = 2 * (Scalar(rand()) / Scalar(RAND_MAX) - half) * delta_trans_max; using AngleAxis = Eigen::AngleAxis; Matrix3s dR(AngleAxis(rand_angle_x, Vec3s::UnitX()) * AngleAxis(rand_angle_y, Vec3s::UnitY()) * AngleAxis(rand_angle_z, Vec3s::UnitZ())); Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z); Matrix3s R = env[i]->getRotation(); Vec3s T = env[i]->getTranslation(); env[i]->setTransform(dR * R, dR * T + dT); env[i]->computeAABB(); } for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->update(); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } std::vector self_data(managers.size()); for (size_t i = 0; i < managers.size(); ++i) { if (exhaustive) self_data[i].request.num_max_contacts = 100000; else self_data[i].request.num_max_contacts = num_max_contacts; } for (size_t i = 0; i < managers.size(); ++i) { CollisionCallBackDefault callback; timers[i].start(); managers[i]->collide(&callback); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (size_t i = 0; i < managers.size(); ++i) std::cout << self_data[i].result.numContacts() << " "; std::cout << std::endl; if (exhaustive) { for (size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); } else { std::vector self_res(managers.size()); for (size_t i = 0; i < self_res.size(); ++i) self_res[i] = (self_data[i].result.numContacts() > 0); for (size_t i = 1; i < self_res.size(); ++i) BOOST_CHECK(self_res[0] == self_res[i]); for (size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); } for (size_t i = 0; i < query.size(); ++i) { std::vector query_callbacks(managers.size()); for (size_t j = 0; j < query_callbacks.size(); ++j) { if (exhaustive) query_callbacks[j].data.request.num_max_contacts = 100000; else query_callbacks[j].data.request.num_max_contacts = num_max_contacts; } for (size_t j = 0; j < query_callbacks.size(); ++j) { timers[j].start(); managers[j]->collide(query[i], &query_callbacks[j]); const auto elapsed_s = timers[j].getElapsedTime(); ts[j].push_back(elapsed_s); } // for(size_t j = 0; j < managers.size(); ++j) // std::cout << query_callbacks[j].result.numContacts() << " "; // std::cout << std::endl; if (exhaustive) { for (size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(query_callbacks[j].data.result.numContacts() == query_callbacks[0].data.result.numContacts()); } else { std::vector query_res(managers.size()); for (size_t j = 0; j < query_res.size(); ++j) query_res[j] = (query_callbacks[j].data.result.numContacts() > 0); for (size_t j = 1; j < query_res.size(); ++j) BOOST_CHECK(query_res[0] == query_res[j]); for (size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(query_callbacks[j].data.result.numContacts() == query_callbacks[0].data.result.numContacts()); } } for (size_t i = 0; i < env.size(); ++i) delete env[i]; for (size_t i = 0; i < query.size(); ++i) delete query[i]; for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); int w = 7; std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "update time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[3] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { double tmp = 0; for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } ================================================ FILE: test/broadphase_collision_2.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Jia Pan */ #define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_2 #include #include "coal/broadphase/broadphase_bruteforce.h" #include "coal/broadphase/broadphase_spatialhash.h" #include "coal/broadphase/broadphase_SaP.h" #include "coal/broadphase/broadphase_SSaP.h" #include "coal/broadphase/broadphase_interval_tree.h" #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #include "coal/broadphase/default_broadphase_callbacks.h" #include "coal/broadphase/detail/sparse_hash_table.h" #include "coal/broadphase/detail/spatial_hash.h" #include "utility.h" #if USE_GOOGLEHASH #include #include #include #endif #include #include using namespace coal; /// @brief test for broad phase collision and self collision void broad_phase_collision_test(Scalar env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); #if USE_GOOGLEHASH template struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; template struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > { GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() { this->set_empty_key(nullptr); } }; #endif /// check broad phase collision for empty collision object set and queries BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) { #ifdef NDEBUG broad_phase_collision_test(2000, 0, 0, 10, false, false); broad_phase_collision_test(2000, 0, 1000, 10, false, false); broad_phase_collision_test(2000, 100, 0, 10, false, false); broad_phase_collision_test(2000, 0, 0, 10, false, true); broad_phase_collision_test(2000, 0, 1000, 10, false, true); broad_phase_collision_test(2000, 100, 0, 10, false, true); broad_phase_collision_test(2000, 0, 0, 10, true, false); broad_phase_collision_test(2000, 0, 1000, 10, true, false); broad_phase_collision_test(2000, 100, 0, 10, true, false); broad_phase_collision_test(2000, 0, 0, 10, true, true); broad_phase_collision_test(2000, 0, 1000, 10, true, true); broad_phase_collision_test(2000, 100, 0, 10, true, true); #else broad_phase_collision_test(2000, 0, 0, 10, false, false); broad_phase_collision_test(2000, 0, 5, 10, false, false); broad_phase_collision_test(2000, 2, 0, 10, false, false); broad_phase_collision_test(2000, 0, 0, 10, false, true); broad_phase_collision_test(2000, 0, 5, 10, false, true); broad_phase_collision_test(2000, 2, 0, 10, false, true); broad_phase_collision_test(2000, 0, 0, 10, true, false); broad_phase_collision_test(2000, 0, 5, 10, true, false); broad_phase_collision_test(2000, 2, 0, 10, true, false); broad_phase_collision_test(2000, 0, 0, 10, true, true); broad_phase_collision_test(2000, 0, 5, 10, true, true); broad_phase_collision_test(2000, 2, 0, 10, true, true); #endif } /// check broad phase collision and self collision, only return collision or not BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 1, false); broad_phase_collision_test(2000, 1000, 1000, 1, false); broad_phase_collision_test(2000, 100, 1000, 1, true); broad_phase_collision_test(2000, 1000, 1000, 1, true); #else broad_phase_collision_test(2000, 10, 100, 1, false); broad_phase_collision_test(2000, 100, 100, 1, false); broad_phase_collision_test(2000, 10, 100, 1, true); broad_phase_collision_test(2000, 100, 100, 1, true); #endif } /// check broad phase collision and self collision, return 10 contacts BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 10, false); broad_phase_collision_test(2000, 1000, 1000, 10, false); #else broad_phase_collision_test(2000, 10, 100, 10, false); broad_phase_collision_test(2000, 100, 100, 10, false); #endif } /// check broad phase collision and self collision, return only collision or /// not, in mesh BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 1, false, true); broad_phase_collision_test(2000, 1000, 1000, 1, false, true); #else broad_phase_collision_test(2000, 2, 5, 1, false, true); broad_phase_collision_test(2000, 5, 5, 1, false, true); #endif } /// check broad phase collision and self collision, return 10 contacts, in mesh BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 10, false, true); broad_phase_collision_test(2000, 1000, 1000, 10, false, true); #else broad_phase_collision_test(2000, 2, 5, 10, false, true); broad_phase_collision_test(2000, 5, 5, 10, false, true); #endif } /// check broad phase collision and self collision, exhaustive, in mesh BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 1, true, true); broad_phase_collision_test(2000, 1000, 1000, 1, true, true); #else broad_phase_collision_test(2000, 2, 5, 1, true, true); broad_phase_collision_test(2000, 5, 5, 1, true, true); #endif } void broad_phase_collision_test(Scalar env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; if (use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; if (use_mesh) generateEnvironmentsMesh(query, env_scale, query_size); else generateEnvironments(query, env_scale, query_size); std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); // Scalar ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0); Scalar ncell_per_axis = 20; Scalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis); // managers.push_back(new SpatialHashingCollisionManager(cell_size, // lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >( cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back( new SpatialHashingCollisionManager >( cell_size, lower_limit, upper_limit)); managers.push_back( new SpatialHashingCollisionManager >( cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeArrayCollisionManager()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeArrayCollisionManager* m = new DynamicAABBTreeArrayCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } std::vector callbacks(managers.size()); for (size_t i = 0; i < managers.size(); ++i) { if (exhaustive) callbacks[i].data.request.num_max_contacts = 100000; else callbacks[i].data.request.num_max_contacts = num_max_contacts; } for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->collide(&callbacks[i]); const auto elapsed_s = timers[i].getElapsedTime(); ts[i].push_back(elapsed_s); } for (size_t i = 0; i < managers.size(); ++i) std::cout << callbacks[i].data.result.numContacts() << " "; std::cout << std::endl; if (exhaustive) { for (size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(callbacks[i].data.result.numContacts() == callbacks[0].data.result.numContacts()); } else { std::vector self_res(managers.size()); for (size_t i = 0; i < self_res.size(); ++i) self_res[i] = (callbacks[i].data.result.numContacts() > 0); for (size_t i = 1; i < self_res.size(); ++i) BOOST_CHECK(self_res[0] == self_res[i]); for (size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(callbacks[i].data.result.numContacts() == callbacks[0].data.result.numContacts()); } for (size_t i = 0; i < query.size(); ++i) { std::vector callbacks(managers.size()); for (size_t j = 0; j < managers.size(); ++j) { if (exhaustive) callbacks[j].data.request.num_max_contacts = 100000; else callbacks[j].data.request.num_max_contacts = num_max_contacts; } for (size_t j = 0; j < managers.size(); ++j) { timers[j].start(); managers[j]->collide(query[i], &callbacks[j]); const auto elapsed_s = timers[j].getElapsedTime(); ts[j].push_back(elapsed_s); } // for(size_t j = 0; j < managers.size(); ++j) // std::cout << callbacks[i].data.result.numContacts() << " "; // std::cout << std::endl; if (exhaustive) { for (size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(callbacks[j].data.result.numContacts() == callbacks[0].data.result.numContacts()); } else { std::vector query_res(managers.size()); for (size_t j = 0; j < query_res.size(); ++j) query_res[j] = (callbacks[j].data.result.numContacts() > 0); for (size_t j = 1; j < query_res.size(); ++j) BOOST_CHECK(query_res[0] == query_res[j]); for (size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(callbacks[j].data.result.numContacts() == callbacks[0].data.result.numContacts()); } } for (size_t i = 0; i < env.size(); ++i) delete env[i]; for (size_t i = 0; i < query.size(); ++i) delete query[i]; for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); int w = 7; std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { double tmp = 0; for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } ================================================ FILE: test/broadphase_dynamic_AABB_tree.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2020. Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of CNRS-LAAS and AIST nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Damrong Guoy (Damrong.Guoy@tri.global) */ /** Tests the dynamic axis-aligned bounding box tree.*/ #define BOOST_TEST_MODULE COAL_BROADPHASE_DYNAMIC_AABB_TREE #include // #include "coal/data_types.h" #include "coal/shape/geometric_shapes.h" #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include #include using namespace coal; // Pack the data for callback function. struct CallBackData { bool expect_object0_then_object1; std::vector* objects; }; // This callback function tests the order of the two collision objects from // the dynamic tree against the `data`. We assume that the first two // parameters are always objects[0] and objects[1] in two possible orders, // so we can safely ignore the second parameter. We do not use the last // Scalar& parameter, which specifies the distance beyond which the // pair of objects will be skipped. struct DistanceCallBackDerived : DistanceCallBackBase { bool distance(CollisionObject* o1, CollisionObject* o2, Scalar& dist) { return distance_callback(o1, o2, &data, dist); } bool distance_callback(CollisionObject* a, CollisionObject*, void* callback_data, Scalar&) { // Unpack the data. CallBackData* data = static_cast(callback_data); const std::vector& objects = *(data->objects); const bool object0_first = a == objects[0]; BOOST_CHECK_EQUAL(data->expect_object0_then_object1, object0_first); // TODO(DamrongGuoy): Remove the statement below when we solve the // repeatability problem as mentioned in: // https://github.com/flexible-collision-library/fcl/issues/368 // Expect to switch the order next time. data->expect_object0_then_object1 = !data->expect_object0_then_object1; // Return true to stop the tree traversal. return true; } CallBackData data; }; // Tests repeatability of a dynamic tree of two spheres when we call update() // and distance() again and again without changing the poses of the objects. // We only use the distance() method to invoke a hierarchy traversal. // The distance-callback function in this test does not compute the signed // distance between the two objects; it only checks their order. // // Currently every call to update() switches the order of the two objects. // TODO(DamrongGuoy): Remove the above comment when we solve the // repeatability problem as mentioned in: // https://github.com/flexible-collision-library/fcl/issues/368 // BOOST_AUTO_TEST_CASE(DynamicAABBTreeCollisionManager_class) { CollisionGeometryPtr_t sphere0 = make_shared(0.1); CollisionGeometryPtr_t sphere1 = make_shared(0.2); CollisionObject object0(sphere0); CollisionObject object1(sphere1); const Vec3s position0(Scalar(0.1), Scalar(0.2), Scalar(0.3)); const Vec3s position1(Scalar(0.11), Scalar(0.21), Scalar(0.31)); // We will use `objects` to check the order of the two collision objects in // our callback function. // // We use std::vector that contains *pointers* to CollisionObject, // instead of std::vector that contains CollisionObject's. // Previously we used std::vector, and it failed the // Eigen alignment assertion on Win32. We also tried, without success, the // custom allocator: // std::vector>, // but some platforms failed to build. std::vector objects; objects.push_back(&object0); objects.push_back(&object1); std::vector positions; positions.push_back(position0); positions.push_back(position1); DynamicAABBTreeCollisionManager dynamic_tree; for (size_t i = 0; i < objects.size(); ++i) { objects[i]->setTranslation(positions[i]); objects[i]->computeAABB(); dynamic_tree.registerObject(objects[i]); } DistanceCallBackDerived callback; callback.data.expect_object0_then_object1 = false; callback.data.objects = &objects; // We repeat update() and distance() many times. Each time, in the // callback function, we check the order of the two objects. for (int count = 0; count < 8; ++count) { dynamic_tree.update(); dynamic_tree.distance(&callback); } } ================================================ FILE: test/bvh_models.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2015, Open Source Robotics Foundation * Copyright (c) 2020, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jeongseok Lee */ #define BOOST_TEST_MODULE COAL_BVH_MODELS #include #include #include "fcl_resources/config.h" #include "coal/collision.h" #include "coal/BVH/BVH_model.h" #include "coal/BVH/BVH_utility.h" #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/shape/geometric_shape_to_BVH_model.h" #include "coal/mesh_loader/assimp.h" #include "coal/mesh_loader/loader.h" #include "utility.h" #include using namespace coal; template void testBVHModelPointCloud() { Box box(Vec3s::Ones()); Scalar a = box.halfSide[0]; Scalar b = box.halfSide[1]; Scalar c = box.halfSide[2]; std::vector points(8); points[0] << a, -b, c; points[1] << a, b, c; points[2] << -a, b, c; points[3] << -a, -b, c; points[4] << a, -b, -c; points[5] << a, b, -c; points[6] << -a, b, -c; points[7] << -a, -b, -c; { shared_ptr > model(new BVHModel); if (model->getNodeType() != BV_AABB && model->getNodeType() != BV_KDOP16 && model->getNodeType() != BV_KDOP18 && model->getNodeType() != BV_KDOP24) { std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType()) << "' does not support point cloud model. " << "Please see issue #67." << std::endl; return; } int result; result = model->beginModel(); BOOST_CHECK_EQUAL(result, BVH_OK); for (std::size_t i = 0; i < points.size(); ++i) { result = model->addVertex(points[i]); BOOST_CHECK_EQUAL(result, BVH_OK); } result = model->endModel(); BOOST_CHECK_EQUAL(result, BVH_OK); model->computeLocalAABB(); BOOST_CHECK_EQUAL(model->num_vertices, 8); BOOST_CHECK_EQUAL(model->num_tris, 0); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); } { shared_ptr > model(new BVHModel); if (model->getNodeType() != BV_AABB && model->getNodeType() != BV_KDOP16 && model->getNodeType() != BV_KDOP18 && model->getNodeType() != BV_KDOP24) { std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType()) << "' does not support point cloud model. " << "Please see issue #67." << std::endl; return; } MatrixX3s all_points((Eigen::DenseIndex)points.size(), 3); for (size_t k = 0; k < points.size(); ++k) all_points.row((Eigen::DenseIndex)k) = points[k].transpose(); int result; result = model->beginModel(); BOOST_CHECK_EQUAL(result, BVH_OK); result = model->addVertices(all_points); result = model->endModel(); BOOST_CHECK_EQUAL(result, BVH_OK); model->computeLocalAABB(); BOOST_CHECK_EQUAL(model->num_vertices, 8); BOOST_CHECK_EQUAL(model->num_tris, 0); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); } } template void testBVHModelTriangles() { shared_ptr > model(new BVHModel); Box box(Vec3s::Ones()); AABB aabb(Vec3s(-1, 0, -1), Vec3s(1, 1, 1)); Scalar a = box.halfSide[0]; Scalar b = box.halfSide[1]; Scalar c = box.halfSide[2]; std::vector points(8); std::vector tri_indices(12); points[0] << a, -b, c; points[1] << a, b, c; points[2] << -a, b, c; points[3] << -a, -b, c; points[4] << a, -b, -c; points[5] << a, b, -c; points[6] << -a, b, -c; points[7] << -a, -b, -c; tri_indices[0].set(0, 4, 1); tri_indices[1].set(1, 4, 5); tri_indices[2].set(2, 6, 3); tri_indices[3].set(3, 6, 7); tri_indices[4].set(3, 0, 2); tri_indices[5].set(2, 0, 1); tri_indices[6].set(6, 5, 7); tri_indices[7].set(7, 5, 4); tri_indices[8].set(1, 5, 2); tri_indices[9].set(2, 5, 6); tri_indices[10].set(3, 7, 0); tri_indices[11].set(0, 7, 4); int result; result = model->beginModel(); BOOST_CHECK_EQUAL(result, BVH_OK); for (std::size_t i = 0; i < tri_indices.size(); ++i) { result = model->addTriangle(points[tri_indices[i][0]], points[tri_indices[i][1]], points[tri_indices[i][2]]); BOOST_CHECK_EQUAL(result, BVH_OK); } result = model->endModel(); BOOST_CHECK_EQUAL(result, BVH_OK); model->computeLocalAABB(); BOOST_CHECK_EQUAL(model->num_vertices, 12 * 3); BOOST_CHECK_EQUAL(model->num_tris, 12); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); Transform3s pose; shared_ptr > cropped(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); pose.setTranslation(Vec3s(0, 1, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); pose.setTranslation(Vec3s(0, 0, 0)); Scalar sqrt2_2 = std::sqrt(Scalar(2)) / 2; pose.setQuatRotation(Quats(sqrt2_2, sqrt2_2, 0, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); pose.setTranslation(-Vec3s(1, 1, 1)); pose.setQuatRotation(Quats::Identity()); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_CHECK(!cropped); aabb = AABB(Vec3s(Scalar(-0.1), Scalar(-0.1), Scalar(-0.1)), Vec3s(Scalar(0.1), Scalar(0.1), Scalar(0.1))); pose.setTranslation(Vec3s(-0.5, -0.5, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK_EQUAL(cropped->num_tris, 2); BOOST_CHECK_EQUAL(cropped->num_vertices, 6); } template void testBVHModelSubModel() { shared_ptr > model(new BVHModel); Box box(Vec3s::Ones()); Scalar a = box.halfSide[0]; Scalar b = box.halfSide[1]; Scalar c = box.halfSide[2]; std::vector points(8); std::vector tri_indices(12); points[0] << a, -b, c; points[1] << a, b, c; points[2] << -a, b, c; points[3] << -a, -b, c; points[4] << a, -b, -c; points[5] << a, b, -c; points[6] << -a, b, -c; points[7] << -a, -b, -c; tri_indices[0].set(0, 4, 1); tri_indices[1].set(1, 4, 5); tri_indices[2].set(2, 6, 3); tri_indices[3].set(3, 6, 7); tri_indices[4].set(3, 0, 2); tri_indices[5].set(2, 0, 1); tri_indices[6].set(6, 5, 7); tri_indices[7].set(7, 5, 4); tri_indices[8].set(1, 5, 2); tri_indices[9].set(2, 5, 6); tri_indices[10].set(3, 7, 0); tri_indices[11].set(0, 7, 4); int result; result = model->beginModel(); BOOST_CHECK_EQUAL(result, BVH_OK); result = model->addSubModel(points, tri_indices); BOOST_CHECK_EQUAL(result, BVH_OK); result = model->endModel(); BOOST_CHECK_EQUAL(result, BVH_OK); model->computeLocalAABB(); BOOST_CHECK_EQUAL(model->num_vertices, 8); BOOST_CHECK_EQUAL(model->num_tris, 12); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); } template void testBVHModel() { testBVHModelTriangles(); testBVHModelPointCloud(); testBVHModelSubModel(); } BOOST_AUTO_TEST_CASE(building_bvh_models) { testBVHModel(); testBVHModel(); testBVHModel(); testBVHModel(); testBVHModel(); testBVHModel >(); testBVHModel >(); testBVHModel >(); } template void testLoadPolyhedron() { boost::filesystem::path path(TEST_RESOURCES_DIR); std::string env = (path / "env.obj").string(), rob = (path / "rob.obj").string(); typedef BVHModel Polyhedron_t; typedef shared_ptr PolyhedronPtr_t; PolyhedronPtr_t P1(new Polyhedron_t), P2; Vec3s scale; scale.setConstant(1); loadPolyhedronFromResource(env, scale, P1); scale.setConstant(-1); CachedMeshLoader loader(P1->getNodeType()); CollisionGeometryPtr_t geom = loader.load(env, scale); P2 = dynamic_pointer_cast(geom); BOOST_REQUIRE(P2); BOOST_CHECK_EQUAL(P1->num_tris, P2->num_tris); BOOST_CHECK_EQUAL(P1->num_vertices, P2->num_vertices); BOOST_CHECK_EQUAL(P1->getNumBVs(), P2->getNumBVs()); CollisionGeometryPtr_t geom2 = loader.load(env, scale); BOOST_CHECK_EQUAL(geom, geom2); } template void testLoadGerardBauzil() { boost::filesystem::path path(TEST_RESOURCES_DIR); std::string env = (path / "staircases_koroibot_hr.dae").string(); typedef BVHModel Polyhedron_t; typedef shared_ptr PolyhedronPtr_t; PolyhedronPtr_t P1(new Polyhedron_t), P2; Vec3s scale; scale.setConstant(1); loadPolyhedronFromResource(env, scale, P1); CollisionGeometryPtr_t cylinder(new Cylinder(Scalar(.27), Scalar(.27))); Transform3s pos(Vec3s(Scalar(-1.33), Scalar(1.36), Scalar(.14))); CollisionObject obj(cylinder, pos); CollisionObject stairs(P1); CollisionRequest request; CollisionResult result; collide(&stairs, &obj, request, result); BOOST_CHECK(result.isCollision()); } BOOST_AUTO_TEST_CASE(load_polyhedron) { testLoadPolyhedron(); testLoadPolyhedron(); testLoadPolyhedron(); testLoadPolyhedron(); testLoadPolyhedron(); testLoadPolyhedron >(); testLoadPolyhedron >(); testLoadPolyhedron >(); } BOOST_AUTO_TEST_CASE(gerard_bauzil) { testLoadGerardBauzil(); testLoadGerardBauzil(); testLoadGerardBauzil(); testLoadGerardBauzil(); } BOOST_AUTO_TEST_CASE(load_illformated_mesh) { boost::filesystem::path path(TEST_RESOURCES_DIR); const std::string filename = (path / "illformated_mesh.dae").string(); MeshLoader loader; BOOST_CHECK_NO_THROW(loader.load(filename)); } BOOST_AUTO_TEST_CASE(test_convex) { Box* box_ptr = new coal::Box(1, 1, 1); CollisionGeometryPtr_t b1(box_ptr); BVHModel box_bvh_model = BVHModel(); generateBVHModel(box_bvh_model, *box_ptr, Transform3s()); box_bvh_model.buildConvexRepresentation(false); box_bvh_model.convex->computeLocalAABB(); std::shared_ptr convex_copy(box_bvh_model.convex->clone()); BOOST_CHECK(*convex_copy.get() == *box_bvh_model.convex.get()); } ================================================ FILE: test/capsule_box_1.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2014-2016, CNRS-LAAS and AIST * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of CNRS-LAAS and AIST nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include #include "coal/distance.h" #include "coal/math/transform.h" #include "coal/collision.h" #include "coal/collision_object.h" #include "coal/shape/geometric_shapes.h" #include "utility.h" using coal::Scalar; BOOST_AUTO_TEST_CASE(distance_capsule_box) { using coal::CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 CollisionGeometryPtr_t capsuleGeometry(new coal::Capsule(2., 4.)); // Box of size 1 by 2 by 4 CollisionGeometryPtr_t boxGeometry(new coal::Box(1., 2., 4.)); // Enable computation of nearest points coal::DistanceRequest distanceRequest(true, 0, 0); coal::DistanceResult distanceResult; coal::Transform3s tf1(coal::Vec3s(3., 0, 0)); coal::Transform3s tf2; coal::CollisionObject capsule(capsuleGeometry, tf1); coal::CollisionObject box(boxGeometry, tf2); // test distance coal::distance(&capsule, &box, distanceRequest, distanceResult); // Nearest point on capsule coal::Vec3s o1(distanceResult.nearest_points[0]); // Nearest point on box coal::Vec3s o2(distanceResult.nearest_points[1]); BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.5, 1e-1); BOOST_CHECK_CLOSE(o1[0], 1.0, 1e-1); CHECK_CLOSE_TO_0(o1[1], 1e-1); BOOST_CHECK_CLOSE(o2[0], 0.5, 1e-1); CHECK_CLOSE_TO_0(o2[1], 1e-1); // Move capsule above box tf1 = coal::Transform3s(coal::Vec3s(0., 0., 8.)); capsule.setTransform(tf1); // test distance distanceResult.clear(); coal::distance(&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points[0]; o2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 2.0, 1e-1); CHECK_CLOSE_TO_0(o1[0], 1e-1); CHECK_CLOSE_TO_0(o1[1], 1e-1); BOOST_CHECK_CLOSE(o1[2], 4.0, 1e-1); CHECK_CLOSE_TO_0(o2[0], 1e-1); CHECK_CLOSE_TO_0(o2[1], 1e-1); BOOST_CHECK_CLOSE(o2[2], 2.0, 1e-1); // Rotate capsule around y axis by pi/2 and move it behind box tf1.setTranslation(coal::Vec3s(-10., 0., 0.)); tf1.setQuatRotation( coal::makeQuat(sqrt(Scalar(2)) / 2, 0, sqrt(Scalar(2)) / 2, 0)); capsule.setTransform(tf1); // test distance distanceResult.clear(); coal::distance(&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points[0]; o2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-1); BOOST_CHECK_CLOSE(o1[0], -6, 1e-2); CHECK_CLOSE_TO_0(o1[1], 1e-1); CHECK_CLOSE_TO_0(o1[2], 1e-1); BOOST_CHECK_CLOSE(o2[0], -0.5, 1e-2); CHECK_CLOSE_TO_0(o2[1], 1e-1); CHECK_CLOSE_TO_0(o2[2], 1e-1); } ================================================ FILE: test/capsule_box_2.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2014-2015, CNRS-LAAS and AIST * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of CNRS-LAAS and AIST nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include "utility.h" #include #include "coal/distance.h" #include "coal/math/transform.h" #include "coal/collision.h" #include "coal/collision_object.h" #include "coal/shape/geometric_shapes.h" using coal::Scalar; BOOST_AUTO_TEST_CASE(distance_capsule_box) { typedef coal::shared_ptr CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 CollisionGeometryPtr_t capsuleGeometry(new coal::Capsule(2., 4.)); // Box of size 1 by 2 by 4 CollisionGeometryPtr_t boxGeometry(new coal::Box(1., 2., 4.)); // Enable computation of nearest points coal::DistanceRequest distanceRequest(true, 0, 0); coal::DistanceResult distanceResult; // Rotate capsule around y axis by pi/2 and move it behind box coal::Transform3s tf1( coal::makeQuat(sqrt(Scalar(2)) / 2, 0, sqrt(Scalar(2)) / 2, 0), coal::Vec3s(Scalar(-10.), Scalar(0.8), Scalar(1.5))); coal::Transform3s tf2; coal::CollisionObject capsule(capsuleGeometry, tf1); coal::CollisionObject box(boxGeometry, tf2); // test distance distanceResult.clear(); coal::distance(&capsule, &box, distanceRequest, distanceResult); coal::Vec3s o1 = distanceResult.nearest_points[0]; coal::Vec3s o2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-2); BOOST_CHECK_CLOSE(o1[0], -6, 1e-2); BOOST_CHECK_CLOSE(o1[1], 0.8, 1e-1); BOOST_CHECK_CLOSE(o1[2], 1.5, 1e-2); BOOST_CHECK_CLOSE(o2[0], -0.5, 1e-2); BOOST_CHECK_CLOSE(o2[1], 0.8, 1e-1); BOOST_CHECK_CLOSE(o2[2], 1.5, 1e-2); } ================================================ FILE: test/capsule_capsule.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Karsten Knese */ #define BOOST_TEST_MODULE COAL_CAPSULE_CAPSULE #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include #include #include "coal/distance.h" #include "coal/collision.h" #include "coal/math/transform.h" #include "coal/collision.h" #include "coal/collision_object.h" #include "coal/shape/geometric_shapes.h" #include "utility.h" using namespace coal; using Quat = Eigen::Quaternion; using Vec4s = Eigen::Matrix; BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) { const Scalar radius = 1.; CollisionGeometryPtr_t c1(new Capsule(radius, 0.)); CollisionGeometryPtr_t c2(new Capsule(radius, 0.)); CollisionGeometryPtr_t s1(new Sphere(radius)); CollisionGeometryPtr_t s2(new Sphere(radius)); #ifndef NDEBUG int num_tests = 1e3; #else int num_tests = 1e6; #endif Transform3s tf1; Transform3s tf2; for (int i = 0; i < num_tests; ++i) { Vec3s p1 = Vec3s::Random() * (2. * radius); Vec3s p2 = Vec3s::Random() * (2. * radius); Matrix3s rot1 = Quat(Vec4s::Random().normalized()).toRotationMatrix(); Matrix3s rot2 = Quat(Vec4s::Random().normalized()).toRotationMatrix(); tf1.setTranslation(p1); tf1.setRotation(rot1); tf2.setTranslation(p2); tf2.setRotation(rot2); CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); CollisionObject sphere_o1(s1, tf1); CollisionObject sphere_o2(s2, tf2); // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult, sphere_collisionResult; size_t sphere_num_collisions = collide( &sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult); size_t capsule_num_collisions = collide( &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); BOOST_CHECK_EQUAL(sphere_num_collisions, capsule_num_collisions); if (sphere_num_collisions == 0 && capsule_num_collisions == 0) BOOST_CHECK_CLOSE(sphere_collisionResult.distance_lower_bound, capsule_collisionResult.distance_lower_bound, 1e-6); } } BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { const Scalar radius = Scalar(0.01); const Scalar length = Scalar(0.2); CollisionGeometryPtr_t c1(new Capsule(radius, length)); CollisionGeometryPtr_t c2(new Capsule(radius, length)); #ifndef NDEBUG int num_tests = 1e3; #else int num_tests = 1e6; #endif Transform3s tf1; Transform3s tf2; Vec3s p1 = Vec3s::Zero(); Vec3s p2_no_collision = Vec3s(0, 0, Scalar(2 * (length / 2. + radius) + 1e-3)); // because capsule are along the Z axis for (int i = 0; i < num_tests; ++i) { Matrix3s rot = Quat(Vec4s::Random().normalized()).toRotationMatrix(); tf1.setTranslation(p1); tf1.setRotation(rot); tf2.setTranslation(p2_no_collision); tf2.setRotation(rot); CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult; size_t capsule_num_collisions = collide( &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); BOOST_CHECK(capsule_num_collisions == 0); } Vec3s p2_with_collision = Vec3s(0, 0, Scalar(std::min(length / 2, radius) * (1. - 1e-2))); for (int i = 0; i < num_tests; ++i) { Matrix3s rot = Quat(Vec4s::Random().normalized()).toRotationMatrix(); tf1.setTranslation(p1); tf1.setRotation(rot); tf2.setTranslation(p2_with_collision); tf2.setRotation(rot); CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult; size_t capsule_num_collisions = collide( &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); BOOST_CHECK(capsule_num_collisions > 0); } p2_no_collision = Vec3s(0, 0, Scalar(2 * (length / 2. + radius) + 1e-3)); Transform3s geom1_placement(Matrix3s::Identity(), Vec3s::Zero()); Transform3s geom2_placement(Matrix3s::Identity(), p2_no_collision); for (int i = 0; i < num_tests; ++i) { Matrix3s rot = Quat(Vec4s::Random().normalized()).toRotationMatrix(); Vec3s trans = Vec3s::Random(); Transform3s displacement(rot, trans); Transform3s tf1 = displacement * geom1_placement; Transform3s tf2 = displacement * geom2_placement; CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult; size_t capsule_num_collisions = collide( &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); BOOST_CHECK(capsule_num_collisions == 0); } // p2_with_collision = // Vec3s(0.,0.,std::min(length/2.,radius)*(1.-1e-2)); p2_with_collision = Vec3s(0, 0, Scalar(0.01)); geom2_placement.setTranslation(p2_with_collision); for (int i = 0; i < num_tests; ++i) { Matrix3s rot = Quat(Vec4s::Random().normalized()).toRotationMatrix(); Vec3s trans = Vec3s::Random(); Transform3s displacement(rot, trans); Transform3s tf1 = displacement * geom1_placement; Transform3s tf2 = displacement * geom2_placement; CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult; size_t capsule_num_collisions = collide( &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); BOOST_CHECK(capsule_num_collisions > 0); } } BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3s tf1; Transform3s tf2(Vec3s(Scalar(20.1), 0, 0)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); // Enable computation of nearest points DistanceRequest distanceRequest(true); DistanceResult distanceResult; distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied translation on two capsules"; std::cerr << " T1 = " << tf1.getTranslation() << ", T2 = " << tf2.getTranslation() << std::endl; std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6); } BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3s tf1; Transform3s tf2(Vec3s(20, 20, 0)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); // Enable computation of nearest points DistanceRequest distanceRequest(true); DistanceResult distanceResult; distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied translation on two capsules"; std::cerr << " T1 = " << tf1.getTranslation() << ", T2 = " << tf2.getTranslation() << std::endl; std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; Scalar expected = sqrt(Scalar(800)) - 10; BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6); } BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3s tf1; Transform3s tf2(Vec3s(0, 0, Scalar(20.1))); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); // Enable computation of nearest points DistanceRequest distanceRequest(true); DistanceResult distanceResult; distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied translation on two capsules"; std::cerr << " T1 = " << tf1.getTranslation() << ", T2 = " << tf2.getTranslation() << std::endl; std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.1, 1e-6); } BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3s tf1; Transform3s tf2(makeQuat(sqrt(Scalar(2)) / 2, 0, sqrt(Scalar(2)) / 2, 0), Vec3s(0, 0, Scalar(25.1))); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); // Enable computation of nearest points DistanceRequest distanceRequest(true); DistanceResult distanceResult; distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied rotation and translation on two capsules" << std::endl; std::cerr << "R1 = " << tf1.getRotation() << std::endl << "T1 = " << tf1.getTranslation().transpose() << std::endl << "R2 = " << tf2.getRotation() << std::endl << "T2 = " << tf2.getTranslation().transpose() << std::endl; std::cerr << "Closest points:" << std::endl << "p1 = " << distanceResult.nearest_points[0].transpose() << std::endl << "p2 = " << distanceResult.nearest_points[1].transpose() << std::endl << "distance = " << distanceResult.min_distance << std::endl; const Vec3s& p1 = distanceResult.nearest_points[0]; const Vec3s& p2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6); CHECK_CLOSE_TO_0(p1[0], 1e-4); CHECK_CLOSE_TO_0(p1[1], 1e-4); BOOST_CHECK_CLOSE(p1[2], 10, 1e-4); CHECK_CLOSE_TO_0(p2[0], 1e-4); CHECK_CLOSE_TO_0(p2[1], 1e-4); BOOST_CHECK_CLOSE(p2[2], 20.1, 1e-4); } ================================================ FILE: test/collision.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Joseph Mirabel */ #include #define BOOST_TEST_MODULE COAL_COLLISION #include #include #include #include "coal/fwd.hh" COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #include "coal/collision.h" COAL_COMPILER_DIAGNOSTIC_POP #include "coal/BV/BV.h" #include "coal/mesh_loader/assimp.h" #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes.h" #include "../src/collision_node.h" #include "coal/internal/BV_splitter.h" #include "coal/internal/traversal_node_bvhs.h" #include "coal/internal/traversal_node_setup.h" #include "coal/timings.h" #include "fcl_resources/config.h" #include "utility.h" using namespace coal; namespace utf = boost::unit_test::framework; int num_max_contacts = (std::numeric_limits::max)(); BOOST_AUTO_TEST_CASE(OBB_Box_test) { Scalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; aabb1.min_ = Vec3s(-600, -600, -600); aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); Box box1; Transform3s box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); Scalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < transforms.size(); ++i) { AABB aabb; aabb.min_ = aabb1.min_ * 0.5; aabb.max_ = aabb1.max_ * 0.5; OBB obb2; convertBV(aabb, transforms[i], obb2); Box box2; Transform3s box2_tf; constructBox(aabb, transforms[i], box2, box2_tf); GJKSolver solver; bool overlap_obb = obb1.overlap(obb2); CollisionRequest request; CollisionResult result; // Convention: when doing primitive-primitive collision, either use // `collide` or `ShapeShapeCollide`. // **DO NOT** use methods of the GJKSolver directly if you don't know what // you are doing. ShapeShapeCollide(&box1, box1_tf, &box2, box2_tf, &solver, request, result); bool overlap_box = result.isCollision(); BOOST_CHECK(overlap_obb == overlap_box); } } BOOST_AUTO_TEST_CASE(OBB_shape_test) { Scalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; aabb1.min_ = Vec3s(-600, -600, -600); aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); Box box1; Transform3s box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); Scalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < transforms.size(); ++i) { Scalar len = (aabb1.max_[0] - aabb1.min_[0]) * Scalar(0.5); OBB obb2; GJKSolver solver; { Sphere sphere(len); computeBV(sphere, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); CollisionRequest request; CollisionResult result; ShapeShapeCollide(&box1, box1_tf, &sphere, transforms[i], &solver, request, result); bool overlap_sphere = result.isCollision(); // The sphere is contained inside obb2. So if the sphere overlaps with // obb1, then necessarily obb2 overlaps with obb1. BOOST_CHECK(overlap_obb >= overlap_sphere); } { Capsule capsule(len, 2 * len); computeBV(capsule, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); CollisionRequest request; CollisionResult result; ShapeShapeCollide(&box1, box1_tf, &capsule, transforms[i], &solver, request, result); bool overlap_capsule = result.isCollision(); BOOST_CHECK(overlap_obb >= overlap_capsule); } { Cone cone(len, 2 * len); computeBV(cone, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); CollisionRequest request; CollisionResult result; ShapeShapeCollide(&box1, box1_tf, &cone, transforms[i], &solver, request, result); bool overlap_cone = result.isCollision(); BOOST_CHECK(overlap_obb >= overlap_cone); } { Cylinder cylinder(len, 2 * len); computeBV(cylinder, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); CollisionRequest request; CollisionResult result; ShapeShapeCollide(&box1, box1_tf, &cylinder, transforms[i], &solver, request, result); bool overlap_cylinder = result.isCollision(); BOOST_CHECK(overlap_obb >= overlap_cylinder); } } } BOOST_AUTO_TEST_CASE(OBB_AABB_test) { Scalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; generateRandomTransforms(extents, transforms, n); AABB aabb1; aabb1.min_ = Vec3s(-600, -600, -600); aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; convertBV(aabb1, Transform3s(), obb1); for (std::size_t i = 0; i < transforms.size(); ++i) { AABB aabb; aabb.min_ = aabb1.min_ * 0.5; aabb.max_ = aabb1.max_ * 0.5; AABB aabb2 = translate(aabb, transforms[i].getTranslation()); OBB obb2; convertBV(aabb, Transform3s(transforms[i].getTranslation()), obb2); bool overlap_aabb = aabb1.overlap(aabb2); bool overlap_obb = obb1.overlap(obb2); if (overlap_aabb != overlap_obb) { std::cout << aabb1.min_ << " " << aabb1.max_ << std::endl; std::cout << aabb2.min_ << " " << aabb2.max_ << std::endl; std::cout << obb1.To << " " << obb1.extent << " " << obb1.axes << std::endl; std::cout << obb2.To << " " << obb2.extent << " " << obb2.axes << std::endl; } BOOST_CHECK(overlap_aabb == overlap_obb); } std::cout << std::endl; } std::ostream* bench_stream = NULL; bool bs_nl = true; bool bs_hp = false; #define BENCHMARK(stream) \ if (bench_stream != NULL) { \ *bench_stream << (bs_nl ? "" : ", ") << stream; \ bs_nl = false; \ } #define BENCHMARK_HEADER(stream) \ if (!bs_hp) { \ BENCHMARK(stream) \ } #define BENCHMARK_NEXT() \ if (bench_stream != NULL && !bs_nl) { \ *bench_stream << '\n'; \ bs_nl = true; \ bs_hp = true; \ } typedef std::vector Contacts_t; typedef boost::mpl::vector, KDOP<18>, KDOP<16>, kIOS, OBBRSS> BVs_t; std::vector splitMethods = boost::assign::list_of( SPLIT_METHOD_MEAN)(SPLIT_METHOD_MEDIAN)(SPLIT_METHOD_BV_CENTER); #define BV_STR_SPECIALIZATION(bv) \ template <> \ const char* str() { \ return #bv; \ } template const char* str(); BV_STR_SPECIALIZATION(AABB) BV_STR_SPECIALIZATION(OBB) BV_STR_SPECIALIZATION(RSS) BV_STR_SPECIALIZATION(KDOP<24>) BV_STR_SPECIALIZATION(KDOP<18>) BV_STR_SPECIALIZATION(KDOP<16>) BV_STR_SPECIALIZATION(kIOS) BV_STR_SPECIALIZATION(OBBRSS) template struct wrap {}; struct base_traits { enum { IS_IMPLEMENTED = true }; }; enum { Oriented = true, NonOriented = false, Recursive = true, NonRecursive = false }; template struct traits : base_traits {}; template struct traits, Oriented, recursive> : base_traits { enum { IS_IMPLEMENTED = false }; }; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS struct mesh_mesh_run_test { mesh_mesh_run_test(const std::vector& _transforms, const CollisionRequest _request) : transforms(_transforms), request(_request), enable_statistics(false), benchmark(false), isInit(false), indent(0) {} const std::vector& transforms; const CollisionRequest request; bool enable_statistics, benchmark; std::vector contacts; std::vector contacts_ref; bool isInit; int indent; COAL_COMPILER_DIAGNOSTIC_POP const char* getindent() { assert(indent < 9); static const char* t[] = {"", "\t", "\t\t", "\t\t\t", "\t\t\t\t", "\t\t\t\t\t", "\t\t\t\t\t\t", "\t\t\t\t\t\t\t", "\t\t\t\t\t\t\t\t"}; return t[indent]; } template void query(const std::vector& transforms, SplitMethodType splitMethod, const CollisionRequest request, std::vector& contacts) { BENCHMARK_HEADER("BV"); BENCHMARK_HEADER("oriented"); BENCHMARK_HEADER("Split method"); if (enable_statistics) { BENCHMARK_HEADER("num_bv_tests"); BENCHMARK_HEADER("num_leaf_tests"); } BENCHMARK_HEADER("numContacts"); BENCHMARK_HEADER("distance_lower_bound"); BENCHMARK_HEADER("time"); BENCHMARK_NEXT(); typedef BVHModel BVH_t; typedef shared_ptr BVHPtr_t; BVHPtr_t model1(new BVH_t), model2(new BVH_t); model1->bv_splitter.reset(new BVSplitter(splitMethod)); model2->bv_splitter.reset(new BVSplitter(splitMethod)); loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3s::Ones(), model1); loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3s::Ones(), model2); Timer timer(false); const Transform3s tf2; const std::size_t N = transforms.size(); contacts.resize(3 * N); if (traits::IS_IMPLEMENTED) { BOOST_TEST_MESSAGE(getindent() << "BV: " << str() << " oriented"); ++indent; for (std::size_t i = 0; i < transforms.size(); ++i) { const Transform3s& tf1 = transforms[i]; timer.start(); CollisionResult local_result; MeshCollisionTraversalNode node(request); node.enable_statistics = enable_statistics; bool success = initialize(node, *model1, tf1, *model2, tf2, local_result); BOOST_REQUIRE(success); collide(&node, request, local_result); timer.stop(); BENCHMARK(str()); BENCHMARK(1); BENCHMARK(splitMethod); if (enable_statistics) { BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests << " " << node.num_leaf_tests); BOOST_TEST_MESSAGE(getindent() << "nb contacts: " << local_result.numContacts()); BENCHMARK(node.num_bv_tests); BENCHMARK(node.num_leaf_tests); } BENCHMARK(local_result.numContacts()); BENCHMARK(local_result.distance_lower_bound); BENCHMARK(timer.duration().count()); BENCHMARK_NEXT(); if (local_result.numContacts() > 0) { local_result.getContacts(contacts[i]); std::sort(contacts[i].begin(), contacts[i].end()); } } --indent; } if (traits::IS_IMPLEMENTED) { BOOST_TEST_MESSAGE(getindent() << "BV: " << str()); ++indent; for (std::size_t i = 0; i < transforms.size(); ++i) { const Transform3s tf1 = transforms[i]; timer.start(); CollisionResult local_result; MeshCollisionTraversalNode node( request); node.enable_statistics = enable_statistics; BVH_t* model1_tmp = new BVH_t(*model1); Transform3s tf1_tmp = tf1; BVH_t* model2_tmp = new BVH_t(*model2); Transform3s tf2_tmp = tf2; bool success = initialize(node, *model1_tmp, tf1_tmp, *model2_tmp, tf2_tmp, local_result, true, true); BOOST_REQUIRE(success); collide(&node, request, local_result); delete model1_tmp; delete model2_tmp; timer.stop(); BENCHMARK(str()); BENCHMARK(2); BENCHMARK(splitMethod); if (enable_statistics) { BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests << " " << node.num_leaf_tests); BOOST_TEST_MESSAGE(getindent() << "nb contacts: " << local_result.numContacts()); BENCHMARK(node.num_bv_tests); BENCHMARK(node.num_leaf_tests); } BENCHMARK(local_result.numContacts()); BENCHMARK(local_result.distance_lower_bound); BENCHMARK(timer.duration().count()); BENCHMARK_NEXT(); if (local_result.numContacts() > 0) { local_result.getContacts(contacts[i + N]); std::sort(contacts[i + N].begin(), contacts[i + N].end()); } } --indent; } if (traits::IS_IMPLEMENTED) { BOOST_TEST_MESSAGE(getindent() << "BV: " << str() << " oriented non-recursive"); ++indent; for (std::size_t i = 0; i < transforms.size(); ++i) { timer.start(); const Transform3s tf1 = transforms[i]; CollisionResult local_result; MeshCollisionTraversalNode node(request); node.enable_statistics = enable_statistics; bool success = initialize(node, *model1, tf1, *model2, tf2, local_result); BOOST_REQUIRE(success); collide(&node, request, local_result, NULL, false); timer.stop(); BENCHMARK(str()); BENCHMARK(0); BENCHMARK(splitMethod); if (enable_statistics) { BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests << " " << node.num_leaf_tests); BOOST_TEST_MESSAGE(getindent() << "nb contacts: " << local_result.numContacts()); BENCHMARK(node.num_bv_tests); BENCHMARK(node.num_leaf_tests); } BENCHMARK(local_result.numContacts()); BENCHMARK(local_result.distance_lower_bound); BENCHMARK(timer.duration().count()); BENCHMARK_NEXT(); if (local_result.numContacts() > 0) { local_result.getContacts(contacts[i + 2 * N]); std::sort(contacts[i + 2 * N].begin(), contacts[i + 2 * N].end()); } } --indent; } } void check_contacts(std::size_t i0, std::size_t i1, bool warn) { for (std::size_t i = i0; i < i1; ++i) { Contacts_t in_ref_but_not_in_i; std::set_difference( contacts_ref[i].begin(), contacts_ref[i].end(), contacts[i].begin(), contacts[i].end(), std::inserter(in_ref_but_not_in_i, in_ref_but_not_in_i.begin())); if (!in_ref_but_not_in_i.empty()) { for (std::size_t j = 0; j < in_ref_but_not_in_i.size(); ++j) { if (warn) { BOOST_WARN_MESSAGE( false, "Missed contacts: " << in_ref_but_not_in_i[j].b1 << ", " << in_ref_but_not_in_i[j].b2); } else { BOOST_CHECK_MESSAGE( false, "Missed contacts: " << in_ref_but_not_in_i[j].b1 << ", " << in_ref_but_not_in_i[j].b2); } } } Contacts_t in_i_but_not_in_ref; std::set_difference( contacts[i].begin(), contacts[i].end(), contacts_ref[i].begin(), contacts_ref[i].end(), std::inserter(in_i_but_not_in_ref, in_i_but_not_in_ref.begin())); if (!in_i_but_not_in_ref.empty()) { for (std::size_t j = 0; j < in_i_but_not_in_ref.size(); ++j) { if (warn) { BOOST_WARN_MESSAGE( false, "False contacts: " << in_i_but_not_in_ref[j].b1 << ", " << in_i_but_not_in_ref[j].b2); } else { BOOST_CHECK_MESSAGE( false, "False contacts: " << in_i_but_not_in_ref[j].b1 << ", " << in_i_but_not_in_ref[j].b2); } } } } } template void check() { if (benchmark) return; const std::size_t N = transforms.size(); BOOST_REQUIRE_EQUAL(contacts.size(), 3 * N); BOOST_REQUIRE_EQUAL(contacts.size(), contacts_ref.size()); if (traits::IS_IMPLEMENTED) { BOOST_TEST_MESSAGE(getindent() << "BV: " << str() << " oriented"); ++indent; check_contacts(0, N, false); --indent; } if (traits::IS_IMPLEMENTED) { BOOST_TEST_MESSAGE(getindent() << "BV: " << str()); ++indent; check_contacts(N, 2 * N, true); --indent; } if (traits::IS_IMPLEMENTED) { BOOST_TEST_MESSAGE(getindent() << "BV: " << str() << " oriented non-recursive"); ++indent; check_contacts(2 * N, 3 * N, false); --indent; } } template void operator()(wrap) { for (std::size_t i = 0; i < splitMethods.size(); ++i) { BOOST_TEST_MESSAGE(getindent() << "splitMethod: " << splitMethods[i]); ++indent; query(transforms, splitMethods[i], request, (isInit ? contacts : contacts_ref)); if (isInit) check(); isInit = true; --indent; } } }; // This test // 1. load two objects "env.obj" and "rob.obj" from directory // fcl_resources, // 2. generates n random transformation and for each of them denote tf, // 2.1 performs a collision test where object 1 is in pose tf. All // the contacts are stored in vector global_pairs. // 2.2 performs a series of collision tests with the same object and // the same poses using various methods and various types of bounding // volumes. Each time the contacts are stored in vector global_pairs_now. // // The methods used to test collision are // - collide_Test that calls function collide with tf for object1 pose and // identity for the second object pose, // - collide_Test2 that moves all vertices of object1 in pose tf and that // calls function collide with identity for both object poses, // BOOST_AUTO_TEST_CASE(mesh_mesh) { std::vector transforms; Scalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG // if debug mode std::size_t n = 1; #else std::size_t n = 10; #endif n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n); generateRandomTransforms(extents, transforms, n); Eigen::IOFormat f(Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")"); for (std::size_t i = 0; i < transforms.size(); ++i) { BOOST_TEST_MESSAGE( "q" << i << "=" << transforms[i].getTranslation().format(f) << "+" << transforms[i].getQuatRotation().coeffs().format(f)); } COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS // Request all contacts and check that all methods give the same result. mesh_mesh_run_test runner( transforms, CollisionRequest(CONTACT, (size_t)num_max_contacts)); runner.enable_statistics = true; boost::mpl::for_each>(runner); COAL_COMPILER_DIAGNOSTIC_POP } BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) { std::vector transforms; Scalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG std::size_t n = 0; #else std::size_t n = 10; #endif n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n); generateRandomTransforms(extents, transforms, n); Eigen::IOFormat f(Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")"); for (std::size_t i = 0; i < transforms.size(); ++i) { BOOST_TEST_MESSAGE( "q" << i << "=" << transforms[i].getTranslation().format(f) << "+" << transforms[i].getQuatRotation().coeffs().format(f)); } // Request all contacts and check that all methods give the same result. typedef boost::mpl::vector, KDOP<18>, KDOP<16>, kIOS, OBBRSS> BVs_t; std::ofstream ofs("./collision.benchmark.csv", std::ofstream::out); bench_stream = &ofs; // without lower bound. mesh_mesh_run_test runner1(transforms, CollisionRequest()); runner1.enable_statistics = false; runner1.benchmark = true; boost::mpl::for_each>(runner1); // with lower bound. mesh_mesh_run_test runner2(transforms, CollisionRequest(DISTANCE_LOWER_BOUND, 1)); runner2.enable_statistics = false; runner2.benchmark = true; boost::mpl::for_each>(runner2); bench_stream = NULL; ofs.close(); } ================================================ FILE: test/collision_node_asserts.cpp ================================================ #define BOOST_TEST_MODULE COAL_COLLISION_NODE_ASSERT #include #include #include "coal/BVH/BVH_model.h" #include "coal/collision.h" using namespace coal; constexpr Scalar pi = boost::math::constants::pi(); Scalar DegToRad(const Scalar& deg) { static Scalar degToRad = pi / Scalar(180); return deg * degToRad; } std::vector dirs{Vec3s::UnitZ(), -Vec3s::UnitZ(), Vec3s::UnitY(), -Vec3s::UnitY(), Vec3s::UnitX(), -Vec3s::UnitX()}; BOOST_AUTO_TEST_CASE(TestTriangles) { std::vector triVertices{Vec3s(1, 0, 0), Vec3s(1, 1, 0), Vec3s(0, 1, 0)}; std::vector triangle{{0, 1, 2}}; BVHModel tri1{}; BVHModel tri2{}; tri1.beginModel(); tri1.addSubModel(triVertices, triangle); tri1.endModel(); tri2.beginModel(); tri2.addSubModel(triVertices, triangle); tri2.endModel(); CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); ComputeCollision compute(&tri1, &tri2); Transform3s tri1Tf{}; Transform3s tri2Tf{}; /// check some angles for two triangles for (int i = 0; i < 360; i += 30) { for (int j = 0; j < 180; j += 30) { for (int k = 0; k < 180; k += 30) { tri1Tf.setQuatRotation( Eigen::AngleAxis(0., Vec3s::UnitZ()) * Eigen::AngleAxis(DegToRad(Scalar(k)), Vec3s::UnitY())); tri2Tf.setQuatRotation( Eigen::AngleAxis(DegToRad(Scalar(i)), Vec3s::UnitZ()) * Eigen::AngleAxis(DegToRad(Scalar(j)), Vec3s::UnitY())); CollisionResult result; /// assertion: src/collision_node.cpp:58 BOOST_CHECK_NO_THROW(compute(tri2Tf, tri1Tf, request, result)); } } } } ================================================ FILE: test/contact_patch.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of INRIA nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #define BOOST_TEST_MODULE COAL_CONTACT_PATCH #include #include "coal/contact_patch.h" #include "utility.h" using namespace coal; BOOST_AUTO_TEST_CASE(box_box_no_collision) { const Scalar halfside = Scalar(0.5); const Box box1(2 * halfside, 2 * halfside, 2 * halfside); const Box box2(2 * halfside, 2 * halfside, 2 * halfside); const Transform3s tf1; Transform3s tf2; // set translation to separate the shapes const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, 2 * halfside + offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(!col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 0); } BOOST_AUTO_TEST_CASE(box_sphere) { const Scalar halfside = Scalar(0.5); const Box box(2 * halfside, 2 * halfside, 2 * halfside); const Sphere sphere(halfside); const Transform3s tf1; Transform3s tf2; // set translation to have a collision const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&box, tf1, &sphere, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&box, tf1, &sphere, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); const Scalar tol = Scalar(1e-8); EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); BOOST_CHECK(std::abs(contact_patch.penetration_depth - contact.penetration_depth) < tol); } } BOOST_AUTO_TEST_CASE(box_box) { const Scalar halfside = 0.5; const Box box1(2 * halfside, 2 * halfside, 2 * halfside); const Box box2(2 * halfside, 2 * halfside, 2 * halfside); const Transform3s tf1; Transform3s tf2; // set translation to have a collision const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, patch_res1); coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; const std::array corners = { Vec3s(halfside, halfside, halfside), Vec3s(halfside, -halfside, halfside), Vec3s(-halfside, -halfside, halfside), Vec3s(-halfside, halfside, halfside), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] + (contact.penetration_depth * contact.normal) / 2); } BOOST_CHECK(patch_res1.getContactPatch(0).isSame(expected, tol)); BOOST_CHECK(patch_res2.getContactPatch(0).isSame(expected, tol)); } } BOOST_AUTO_TEST_CASE(halfspace_box) { const Halfspace hspace(0, 0, 1, 0); const Scalar halfside = Scalar(0.5); const Box box(2 * halfside, 2 * halfside, 2 * halfside); const Transform3s tf1; Transform3s tf2; // set translation to have a collision const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&hspace, tf1, &box, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); coal::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, patch_res1); coal::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; const std::array corners = { tf2.transform(Vec3s(halfside, halfside, -halfside)), tf2.transform(Vec3s(halfside, -halfside, -halfside)), tf2.transform(Vec3s(-halfside, -halfside, -halfside)), tf2.transform(Vec3s(-halfside, halfside, -halfside)), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] - (contact.penetration_depth * contact.normal) / 2); } BOOST_CHECK(patch_res1.getContactPatch(0).isSame(expected, tol)); BOOST_CHECK(patch_res2.getContactPatch(0).isSame(expected, tol)); } } BOOST_AUTO_TEST_CASE(cylinder_plane_simplification) { const Scalar radius = Scalar(0.25); const Scalar length = Scalar(1.0); const Cylinder cylinder(radius, length); const Halfspace plane(Vec3s(0, 0, 1), Scalar(0)); Transform3s tf_cylinder; tf_cylinder.setIdentity(); const Scalar penetration = -Scalar(0.02); tf_cylinder.translation() = Vec3s(0, 0, cylinder.halfLength + penetration); const Transform3s tf_plane; const CollisionRequest col_req; CollisionResult col_res; coal::collide(&cylinder, tf_cylinder, &plane, tf_plane, col_req, col_res); BOOST_REQUIRE(col_res.isCollision()); const ContactPatchRequest patch_req(col_req); ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&cylinder, tf_cylinder, &plane, tf_plane, col_res, patch_req, patch_res); BOOST_REQUIRE_EQUAL(patch_res.numContactPatches(), 1); const ContactPatch& patch = patch_res.getContactPatch(0); BOOST_REQUIRE_GE(patch.size(), static_cast(6)); const Scalar tol = Scalar(1e-8); BOOST_CHECK( patch.tf.translation().isApprox(Vec3s(0, 0, penetration / 2), tol)); BOOST_CHECK_SMALL(std::abs(patch.penetration_depth - penetration), tol); const auto point_subset_check = [&](const ContactPatch& reduced, const size_t target) { BOOST_CHECK_EQUAL(reduced.size(), target); BOOST_CHECK(reduced.tf.translation().isApprox(patch.tf.translation(), tol)); BOOST_CHECK(reduced.tf.rotation().isApprox(patch.tf.rotation(), tol)); BOOST_CHECK_SMALL( std::abs(reduced.penetration_depth - patch.penetration_depth), tol); for (size_t i = 0; i < reduced.size(); ++i) { bool found = false; for (size_t j = 0; j < patch.size(); ++j) { if (reduced.point(i).isApprox(patch.point(j), tol)) { found = true; break; } } BOOST_CHECK(found); } }; ContactPatchSimplifierGreedy greedy; ContactPatch greedy_out(patch.size()); greedy.compute(patch, 4, greedy_out); point_subset_check(greedy_out, 4); greedy.compute(patch, 3, greedy_out); point_subset_check(greedy_out, 3); ContactPatch patch_inplace = patch; greedy.simplify(patch_inplace, 4); point_subset_check(patch_inplace, 4); ContactPatchSimplifierMaxArea max_area; ContactPatch max_out(patch.size()); max_area.compute(patch, 4, max_out); point_subset_check(max_out, 4); max_area.compute(patch, 2, max_out); point_subset_check(max_out, 2); ContactPatch max_inplace = patch; max_area.simplify(max_inplace, 3); point_subset_check(max_inplace, 3); } BOOST_AUTO_TEST_CASE(halfspace_capsule) { const Halfspace hspace(0, 0, 1, 0); const Scalar radius = Scalar(0.25); const Scalar height = 1.; const Capsule capsule(radius, height); const Transform3s tf1; Transform3s tf2; // set translation to have a collision const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 1; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; const Vec3s capsule_end(0, 0, -capsule.halfLength); expected.addPoint(tf2.transform(capsule_end)); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.tf == contact_patch.tf); BOOST_CHECK(expected.isSame(contact_patch, tol)); } // Rotate capsule 180 degrees around y-axis // Should only have one contact. tf2.rotation().col(0) << -1, 0, 0; tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 1; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; const Vec3s capsule_end(0, 0, capsule.halfLength); expected.addPoint(tf2.transform(capsule_end)); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.tf == contact_patch.tf); BOOST_CHECK(expected.isSame(contact_patch, tol)); } // Rotate cone 90 degrees around y-axis // Should only have two contacts. tf2.rotation().col(0) << 0, 0, 1; tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << -1, 0, 0; tf2.translation() << 0, 0, capsule.radius - offset; col_res.clear(); coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 2; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; const Vec3s p1(-capsule.radius, 0, capsule.halfLength); const Vec3s p2(-capsule.radius, 0, -capsule.halfLength); expected.addPoint(tf2.transform(p1)); expected.addPoint(tf2.transform(p2)); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.tf == contact_patch.tf); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } BOOST_AUTO_TEST_CASE(halfspace_cone) { const Halfspace hspace(0, 0, 1, 0); const Scalar radius = 0.25; const Scalar height = 1.; const Cone cone(radius, height); const Transform3s tf1; Transform3s tf2; // set translation to have a collision const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = ContactPatch::default_preallocated_size; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; std::array points; const Scalar angle_increment = 2.0 * (Scalar)(EIGEN_PI) / ((Scalar)(6)); for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { const Scalar theta = (Scalar)(i)*angle_increment; Vec3s point_on_cone_base(std::cos(theta) * cone.radius, std::sin(theta) * cone.radius, -cone.halfLength); expected.addPoint(tf2.transform(point_on_cone_base)); } const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.tf == contact_patch.tf); BOOST_CHECK(expected.isSame(contact_patch, tol)); } // Rotate cone 180 degrees around y-axis // Should only have one contact, due to cone-tip/halfspace collision. tf2.rotation().col(0) << -1, 0, 0; tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); const Scalar tol = Scalar(1e-8); EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); BOOST_CHECK(std::abs(contact_patch.penetration_depth - contact.penetration_depth) < tol); const size_t expected_size = 1; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; const Vec3s cone_tip(0, 0, cone.halfLength); expected.addPoint(tf2.transform(cone_tip)); BOOST_CHECK(contact_patch.isSame(expected, tol)); } // Rotate cone 90 degrees around y-axis // Should only have one contact, on cone circle basis. tf2.rotation().col(0) << 0, 0, 1; tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << -1, 0, 0; tf2.translation() << 0, 0, cone.radius - offset; col_res.clear(); coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); const Scalar tol = Scalar(1e-8); EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); BOOST_CHECK(std::abs(contact_patch.penetration_depth - contact.penetration_depth) < tol); const size_t expected_size = 1; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; const Vec3s point_on_circle_basis(-cone.radius, 0, -cone.halfLength); expected.addPoint(tf2.transform(point_on_circle_basis)); BOOST_CHECK(contact_patch.isSame(expected, tol)); } } BOOST_AUTO_TEST_CASE(halfspace_cylinder) { const Halfspace hspace(0, 0, 1, 0); const Scalar radius = Scalar(0.25); const Scalar height = 1; const Cylinder cylinder(radius, height); const Transform3s tf1; Transform3s tf2; // set translation to have a collision const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); if (col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const size_t expected_size = ContactPatch::default_preallocated_size; const Scalar tol = Scalar(1e-6); ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; std::array points; const Scalar angle_increment = 2.0 * (Scalar)(EIGEN_PI) / ((Scalar)(6)); for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { const Scalar theta = (Scalar)(i)*angle_increment; Vec3s point_on_cone_base(std::cos(theta) * cylinder.radius, std::sin(theta) * cylinder.radius, -cylinder.halfLength); expected.addPoint(tf2.transform(point_on_cone_base)); } const ContactPatchRequest patch_req; BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.tf == contact_patch.tf); BOOST_CHECK(expected.isSame(contact_patch, tol)); } // Rotate cylinder 180 degrees around y-axis. // Should only have the same contact-patch, due to cylinder symmetry. tf2.rotation().col(0) << -1, 0, 0; tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.tf == contact_patch.tf); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } // Rotate cylinder 90 degrees around y-axis. // Should have 2 contact points. tf2.rotation().col(0) << 0, 0, 1; tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << -1, 0, 0; tf2.translation() << 0, 0, cylinder.radius - offset; col_res.clear(); coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (col_res.isCollision() && patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); const size_t expected_size = 2; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint( tf2.transform(Vec3s(cylinder.radius, 0, cylinder.halfLength))); expected.addPoint( tf2.transform(Vec3s(cylinder.radius, 0, -cylinder.halfLength))); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } BOOST_AUTO_TEST_CASE(cylinder_box) { const Scalar radius = Scalar(0.25); const Scalar length = Scalar(1.0); const Cylinder cylinder(radius, length); Transform3s tf_cylinder; tf_cylinder.setIdentity(); const Scalar penetration = -Scalar(0.02); tf_cylinder.translation() = Vec3s(0, 0, cylinder.halfLength + penetration); const Scalar halfside = 0.5; const Box box(2 * halfside * Vec3s::Ones()); const Transform3s tf_box; const CollisionRequest col_req; CollisionResult col_res; coal::collide(&cylinder, tf_cylinder, &box, tf_box, col_req, col_res); BOOST_REQUIRE(col_res.isCollision()); const ContactPatchRequest patch_req(col_req); ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&cylinder, tf_cylinder, &box, tf_box, col_res, patch_req, patch_res); // The cylinder lower circle is parallel and touching the box's upper face. // The radius of the cylinder is smaller than the halfside of the box. // Thus there should be ContactPatch::default_preallocated_size points in the // contact patch. BOOST_REQUIRE_EQUAL(patch_res.numContactPatches(), 1); const ContactPatch& patch = patch_res.getContactPatch(0); BOOST_REQUIRE_GE(patch.size(), static_cast( ContactPatch::default_preallocated_size)); } BOOST_AUTO_TEST_CASE(cylinder_convex) { const Scalar radius = Scalar(0.25); const Scalar length = Scalar(1.0); const Cylinder cylinder(radius, length); Transform3s tf_cylinder; tf_cylinder.setIdentity(); const Scalar penetration = -Scalar(0.02); tf_cylinder.translation() = Vec3s(0, 0, cylinder.halfLength + penetration); const Scalar halfside = 0.5; const ConvexTpl box = buildBox(halfside, halfside, halfside); const Transform3s tf_box; const CollisionRequest col_req; CollisionResult col_res; coal::collide(&cylinder, tf_cylinder, &box, tf_box, col_req, col_res); BOOST_REQUIRE(col_res.isCollision()); const ContactPatchRequest patch_req(col_req); ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&cylinder, tf_cylinder, &box, tf_box, col_res, patch_req, patch_res); // The cylinder lower circle is parallel and touching the box's upper face. // The radius of the cylinder is smaller than the halfside of the box. // Thus there should be ContactPatch::default_preallocated_size points in the // contact patch. BOOST_REQUIRE_EQUAL(patch_res.numContactPatches(), 1); const ContactPatch& patch = patch_res.getContactPatch(0); BOOST_REQUIRE_GE(patch.size(), static_cast( ContactPatch::default_preallocated_size)); } BOOST_AUTO_TEST_CASE(convex_convex) { const Scalar halfside = 0.5; const ConvexTpl box1(buildBox(halfside, halfside, halfside)); const ConvexTpl box2(buildBox(halfside, halfside, halfside)); const Transform3s tf1; Transform3s tf2; // set translation to have a collision const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, patch_res1); coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; const std::array corners = { Vec3s(halfside, halfside, halfside), Vec3s(halfside, -halfside, halfside), Vec3s(-halfside, -halfside, halfside), Vec3s(-halfside, halfside, halfside), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] + (contact.penetration_depth * contact.normal) / 2); } BOOST_CHECK(patch_res1.getContactPatch(0).isSame(expected, tol)); BOOST_CHECK(patch_res2.getContactPatch(0).isSame(expected, tol)); } } BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { // This case covers the segment-segment edge case of contact patches. // Two tetrahedrons make contact on one of their edge. const size_t expected_size = 2; const Vec3s expected_cp1(0, 0.5, 0); const Vec3s expected_cp2(0, 1, 0); const Transform3s tf1; // identity const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); { // Case 1 - Face-Face contact std::shared_ptr> pts1(new std::vector({ Vec3s(-1, 0, 0), Vec3s(0, 0, 0), Vec3s(0, 1, 0), Vec3s(-1, -1, -1), })); std::shared_ptr> tris1(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra1(pts1, 4, tris1, 4); std::shared_ptr> pts2(new std::vector({ Vec3s(0, 0.5, 0), Vec3s(0, 1.5, 0), Vec3s(1, 0.5, 0), Vec3s(1, 1, 1), })); std::shared_ptr> tris2(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra2(pts2, 4, tris2, 4); col_res.clear(); coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); ContactPatch expected(expected_size); // GJK/EPA can return any normal which is in the dual cone // of the cone {(-1, 0, 0)}. expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint(expected_cp1); expected.addPoint(expected_cp2); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } { // Case 2 - Face-Segment contact std::shared_ptr> pts1(new std::vector({ Vec3s(-1, 0, Scalar(-0.2)), Vec3s(0, 0, 0), Vec3s(0, 1, 0), Vec3s(-1, -1, -1), })); std::shared_ptr> tris1(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra1(pts1, 4, tris1, 4); std::shared_ptr> pts2(new std::vector({ Vec3s(0, 0.5, 0), Vec3s(0, 1.5, 0), Vec3s(1, 0.5, 0), Vec3s(1, 1, 1), })); std::shared_ptr> tris2(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra2(pts2, 4, tris2, 4); col_res.clear(); coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint(expected_cp1); expected.addPoint(expected_cp2); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } { // Case 3 - Segment-Segment contact std::shared_ptr> pts1(new std::vector({ Vec3s(-1, 0, Scalar(-0.2)), Vec3s(0, 0, 0), Vec3s(0, 1, 0), Vec3s(-1, -1, -1), })); std::shared_ptr> tris1(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra1(pts1, 4, tris1, 4); std::shared_ptr> pts2(new std::vector({ Vec3s(0, 0.5, 0), Vec3s(0, 1.5, 0), Vec3s(1, 0.5, 0.5), Vec3s(1, 1, 1), })); std::shared_ptr> tris2(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra2(pts2, 4, tris2, 4); col_res.clear(); coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint(expected_cp1); expected.addPoint(expected_cp2); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } } BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { // This case covers the vertex-vertex edge case of contact patches. // Two tetrahedrons make contact on one of their vertex. const size_t expected_size = 1; const Vec3s expected_cp(0, 0, 0); const Transform3s tf1; // identity const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); { // Case 1 - Face-Face contact std::shared_ptr> pts1(new std::vector({ Vec3s(-1, 0, 0), Vec3s(0, 0, 0), Vec3s(0, 1, 0), Vec3s(-1, -1, -1), })); std::shared_ptr> tris1(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra1(pts1, 4, tris1, 4); std::shared_ptr> pts2(new std::vector({ Vec3s(1, 0, 0), Vec3s(0, 0, 0), Vec3s(0, -1, 0), Vec3s(1, 1, 1), })); std::shared_ptr> tris2(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra2(pts2, 4, tris2, 4); col_res.clear(); coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint(expected_cp); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } { // Case 2 - Segment-Face contact std::shared_ptr> pts1(new std::vector({ Vec3s(-1, 0, -0.5), Vec3s(0, 0, 0), Vec3s(0, 1, 0), Vec3s(-1, -1, -1), })); std::shared_ptr> tris1(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra1(pts1, 4, tris1, 4); std::shared_ptr> pts2(new std::vector({ Vec3s(1, 0, 0), Vec3s(0, 0, 0), Vec3s(0, -1, 0), Vec3s(1, 1, 1), })); std::shared_ptr> tris2(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra2(pts2, 4, tris2, 4); col_res.clear(); coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint(expected_cp); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } { // Case 2 - Segment-Segment contact std::shared_ptr> pts1(new std::vector({ Vec3s(-1, 0, Scalar(-0.2)), Vec3s(0, 0, 0), Vec3s(0, 1, 0), Vec3s(-1, -1, -1), })); std::shared_ptr> tris1(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra1(pts1, 4, tris1, 4); std::shared_ptr> pts2(new std::vector({ Vec3s(1, 0, 0), Vec3s(0, 0, 0), Vec3s(0, -1, 0.5), Vec3s(1, 1, 1), })); std::shared_ptr> tris2(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra2(pts2, 4, tris2, 4); col_res.clear(); coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint(expected_cp); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } } BOOST_AUTO_TEST_CASE(edge_case_segment_face) { // This case covers the segment-face edge case of contact patches. // Two tetrahedrons make contact on one of their segment/face respectively. const size_t expected_size = 2; const Vec3s expected_cp1(0, 0, 0); const Vec3s expected_cp2(-0.5, 0.5, 0); const Transform3s tf1; // identity const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); { std::shared_ptr> pts1(new std::vector({ Vec3s(-1, 0, -0), Vec3s(0, 0, 0), Vec3s(0, 1, 0), Vec3s(-1, -1, -1), })); std::shared_ptr> tris1(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra1(pts1, 4, tris1, 4); std::shared_ptr> pts2(new std::vector({ Vec3s(-0.5, 0.5, 0), Vec3s(0.5, -0.5, 0), Vec3s(1, 0.5, 0.5), Vec3s(1, 1, 1), })); std::shared_ptr> tris2(new std::vector( {Triangle32(0, 1, 2), Triangle32(0, 2, 3), Triangle32(0, 3, 1), Triangle32(2, 1, 3)})); ConvexTpl tetra2(pts2, 4, tris2, 4); col_res.clear(); coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); const Scalar tol = Scalar(1e-6); ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint(expected_cp1); expected.addPoint(expected_cp2); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); } } } ================================================ FILE: test/convex.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2019, LAAS-CNRS * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Joseph Mirabel */ #define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #include "coal/shape/convex.h" #include "coal/collision.h" #include "coal/distance.h" #include "utility.h" using namespace coal; BOOST_AUTO_TEST_CASE(convex) { Scalar l = 1, w = 1, d = 1; ConvexTpl box(buildBox(l, w, d)); // Check neighbors for (size_t i = 0; i < 8; ++i) { BOOST_CHECK_EQUAL((*box.neighbors)[i].count, 3); } BOOST_CHECK_EQUAL(box.neighbor(0, 0), 1); BOOST_CHECK_EQUAL(box.neighbor(0, 1), 2); BOOST_CHECK_EQUAL(box.neighbor(0, 2), 4); BOOST_CHECK_EQUAL(box.neighbor(1, 0), 0); BOOST_CHECK_EQUAL(box.neighbor(1, 1), 3); BOOST_CHECK_EQUAL(box.neighbor(1, 2), 5); BOOST_CHECK_EQUAL(box.neighbor(2, 0), 0); BOOST_CHECK_EQUAL(box.neighbor(2, 1), 3); BOOST_CHECK_EQUAL(box.neighbor(2, 2), 6); BOOST_CHECK_EQUAL(box.neighbor(3, 0), 1); BOOST_CHECK_EQUAL(box.neighbor(3, 1), 2); BOOST_CHECK_EQUAL(box.neighbor(3, 2), 7); BOOST_CHECK_EQUAL(box.neighbor(4, 0), 0); BOOST_CHECK_EQUAL(box.neighbor(4, 1), 5); BOOST_CHECK_EQUAL(box.neighbor(4, 2), 6); BOOST_CHECK_EQUAL(box.neighbor(5, 0), 1); BOOST_CHECK_EQUAL(box.neighbor(5, 1), 4); BOOST_CHECK_EQUAL(box.neighbor(5, 2), 7); BOOST_CHECK_EQUAL(box.neighbor(6, 0), 2); BOOST_CHECK_EQUAL(box.neighbor(6, 1), 4); BOOST_CHECK_EQUAL(box.neighbor(6, 2), 7); BOOST_CHECK_EQUAL(box.neighbor(7, 0), 3); BOOST_CHECK_EQUAL(box.neighbor(7, 1), 5); BOOST_CHECK_EQUAL(box.neighbor(7, 2), 6); // Cast to int16 and check that the box is the same ConvexTpl box16 = box.cast(); BOOST_CHECK_EQUAL(box16.neighbor(0, 0), 1); BOOST_CHECK_EQUAL(box16.neighbor(0, 1), 2); BOOST_CHECK_EQUAL(box16.neighbor(0, 2), 4); BOOST_CHECK_EQUAL(box16.neighbor(1, 0), 0); BOOST_CHECK_EQUAL(box16.neighbor(1, 1), 3); BOOST_CHECK_EQUAL(box16.neighbor(1, 2), 5); BOOST_CHECK_EQUAL(box16.neighbor(2, 0), 0); BOOST_CHECK_EQUAL(box16.neighbor(2, 1), 3); BOOST_CHECK_EQUAL(box16.neighbor(2, 2), 6); BOOST_CHECK_EQUAL(box16.neighbor(3, 0), 1); BOOST_CHECK_EQUAL(box16.neighbor(3, 1), 2); BOOST_CHECK_EQUAL(box16.neighbor(3, 2), 7); BOOST_CHECK_EQUAL(box16.neighbor(4, 0), 0); BOOST_CHECK_EQUAL(box16.neighbor(4, 1), 5); BOOST_CHECK_EQUAL(box16.neighbor(4, 2), 6); BOOST_CHECK_EQUAL(box16.neighbor(5, 0), 1); BOOST_CHECK_EQUAL(box16.neighbor(5, 1), 4); BOOST_CHECK_EQUAL(box16.neighbor(5, 2), 7); BOOST_CHECK_EQUAL(box16.neighbor(6, 0), 2); BOOST_CHECK_EQUAL(box16.neighbor(6, 1), 4); BOOST_CHECK_EQUAL(box16.neighbor(6, 2), 7); BOOST_CHECK_EQUAL(box16.neighbor(7, 0), 3); BOOST_CHECK_EQUAL(box16.neighbor(7, 1), 5); BOOST_CHECK_EQUAL(box16.neighbor(7, 2), 6); const std::vector& box_points = *(box.points); const std::vector& box16_points = *(box16.points); for (size_t i = 0; i < box.points->size(); ++i) { EIGEN_VECTOR_IS_APPROX(box_points[i], box16_points[i], 1e-12); } } template void compareShapeIntersection(const Sa& sa, const Sb& sb, const Transform3s& tf1, const Transform3s& tf2, Scalar tol = Scalar(1e-9)) { CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); CollisionResult resA, resB; collide(&sa, tf1, &sa, tf2, request, resA); collide(&sb, tf1, &sb, tf2, request, resB); BOOST_CHECK_EQUAL(resA.isCollision(), resB.isCollision()); BOOST_CHECK_EQUAL(resA.numContacts(), resB.numContacts()); if (resA.isCollision() && resB.isCollision()) { Contact cA = resA.getContact(0), cB = resB.getContact(0); BOOST_TEST_MESSAGE(tf1 << '\n' << cA.pos.format(pyfmt) << '\n' << '\n' << tf2 << '\n' << cB.pos.format(pyfmt) << '\n'); // Only warnings because there are still some bugs. BOOST_WARN_SMALL((cA.pos - cB.pos).squaredNorm(), tol); BOOST_WARN_SMALL((cA.normal - cB.normal).squaredNorm(), tol); } else { BOOST_CHECK_CLOSE(resA.distance_lower_bound, resB.distance_lower_bound, tol); // distances should be same } } template void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3s& tf1, const Transform3s& tf2, Scalar tol = Scalar(1e-9)) { DistanceRequest request(true); DistanceResult resA, resB; distance(&sa, tf1, &sa, tf2, request, resA); distance(&sb, tf1, &sb, tf2, request, resB); BOOST_TEST_MESSAGE(tf1 << '\n' << resA.normal.format(pyfmt) << '\n' << resA.nearest_points[0].format(pyfmt) << '\n' << resA.nearest_points[1].format(pyfmt) << '\n' << '\n' << tf2 << '\n' << resB.normal.format(pyfmt) << '\n' << resB.nearest_points[0].format(pyfmt) << '\n' << resB.nearest_points[1].format(pyfmt) << '\n'); // TODO in one case, there is a mismatch between the distances and I cannot // say which one is correct. To visualize the case, use script // test/geometric_shapes.py BOOST_WARN_CLOSE(resA.min_distance, resB.min_distance, tol); // BOOST_CHECK_CLOSE(resA.min_distance, resB.min_distance, tol); // Only warnings because there are still some bugs. BOOST_WARN_SMALL((resA.normal - resA.normal).squaredNorm(), tol); BOOST_WARN_SMALL( (resA.nearest_points[0] - resB.nearest_points[0]).squaredNorm(), tol); BOOST_WARN_SMALL( (resA.nearest_points[1] - resB.nearest_points[1]).squaredNorm(), tol); } BOOST_AUTO_TEST_CASE(compare_convex_box) { Scalar extents[6] = {0, 0, 0, 10, 10, 10}; Scalar l = 1, w = 1, d = 1, eps = Scalar(1e-4); Box box(l * 2, w * 2, d * 2); ConvexTpl convex_box(buildBox(l, w, d)); Transform3s tf1; Transform3s tf2; tf2.setTranslation(Vec3s(3, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); compareShapeDistance(box, convex_box, tf1, tf2, eps); tf2.setTranslation(Vec3s(0, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); compareShapeDistance(box, convex_box, tf1, tf2, eps); for (int i = 0; i < 1000; ++i) { generateRandomTransform(extents, tf2); compareShapeIntersection(box, convex_box, tf1, tf2, eps); compareShapeDistance(box, convex_box, tf1, tf2, eps); } } #ifdef COAL_HAS_QHULL BOOST_AUTO_TEST_CASE(convex_hull_throw) { std::shared_ptr> points( new std::vector({Vec3s(1, 1, 1), Vec3s(0, 0, 0), Vec3s(1, 0, 0)})); BOOST_CHECK_THROW(ConvexBase32::convexHull(points, 0, false, NULL), std::invalid_argument); BOOST_CHECK_THROW(ConvexBase32::convexHull(points, 1, false, NULL), std::invalid_argument); BOOST_CHECK_THROW(ConvexBase32::convexHull(points, 2, false, NULL), std::invalid_argument); BOOST_CHECK_THROW(ConvexBase32::convexHull(points, 3, false, NULL), std::invalid_argument); } BOOST_AUTO_TEST_CASE(convex_hull_quad) { std::shared_ptr> points(new std::vector({ Vec3s(1, 1, 1), Vec3s(0, 0, 0), Vec3s(1, 0, 0), Vec3s(0, 0, 1), })); ConvexBase32* convexHull = ConvexBase32::convexHull(points, 4, false, NULL); BOOST_REQUIRE_EQUAL(convexHull->num_points, 4); BOOST_CHECK_EQUAL((*(convexHull->neighbors))[0].count, 3); BOOST_CHECK_EQUAL((*(convexHull->neighbors))[1].count, 3); BOOST_CHECK_EQUAL((*(convexHull->neighbors))[2].count, 3); delete convexHull; } BOOST_AUTO_TEST_CASE(convex_hull_box_like) { std::shared_ptr> points(new std::vector({ Vec3s(1, 1, 1), Vec3s(1, 1, -1), Vec3s(1, -1, 1), Vec3s(1, -1, -1), Vec3s(-1, 1, 1), Vec3s(-1, 1, -1), Vec3s(-1, -1, 1), Vec3s(-1, -1, -1), Vec3s(0, 0, 0), Vec3s(0, 0, Scalar(0.99)), })); ConvexBase32* convexHull = ConvexBase32::convexHull(points, 9, false, NULL); BOOST_REQUIRE_EQUAL(8, convexHull->num_points); { const std::vector& cvxhull_points = *(convexHull->points); for (size_t i = 0; i < 8; ++i) { BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1)); BOOST_CHECK_EQUAL((*(convexHull->neighbors))[i].count, 3); } } delete convexHull; convexHull = ConvexBase32::convexHull(points, 9, true, NULL); ConvexTpl* convex_tri = dynamic_cast*>(convexHull); BOOST_CHECK(convex_tri != NULL); BOOST_REQUIRE_EQUAL(8, convexHull->num_points); { const std::vector& cvxhull_points = *(convexHull->points); for (size_t i = 0; i < 8; ++i) { BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1)); BOOST_CHECK((*(convexHull->neighbors))[i].count >= 3); BOOST_CHECK((*(convexHull->neighbors))[i].count <= 6); } } delete convexHull; } BOOST_AUTO_TEST_CASE(convex_copy_constructor) { ConvexTpl* convexHullTriCopy; { std::shared_ptr> points(new std::vector({ Vec3s(1, 1, 1), Vec3s(1, 1, -1), Vec3s(1, -1, 1), Vec3s(1, -1, -1), Vec3s(-1, 1, 1), Vec3s(-1, 1, -1), Vec3s(-1, -1, 1), Vec3s(-1, -1, -1), Vec3s(0, 0, 0), })); ConvexTpl* convexHullTri = dynamic_cast*>( ConvexBase32::convexHull(points, 9, true, NULL)); convexHullTriCopy = new ConvexTpl(*convexHullTri); BOOST_CHECK(*convexHullTri == *convexHullTriCopy); } ConvexTpl* convexHullTriCopyOfCopy = new ConvexTpl(*convexHullTriCopy); BOOST_CHECK(*convexHullTriCopyOfCopy == *convexHullTriCopy); } BOOST_AUTO_TEST_CASE(convex_clone) { std::shared_ptr> points(new std::vector({ Vec3s(1, 1, 1), Vec3s(1, 1, -1), Vec3s(1, -1, 1), Vec3s(1, -1, -1), Vec3s(-1, 1, 1), Vec3s(-1, 1, -1), Vec3s(-1, -1, 1), Vec3s(-1, -1, -1), Vec3s(0, 0, 0), })); ConvexTpl* convexHullTri = dynamic_cast*>( ConvexBase32::convexHull(points, 9, true, NULL)); ConvexTpl* convexHullTriCopy; convexHullTriCopy = convexHullTri->clone(); BOOST_CHECK(*convexHullTri == *convexHullTriCopy); } #endif ================================================ FILE: test/distance.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE COAL_DISTANCE #include #include #include #include "coal/internal/traversal_node_bvhs.h" #include "coal/internal/traversal_node_setup.h" #include "../src/collision_node.h" #include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" using namespace coal; namespace utf = boost::unit_test::framework; bool verbose = false; Scalar DELTA = Scalar(0.001); template void distance_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose = true); bool collide_Test_OBB(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template void distance_Test_Oriented(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose = true); inline bool nearlyEqual(const Vec3s& a, const Vec3s& b) { if (fabs(a[0] - b[0]) > DELTA) return false; if (fabs(a[1] - b[1]) > DELTA) return false; if (fabs(a[2] - b[2]) > DELTA) return false; return true; } BOOST_AUTO_TEST_CASE(mesh_distance) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); std::vector transforms; // t0 Scalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG // if debug mode std::size_t n = 1; #else std::size_t n = 10; #endif n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n); generateRandomTransforms(extents, transforms, n); double dis_time = 0; double col_time = 0; DistanceRes res, res_now; for (std::size_t i = 0; i < transforms.size(); ++i) { auto timer_col = std::chrono::high_resolution_clock::now(); collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); col_time += std::chrono::duration_cast>( std::chrono::high_resolution_clock::now() - timer_col) .count(); auto timer_dist = std::chrono::high_resolution_clock::now(); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose); dis_time += std::chrono::duration_cast>( std::chrono::high_resolution_clock::now() - timer_dist) .count(); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented( transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); } BOOST_TEST_MESSAGE("distance timing: " << dis_time << " sec"); BOOST_TEST_MESSAGE("collision timing: " << col_time << " sec"); } template void distance_Test_Oriented(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); DistanceResult local_result; TraversalNode node; if (!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, Transform3s(), DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; distance(&node, NULL, qsize); // points are in local coordinate, to global coordinate Vec3s p1 = local_result.nearest_points[0]; Vec3s p2 = local_result.nearest_points[1]; distance_result.distance = local_result.min_distance; distance_result.p1 = p1; distance_result.p2 = p2; if (verbose) { std::cout << "distance " << local_result.min_distance << std::endl; std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl; std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl; std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; } } template void distance_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3s pose1(tf), pose2; DistanceResult local_result; MeshDistanceTraversalNode node; if (!initialize(node, m1, pose1, m2, pose2, DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; distance(&node, NULL, qsize); distance_result.distance = local_result.min_distance; distance_result.p1 = local_result.nearest_points[0]; distance_result.p2 = local_result.nearest_points[1]; if (verbose) { std::cout << "distance " << local_result.min_distance << std::endl; std::cout << local_result.nearest_points[0][0] << " " << local_result.nearest_points[0][1] << " " << local_result.nearest_points[0][2] << std::endl; std::cout << local_result.nearest_points[1][0] << " " << local_result.nearest_points[1][1] << " " << local_result.nearest_points[1][2] << std::endl; std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; } } bool collide_Test_OBB(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); CollisionResult local_result; CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); MeshCollisionTraversalNodeOBB node(request); bool success(initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, Transform3s(), local_result)); BOOST_REQUIRE(success); node.enable_statistics = verbose; collide(&node, request, local_result); if (local_result.numContacts() > 0) return true; else return false; } ================================================ FILE: test/distance_lower_bound.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2014-2016, CNRS-LAAS * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of CNRS-LAAS nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #define BOOST_TEST_MODULE COAL_DISTANCE_LOWER_BOUND #include #include #include "coal/fwd.hh" #include "coal/data_types.h" #include "coal/BV/OBBRSS.h" #include "coal/BVH/BVH_model.h" #include "coal/narrowphase/narrowphase.h" #include "coal/collision.h" #include "coal/distance.h" #include "utility.h" #include "fcl_resources/config.h" using coal::BVHModel; using coal::CollisionGeometryPtr_t; using coal::CollisionObject; using coal::CollisionRequest; using coal::CollisionResult; using coal::DistanceRequest; using coal::DistanceResult; using coal::OBBRSS; using coal::Scalar; using coal::shared_ptr; using coal::Transform3s; using coal::Triangle32; using coal::Vec3s; bool testDistanceLowerBound(const Transform3s& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2, Scalar& distance) { Transform3s pose1(tf), pose2; CollisionRequest request; CollisionResult result; CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); coal::collide(&co1, &co2, request, result); distance = result.distance_lower_bound; return result.isCollision(); } bool testCollide(const Transform3s& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2) { Transform3s pose1(tf), pose2; CollisionRequest request(coal::NO_REQUEST, 1); request.enable_distance_lower_bound = false; CollisionResult result; CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); coal::collide(&co1, &co2, request, result); return result.isCollision(); } bool testDistance(const Transform3s& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2, Scalar& distance) { Transform3s pose1(tf), pose2; DistanceRequest request; DistanceResult result; CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); coal::distance(&co1, &co2, request, result); distance = result.min_distance; if (result.min_distance <= 0) { return true; } else { return false; } } BOOST_AUTO_TEST_CASE(mesh_mesh) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); shared_ptr > m1(new BVHModel); shared_ptr > m2(new BVHModel); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); m2->beginModel(); m2->addSubModel(p2, t2); m2->endModel(); std::vector transforms; Scalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 100; generateRandomTransforms(extents, transforms, n); // collision for (std::size_t i = 0; i < transforms.size(); ++i) { Scalar distanceLowerBound, distance; bool col1, col2, col3; col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound); col2 = testDistance(transforms[i], m1, m2, distance); col3 = testCollide(transforms[i], m1, m2); std::cout << "col1 = " << col1 << ", col2 = " << col2 << ", col3 = " << col3 << std::endl; std::cout << "distance lower bound = " << distanceLowerBound << std::endl; std::cout << "distance = " << distance << std::endl; BOOST_CHECK(col1 == col3); if (!col1) { BOOST_CHECK_MESSAGE(distanceLowerBound <= distance, "distance = " << distance << ", lower bound = " << distanceLowerBound); } } } BOOST_AUTO_TEST_CASE(box_sphere) { shared_ptr sphere(new coal::Sphere(0.5)); shared_ptr box(new coal::Box(1., 1., 1.)); Transform3s M1; M1.setIdentity(); Transform3s M2; M2.setIdentity(); std::vector transforms; Scalar extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; generateRandomTransforms(extents, transforms, n); Scalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(M1, sphere, box, distanceLowerBound); col2 = testDistance(M1, sphere, box, distance); BOOST_CHECK(col1 == col2); BOOST_CHECK(distanceLowerBound <= distance); for (std::size_t i = 0; i < transforms.size(); ++i) { Scalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], sphere, box, distanceLowerBound); col2 = testDistance(transforms[i], sphere, box, distance); BOOST_CHECK(col1 == col2); if (!col1) { BOOST_CHECK_MESSAGE(distanceLowerBound <= distance, "distance = " << distance << ", lower bound = " << distanceLowerBound); } } } BOOST_AUTO_TEST_CASE(sphere_sphere) { shared_ptr sphere1(new coal::Sphere(0.5)); shared_ptr sphere2(new coal::Sphere(1.)); Transform3s M1; M1.setIdentity(); Transform3s M2; M2.setIdentity(); std::vector transforms; Scalar extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; generateRandomTransforms(extents, transforms, n); Scalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(M1, sphere1, sphere2, distanceLowerBound); col2 = testDistance(M1, sphere1, sphere2, distance); BOOST_CHECK(col1 == col2); BOOST_CHECK(distanceLowerBound <= distance); for (std::size_t i = 0; i < transforms.size(); ++i) { Scalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], sphere1, sphere2, distanceLowerBound); col2 = testDistance(transforms[i], sphere1, sphere2, distance); BOOST_CHECK(col1 == col2); if (!col1) { BOOST_CHECK_MESSAGE(distanceLowerBound <= distance, "distance = " << distance << ", lower bound = " << distanceLowerBound); } } } BOOST_AUTO_TEST_CASE(box_mesh) { std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); shared_ptr > m1(new BVHModel); shared_ptr m2(new coal::Box(500, 200, 150)); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); std::vector transforms; Scalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 100; generateRandomTransforms(extents, transforms, n); // collision for (std::size_t i = 0; i < transforms.size(); ++i) { Scalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound); col2 = testDistance(transforms[i], m1, m2, distance); BOOST_CHECK(col1 == col2); if (!col1) { BOOST_CHECK_MESSAGE(distanceLowerBound <= distance, "distance = " << distance << ", lower bound = " << distanceLowerBound); } } } ================================================ FILE: test/fcl_resources/config.h.in ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2010-2015, Rice University. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Rice University nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* Author: Mark Moll */ #ifndef FCL_TEST_RESOURCES_CONFIG_ #define FCL_TEST_RESOURCES_CONFIG_ #define TEST_RESOURCES_DIR "@PROJECT_SOURCE_DIR@/test/fcl_resources" #endif ================================================ FILE: test/fcl_resources/env.obj ================================================ 6540 2180 v -2987.5 -3004.5 2987.5 v 987.5 -3004.5 2987.5 v -2987.5 -2995.5 2987.5 v -2987.5 -2995.5 2987.5 v 987.5 -3004.5 2987.5 v 987.5 -2995.5 2987.5 v 987.5 -3004.5 12.5 v -2987.5 -3004.5 12.5 v 987.5 -2995.5 12.5 v 987.5 -2995.5 12.5 v -2987.5 -3004.5 12.5 v -2987.5 -2995.5 12.5 v 987.5 -3004.5 2987.5 v 987.5 -3004.5 12.5 v 987.5 -2995.5 2987.5 v 987.5 -2995.5 2987.5 v 987.5 -3004.5 12.5 v 987.5 -2995.5 12.5 v -2987.5 -3004.5 12.5 v -2987.5 -3004.5 2987.5 v -2987.5 -2995.5 12.5 v -2987.5 -2995.5 12.5 v -2987.5 -3004.5 2987.5 v -2987.5 -2995.5 2987.5 v -2987.5 -2995.5 12.5 v -2987.5 -2995.5 2987.5 v 987.5 -2995.5 12.5 v 987.5 -2995.5 12.5 v -2987.5 -2995.5 2987.5 v 987.5 -2995.5 2987.5 v -2987.5 -3004.5 12.5 v 987.5 -3004.5 12.5 v -2987.5 -3004.5 2987.5 v -2987.5 -3004.5 2987.5 v 987.5 -3004.5 12.5 v 987.5 -3004.5 2987.5 v -2987.5 -2987.5 -5.5 v 2987.5 -2987.5 -5.5 v -2987.5 2987.5 -5.5 v -2987.5 2987.5 -5.5 v 2987.5 -2987.5 -5.5 v 2987.5 2987.5 -5.5 v 2987.5 -2987.5 -14.5 v -2987.5 -2987.5 -14.5 v 2987.5 2987.5 -14.5 v 2987.5 2987.5 -14.5 v -2987.5 -2987.5 -14.5 v -2987.5 2987.5 -14.5 v 2987.5 -2987.5 -5.5 v 2987.5 -2987.5 -14.5 v 2987.5 2987.5 -5.5 v 2987.5 2987.5 -5.5 v 2987.5 -2987.5 -14.5 v 2987.5 2987.5 -14.5 v -2987.5 -2987.5 -14.5 v -2987.5 -2987.5 -5.5 v -2987.5 2987.5 -14.5 v -2987.5 2987.5 -14.5 v -2987.5 -2987.5 -5.5 v -2987.5 2987.5 -5.5 v -2987.5 2987.5 -14.5 v -2987.5 2987.5 -5.5 v 2987.5 2987.5 -14.5 v 2987.5 2987.5 -14.5 v -2987.5 2987.5 -5.5 v 2987.5 2987.5 -5.5 v -2987.5 -2987.5 -14.5 v 2987.5 -2987.5 -14.5 v -2987.5 -2987.5 -5.5 v -2987.5 -2987.5 -5.5 v 2987.5 -2987.5 -14.5 v 2987.5 -2987.5 -5.5 v 995.5 -2987.5 987.5 v 1004.5 -2987.5 987.5 v 995.5 2987.5 987.5 v 995.5 2987.5 987.5 v 1004.5 -2987.5 987.5 v 1004.5 2987.5 987.5 v 1004.5 -2987.5 12.5 v 995.5 -2987.5 12.5 v 1004.5 2987.5 12.5 v 1004.5 2987.5 12.5 v 995.5 -2987.5 12.5 v 995.5 2987.5 12.5 v 1004.5 -2987.5 987.5 v 1004.5 -2987.5 12.5 v 1004.5 2987.5 987.5 v 1004.5 2987.5 987.5 v 1004.5 -2987.5 12.5 v 1004.5 2987.5 12.5 v 995.5 -2987.5 12.5 v 995.5 -2987.5 987.5 v 995.5 2987.5 12.5 v 995.5 2987.5 12.5 v 995.5 -2987.5 987.5 v 995.5 2987.5 987.5 v 995.5 2987.5 12.5 v 995.5 2987.5 987.5 v 1004.5 2987.5 12.5 v 1004.5 2987.5 12.5 v 995.5 2987.5 987.5 v 1004.5 2987.5 987.5 v 995.5 -2987.5 12.5 v 1004.5 -2987.5 12.5 v 995.5 -2987.5 987.5 v 995.5 -2987.5 987.5 v 1004.5 -2987.5 12.5 v 1004.5 -2987.5 987.5 v 995.5 -2987.5 2987.5 v 1004.5 -2987.5 2987.5 v 995.5 2987.5 2987.5 v 995.5 2987.5 2987.5 v 1004.5 -2987.5 2987.5 v 1004.5 2987.5 2987.5 v 1004.5 -2987.5 2012.5 v 995.5 -2987.5 2012.5 v 1004.5 2987.5 2012.5 v 1004.5 2987.5 2012.5 v 995.5 -2987.5 2012.5 v 995.5 2987.5 2012.5 v 1004.5 -2987.5 2987.5 v 1004.5 -2987.5 2012.5 v 1004.5 2987.5 2987.5 v 1004.5 2987.5 2987.5 v 1004.5 -2987.5 2012.5 v 1004.5 2987.5 2012.5 v 995.5 -2987.5 2012.5 v 995.5 -2987.5 2987.5 v 995.5 2987.5 2012.5 v 995.5 2987.5 2012.5 v 995.5 -2987.5 2987.5 v 995.5 2987.5 2987.5 v 995.5 2987.5 2012.5 v 995.5 2987.5 2987.5 v 1004.5 2987.5 2012.5 v 1004.5 2987.5 2012.5 v 995.5 2987.5 2987.5 v 1004.5 2987.5 2987.5 v 995.5 -2987.5 2012.5 v 1004.5 -2987.5 2012.5 v 995.5 -2987.5 2987.5 v 995.5 -2987.5 2987.5 v 1004.5 -2987.5 2012.5 v 1004.5 -2987.5 2987.5 v 995.5 -2987.5 1987.5 v 1004.5 -2987.5 1987.5 v 995.5 -1012.5 1987.5 v 995.5 -1012.5 1987.5 v 1004.5 -2987.5 1987.5 v 1004.5 -1012.5 1987.5 v 1004.5 -2987.5 1012.5 v 995.5 -2987.5 1012.5 v 1004.5 -1012.5 1012.5 v 1004.5 -1012.5 1012.5 v 995.5 -2987.5 1012.5 v 995.5 -1012.5 1012.5 v 1004.5 -2987.5 1987.5 v 1004.5 -2987.5 1012.5 v 1004.5 -1012.5 1987.5 v 1004.5 -1012.5 1987.5 v 1004.5 -2987.5 1012.5 v 1004.5 -1012.5 1012.5 v 995.5 -2987.5 1012.5 v 995.5 -2987.5 1987.5 v 995.5 -1012.5 1012.5 v 995.5 -1012.5 1012.5 v 995.5 -2987.5 1987.5 v 995.5 -1012.5 1987.5 v 995.5 -1012.5 1012.5 v 995.5 -1012.5 1987.5 v 1004.5 -1012.5 1012.5 v 1004.5 -1012.5 1012.5 v 995.5 -1012.5 1987.5 v 1004.5 -1012.5 1987.5 v 995.5 -2987.5 1012.5 v 1004.5 -2987.5 1012.5 v 995.5 -2987.5 1987.5 v 995.5 -2987.5 1987.5 v 1004.5 -2987.5 1012.5 v 1004.5 -2987.5 1987.5 v 995.5 1012.5 1987.5 v 1004.5 1012.5 1987.5 v 995.5 2987.5 1987.5 v 995.5 2987.5 1987.5 v 1004.5 1012.5 1987.5 v 1004.5 2987.5 1987.5 v 1004.5 1012.5 1012.5 v 995.5 1012.5 1012.5 v 1004.5 2987.5 1012.5 v 1004.5 2987.5 1012.5 v 995.5 1012.5 1012.5 v 995.5 2987.5 1012.5 v 1004.5 1012.5 1987.5 v 1004.5 1012.5 1012.5 v 1004.5 2987.5 1987.5 v 1004.5 2987.5 1987.5 v 1004.5 1012.5 1012.5 v 1004.5 2987.5 1012.5 v 995.5 1012.5 1012.5 v 995.5 1012.5 1987.5 v 995.5 2987.5 1012.5 v 995.5 2987.5 1012.5 v 995.5 1012.5 1987.5 v 995.5 2987.5 1987.5 v 995.5 2987.5 1012.5 v 995.5 2987.5 1987.5 v 1004.5 2987.5 1012.5 v 1004.5 2987.5 1012.5 v 995.5 2987.5 1987.5 v 1004.5 2987.5 1987.5 v 995.5 1012.5 1012.5 v 1004.5 1012.5 1012.5 v 995.5 1012.5 1987.5 v 995.5 1012.5 1987.5 v 1004.5 1012.5 1012.5 v 1004.5 1012.5 1987.5 v -2987.5 2995.5 2987.5 v 987.5 2995.5 2987.5 v -2987.5 3004.5 2987.5 v -2987.5 3004.5 2987.5 v 987.5 2995.5 2987.5 v 987.5 3004.5 2987.5 v 987.5 2995.5 12.5 v -2987.5 2995.5 12.5 v 987.5 3004.5 12.5 v 987.5 3004.5 12.5 v -2987.5 2995.5 12.5 v -2987.5 3004.5 12.5 v 987.5 2995.5 2987.5 v 987.5 2995.5 12.5 v 987.5 3004.5 2987.5 v 987.5 3004.5 2987.5 v 987.5 2995.5 12.5 v 987.5 3004.5 12.5 v -2987.5 2995.5 12.5 v -2987.5 2995.5 2987.5 v -2987.5 3004.5 12.5 v -2987.5 3004.5 12.5 v -2987.5 2995.5 2987.5 v -2987.5 3004.5 2987.5 v -2987.5 3004.5 12.5 v -2987.5 3004.5 2987.5 v 987.5 3004.5 12.5 v 987.5 3004.5 12.5 v -2987.5 3004.5 2987.5 v 987.5 3004.5 2987.5 v -2987.5 2995.5 12.5 v 987.5 2995.5 12.5 v -2987.5 2995.5 2987.5 v -2987.5 2995.5 2987.5 v 987.5 2995.5 12.5 v 987.5 2995.5 2987.5 v -3004.5 -2987.5 2987.5 v -2995.5 -2987.5 2987.5 v -3004.5 2987.5 2987.5 v -3004.5 2987.5 2987.5 v -2995.5 -2987.5 2987.5 v -2995.5 2987.5 2987.5 v -2995.5 -2987.5 12.5 v -3004.5 -2987.5 12.5 v -2995.5 2987.5 12.5 v -2995.5 2987.5 12.5 v -3004.5 -2987.5 12.5 v -3004.5 2987.5 12.5 v -2995.5 -2987.5 2987.5 v -2995.5 -2987.5 12.5 v -2995.5 2987.5 2987.5 v -2995.5 2987.5 2987.5 v -2995.5 -2987.5 12.5 v -2995.5 2987.5 12.5 v -3004.5 -2987.5 12.5 v -3004.5 -2987.5 2987.5 v -3004.5 2987.5 12.5 v -3004.5 2987.5 12.5 v -3004.5 -2987.5 2987.5 v -3004.5 2987.5 2987.5 v -3004.5 2987.5 12.5 v -3004.5 2987.5 2987.5 v -2995.5 2987.5 12.5 v -2995.5 2987.5 12.5 v -3004.5 2987.5 2987.5 v -2995.5 2987.5 2987.5 v -3004.5 -2987.5 12.5 v -2995.5 -2987.5 12.5 v -3004.5 -2987.5 2987.5 v -3004.5 -2987.5 2987.5 v -2995.5 -2987.5 12.5 v -2995.5 -2987.5 2987.5 v -1660 -2650 495 v -1659.24 -2653.83 495 v -1660 -2650 5 v -1660 -2650 5 v -1659.24 -2653.83 495 v -1659.24 -2653.83 5 v -1659.24 -2653.83 495 v -1657.07 -2657.07 495 v -1659.24 -2653.83 5 v -1659.24 -2653.83 5 v -1657.07 -2657.07 495 v -1657.07 -2657.07 5 v -1657.07 -2657.07 495 v -1653.83 -2659.24 495 v -1657.07 -2657.07 5 v -1657.07 -2657.07 5 v -1653.83 -2659.24 495 v -1653.83 -2659.24 5 v -1653.83 -2659.24 495 v -1650 -2660 495 v -1653.83 -2659.24 5 v -1653.83 -2659.24 5 v -1650 -2660 495 v -1650 -2660 5 v -1650 -2660 495 v -1646.17 -2659.24 495 v -1650 -2660 5 v -1650 -2660 5 v -1646.17 -2659.24 495 v -1646.17 -2659.24 5 v -1646.17 -2659.24 495 v -1642.93 -2657.07 495 v -1646.17 -2659.24 5 v -1646.17 -2659.24 5 v -1642.93 -2657.07 495 v -1642.93 -2657.07 5 v -1642.93 -2657.07 495 v -1640.76 -2653.83 495 v -1642.93 -2657.07 5 v -1642.93 -2657.07 5 v -1640.76 -2653.83 495 v -1640.76 -2653.83 5 v -1640.76 -2653.83 495 v -1640 -2650 495 v -1640.76 -2653.83 5 v -1640.76 -2653.83 5 v -1640 -2650 495 v -1640 -2650 5 v -1640 -2650 495 v -1640.76 -2646.17 495 v -1640 -2650 5 v -1640 -2650 5 v -1640.76 -2646.17 495 v -1640.76 -2646.17 5 v -1640.76 -2646.17 495 v -1642.93 -2642.93 495 v -1640.76 -2646.17 5 v -1640.76 -2646.17 5 v -1642.93 -2642.93 495 v -1642.93 -2642.93 5 v -1642.93 -2642.93 495 v -1646.17 -2640.76 495 v -1642.93 -2642.93 5 v -1642.93 -2642.93 5 v -1646.17 -2640.76 495 v -1646.17 -2640.76 5 v -1646.17 -2640.76 495 v -1650 -2640 495 v -1646.17 -2640.76 5 v -1646.17 -2640.76 5 v -1650 -2640 495 v -1650 -2640 5 v -1650 -2640 495 v -1653.83 -2640.76 495 v -1650 -2640 5 v -1650 -2640 5 v -1653.83 -2640.76 495 v -1653.83 -2640.76 5 v -1653.83 -2640.76 495 v -1657.07 -2642.93 495 v -1653.83 -2640.76 5 v -1653.83 -2640.76 5 v -1657.07 -2642.93 495 v -1657.07 -2642.93 5 v -1657.07 -2642.93 495 v -1659.24 -2646.17 495 v -1657.07 -2642.93 5 v -1657.07 -2642.93 5 v -1659.24 -2646.17 495 v -1659.24 -2646.17 5 v -1659.24 -2646.17 495 v -1660 -2650 495 v -1659.24 -2646.17 5 v -1659.24 -2646.17 5 v -1660 -2650 495 v -1660 -2650 5 v -1360 -2650 495 v -1359.24 -2653.83 495 v -1360 -2650 5 v -1360 -2650 5 v -1359.24 -2653.83 495 v -1359.24 -2653.83 5 v -1359.24 -2653.83 495 v -1357.07 -2657.07 495 v -1359.24 -2653.83 5 v -1359.24 -2653.83 5 v -1357.07 -2657.07 495 v -1357.07 -2657.07 5 v -1357.07 -2657.07 495 v -1353.83 -2659.24 495 v -1357.07 -2657.07 5 v -1357.07 -2657.07 5 v -1353.83 -2659.24 495 v -1353.83 -2659.24 5 v -1353.83 -2659.24 495 v -1350 -2660 495 v -1353.83 -2659.24 5 v -1353.83 -2659.24 5 v -1350 -2660 495 v -1350 -2660 5 v -1350 -2660 495 v -1346.17 -2659.24 495 v -1350 -2660 5 v -1350 -2660 5 v -1346.17 -2659.24 495 v -1346.17 -2659.24 5 v -1346.17 -2659.24 495 v -1342.93 -2657.07 495 v -1346.17 -2659.24 5 v -1346.17 -2659.24 5 v -1342.93 -2657.07 495 v -1342.93 -2657.07 5 v -1342.93 -2657.07 495 v -1340.76 -2653.83 495 v -1342.93 -2657.07 5 v -1342.93 -2657.07 5 v -1340.76 -2653.83 495 v -1340.76 -2653.83 5 v -1340.76 -2653.83 495 v -1340 -2650 495 v -1340.76 -2653.83 5 v -1340.76 -2653.83 5 v -1340 -2650 495 v -1340 -2650 5 v -1340 -2650 495 v -1340.76 -2646.17 495 v -1340 -2650 5 v -1340 -2650 5 v -1340.76 -2646.17 495 v -1340.76 -2646.17 5 v -1340.76 -2646.17 495 v -1342.93 -2642.93 495 v -1340.76 -2646.17 5 v -1340.76 -2646.17 5 v -1342.93 -2642.93 495 v -1342.93 -2642.93 5 v -1342.93 -2642.93 495 v -1346.17 -2640.76 495 v -1342.93 -2642.93 5 v -1342.93 -2642.93 5 v -1346.17 -2640.76 495 v -1346.17 -2640.76 5 v -1346.17 -2640.76 495 v -1350 -2640 495 v -1346.17 -2640.76 5 v -1346.17 -2640.76 5 v -1350 -2640 495 v -1350 -2640 5 v -1350 -2640 495 v -1353.83 -2640.76 495 v -1350 -2640 5 v -1350 -2640 5 v -1353.83 -2640.76 495 v -1353.83 -2640.76 5 v -1353.83 -2640.76 495 v -1357.07 -2642.93 495 v -1353.83 -2640.76 5 v -1353.83 -2640.76 5 v -1357.07 -2642.93 495 v -1357.07 -2642.93 5 v -1357.07 -2642.93 495 v -1359.24 -2646.17 495 v -1357.07 -2642.93 5 v -1357.07 -2642.93 5 v -1359.24 -2646.17 495 v -1359.24 -2646.17 5 v -1359.24 -2646.17 495 v -1360 -2650 495 v -1359.24 -2646.17 5 v -1359.24 -2646.17 5 v -1360 -2650 495 v -1360 -2650 5 v -1360 -2350 495 v -1359.24 -2353.83 495 v -1360 -2350 5 v -1360 -2350 5 v -1359.24 -2353.83 495 v -1359.24 -2353.83 5 v -1359.24 -2353.83 495 v -1357.07 -2357.07 495 v -1359.24 -2353.83 5 v -1359.24 -2353.83 5 v -1357.07 -2357.07 495 v -1357.07 -2357.07 5 v -1357.07 -2357.07 495 v -1353.83 -2359.24 495 v -1357.07 -2357.07 5 v -1357.07 -2357.07 5 v -1353.83 -2359.24 495 v -1353.83 -2359.24 5 v -1353.83 -2359.24 495 v -1350 -2360 495 v -1353.83 -2359.24 5 v -1353.83 -2359.24 5 v -1350 -2360 495 v -1350 -2360 5 v -1350 -2360 495 v -1346.17 -2359.24 495 v -1350 -2360 5 v -1350 -2360 5 v -1346.17 -2359.24 495 v -1346.17 -2359.24 5 v -1346.17 -2359.24 495 v -1342.93 -2357.07 495 v -1346.17 -2359.24 5 v -1346.17 -2359.24 5 v -1342.93 -2357.07 495 v -1342.93 -2357.07 5 v -1342.93 -2357.07 495 v -1340.76 -2353.83 495 v -1342.93 -2357.07 5 v -1342.93 -2357.07 5 v -1340.76 -2353.83 495 v -1340.76 -2353.83 5 v -1340.76 -2353.83 495 v -1340 -2350 495 v -1340.76 -2353.83 5 v -1340.76 -2353.83 5 v -1340 -2350 495 v -1340 -2350 5 v -1340 -2350 495 v -1340.76 -2346.17 495 v -1340 -2350 5 v -1340 -2350 5 v -1340.76 -2346.17 495 v -1340.76 -2346.17 5 v -1340.76 -2346.17 495 v -1342.93 -2342.93 495 v -1340.76 -2346.17 5 v -1340.76 -2346.17 5 v -1342.93 -2342.93 495 v -1342.93 -2342.93 5 v -1342.93 -2342.93 495 v -1346.17 -2340.76 495 v -1342.93 -2342.93 5 v -1342.93 -2342.93 5 v -1346.17 -2340.76 495 v -1346.17 -2340.76 5 v -1346.17 -2340.76 495 v -1350 -2340 495 v -1346.17 -2340.76 5 v -1346.17 -2340.76 5 v -1350 -2340 495 v -1350 -2340 5 v -1350 -2340 495 v -1353.83 -2340.76 495 v -1350 -2340 5 v -1350 -2340 5 v -1353.83 -2340.76 495 v -1353.83 -2340.76 5 v -1353.83 -2340.76 495 v -1357.07 -2342.93 495 v -1353.83 -2340.76 5 v -1353.83 -2340.76 5 v -1357.07 -2342.93 495 v -1357.07 -2342.93 5 v -1357.07 -2342.93 495 v -1359.24 -2346.17 495 v -1357.07 -2342.93 5 v -1357.07 -2342.93 5 v -1359.24 -2346.17 495 v -1359.24 -2346.17 5 v -1359.24 -2346.17 495 v -1360 -2350 495 v -1359.24 -2346.17 5 v -1359.24 -2346.17 5 v -1360 -2350 495 v -1360 -2350 5 v -1660 -2350 495 v -1659.24 -2353.83 495 v -1660 -2350 5 v -1660 -2350 5 v -1659.24 -2353.83 495 v -1659.24 -2353.83 5 v -1659.24 -2353.83 495 v -1657.07 -2357.07 495 v -1659.24 -2353.83 5 v -1659.24 -2353.83 5 v -1657.07 -2357.07 495 v -1657.07 -2357.07 5 v -1657.07 -2357.07 495 v -1653.83 -2359.24 495 v -1657.07 -2357.07 5 v -1657.07 -2357.07 5 v -1653.83 -2359.24 495 v -1653.83 -2359.24 5 v -1653.83 -2359.24 495 v -1650 -2360 495 v -1653.83 -2359.24 5 v -1653.83 -2359.24 5 v -1650 -2360 495 v -1650 -2360 5 v -1650 -2360 495 v -1646.17 -2359.24 495 v -1650 -2360 5 v -1650 -2360 5 v -1646.17 -2359.24 495 v -1646.17 -2359.24 5 v -1646.17 -2359.24 495 v -1642.93 -2357.07 495 v -1646.17 -2359.24 5 v -1646.17 -2359.24 5 v -1642.93 -2357.07 495 v -1642.93 -2357.07 5 v -1642.93 -2357.07 495 v -1640.76 -2353.83 495 v -1642.93 -2357.07 5 v -1642.93 -2357.07 5 v -1640.76 -2353.83 495 v -1640.76 -2353.83 5 v -1640.76 -2353.83 495 v -1640 -2350 495 v -1640.76 -2353.83 5 v -1640.76 -2353.83 5 v -1640 -2350 495 v -1640 -2350 5 v -1640 -2350 495 v -1640.76 -2346.17 495 v -1640 -2350 5 v -1640 -2350 5 v -1640.76 -2346.17 495 v -1640.76 -2346.17 5 v -1640.76 -2346.17 495 v -1642.93 -2342.93 495 v -1640.76 -2346.17 5 v -1640.76 -2346.17 5 v -1642.93 -2342.93 495 v -1642.93 -2342.93 5 v -1642.93 -2342.93 495 v -1646.17 -2340.76 495 v -1642.93 -2342.93 5 v -1642.93 -2342.93 5 v -1646.17 -2340.76 495 v -1646.17 -2340.76 5 v -1646.17 -2340.76 495 v -1650 -2340 495 v -1646.17 -2340.76 5 v -1646.17 -2340.76 5 v -1650 -2340 495 v -1650 -2340 5 v -1650 -2340 495 v -1653.83 -2340.76 495 v -1650 -2340 5 v -1650 -2340 5 v -1653.83 -2340.76 495 v -1653.83 -2340.76 5 v -1653.83 -2340.76 495 v -1657.07 -2342.93 495 v -1653.83 -2340.76 5 v -1653.83 -2340.76 5 v -1657.07 -2342.93 495 v -1657.07 -2342.93 5 v -1657.07 -2342.93 495 v -1659.24 -2346.17 495 v -1657.07 -2342.93 5 v -1657.07 -2342.93 5 v -1659.24 -2346.17 495 v -1659.24 -2346.17 5 v -1659.24 -2346.17 495 v -1660 -2350 495 v -1659.24 -2346.17 5 v -1659.24 -2346.17 5 v -1660 -2350 495 v -1660 -2350 5 v -1300 -2300 510 v -1700 -2300 510 v -1300 -2700 510 v -1300 -2700 510 v -1700 -2300 510 v -1700 -2700 510 v -1700 -2300 500 v -1300 -2300 500 v -1700 -2700 500 v -1700 -2700 500 v -1300 -2300 500 v -1300 -2700 500 v -1700 -2300 510 v -1700 -2300 500 v -1700 -2700 510 v -1700 -2700 510 v -1700 -2300 500 v -1700 -2700 500 v -1300 -2300 500 v -1300 -2300 510 v -1300 -2700 500 v -1300 -2700 500 v -1300 -2300 510 v -1300 -2700 510 v -1300 -2700 500 v -1300 -2700 510 v -1700 -2700 500 v -1700 -2700 500 v -1300 -2700 510 v -1700 -2700 510 v -1300 -2300 500 v -1700 -2300 500 v -1300 -2300 510 v -1300 -2300 510 v -1700 -2300 500 v -1700 -2300 510 v -1560 -2705 810 v -1559.24 -2708.83 810 v -1560 -2705 510 v -1560 -2705 510 v -1559.24 -2708.83 810 v -1559.24 -2708.83 510 v -1559.24 -2708.83 810 v -1557.07 -2712.07 810 v -1559.24 -2708.83 510 v -1559.24 -2708.83 510 v -1557.07 -2712.07 810 v -1557.07 -2712.07 510 v -1557.07 -2712.07 810 v -1553.83 -2714.24 810 v -1557.07 -2712.07 510 v -1557.07 -2712.07 510 v -1553.83 -2714.24 810 v -1553.83 -2714.24 510 v -1553.83 -2714.24 810 v -1550 -2715 810 v -1553.83 -2714.24 510 v -1553.83 -2714.24 510 v -1550 -2715 810 v -1550 -2715 510 v -1550 -2715 810 v -1546.17 -2714.24 810 v -1550 -2715 510 v -1550 -2715 510 v -1546.17 -2714.24 810 v -1546.17 -2714.24 510 v -1546.17 -2714.24 810 v -1542.93 -2712.07 810 v -1546.17 -2714.24 510 v -1546.17 -2714.24 510 v -1542.93 -2712.07 810 v -1542.93 -2712.07 510 v -1542.93 -2712.07 810 v -1540.76 -2708.83 810 v -1542.93 -2712.07 510 v -1542.93 -2712.07 510 v -1540.76 -2708.83 810 v -1540.76 -2708.83 510 v -1540.76 -2708.83 810 v -1540 -2705 810 v -1540.76 -2708.83 510 v -1540.76 -2708.83 510 v -1540 -2705 810 v -1540 -2705 510 v -1540 -2705 810 v -1540.76 -2701.17 810 v -1540 -2705 510 v -1540 -2705 510 v -1540.76 -2701.17 810 v -1540.76 -2701.17 510 v -1540.76 -2701.17 810 v -1542.93 -2697.93 810 v -1540.76 -2701.17 510 v -1540.76 -2701.17 510 v -1542.93 -2697.93 810 v -1542.93 -2697.93 510 v -1542.93 -2697.93 810 v -1546.17 -2695.76 810 v -1542.93 -2697.93 510 v -1542.93 -2697.93 510 v -1546.17 -2695.76 810 v -1546.17 -2695.76 510 v -1546.17 -2695.76 810 v -1550 -2695 810 v -1546.17 -2695.76 510 v -1546.17 -2695.76 510 v -1550 -2695 810 v -1550 -2695 510 v -1550 -2695 810 v -1553.83 -2695.76 810 v -1550 -2695 510 v -1550 -2695 510 v -1553.83 -2695.76 810 v -1553.83 -2695.76 510 v -1553.83 -2695.76 810 v -1557.07 -2697.93 810 v -1553.83 -2695.76 510 v -1553.83 -2695.76 510 v -1557.07 -2697.93 810 v -1557.07 -2697.93 510 v -1557.07 -2697.93 810 v -1559.24 -2701.17 810 v -1557.07 -2697.93 510 v -1557.07 -2697.93 510 v -1559.24 -2701.17 810 v -1559.24 -2701.17 510 v -1559.24 -2701.17 810 v -1560 -2705 810 v -1559.24 -2701.17 510 v -1559.24 -2701.17 510 v -1560 -2705 810 v -1560 -2705 510 v -1460 -2705 810 v -1459.24 -2708.83 810 v -1460 -2705 510 v -1460 -2705 510 v -1459.24 -2708.83 810 v -1459.24 -2708.83 510 v -1459.24 -2708.83 810 v -1457.07 -2712.07 810 v -1459.24 -2708.83 510 v -1459.24 -2708.83 510 v -1457.07 -2712.07 810 v -1457.07 -2712.07 510 v -1457.07 -2712.07 810 v -1453.83 -2714.24 810 v -1457.07 -2712.07 510 v -1457.07 -2712.07 510 v -1453.83 -2714.24 810 v -1453.83 -2714.24 510 v -1453.83 -2714.24 810 v -1450 -2715 810 v -1453.83 -2714.24 510 v -1453.83 -2714.24 510 v -1450 -2715 810 v -1450 -2715 510 v -1450 -2715 810 v -1446.17 -2714.24 810 v -1450 -2715 510 v -1450 -2715 510 v -1446.17 -2714.24 810 v -1446.17 -2714.24 510 v -1446.17 -2714.24 810 v -1442.93 -2712.07 810 v -1446.17 -2714.24 510 v -1446.17 -2714.24 510 v -1442.93 -2712.07 810 v -1442.93 -2712.07 510 v -1442.93 -2712.07 810 v -1440.76 -2708.83 810 v -1442.93 -2712.07 510 v -1442.93 -2712.07 510 v -1440.76 -2708.83 810 v -1440.76 -2708.83 510 v -1440.76 -2708.83 810 v -1440 -2705 810 v -1440.76 -2708.83 510 v -1440.76 -2708.83 510 v -1440 -2705 810 v -1440 -2705 510 v -1440 -2705 810 v -1440.76 -2701.17 810 v -1440 -2705 510 v -1440 -2705 510 v -1440.76 -2701.17 810 v -1440.76 -2701.17 510 v -1440.76 -2701.17 810 v -1442.93 -2697.93 810 v -1440.76 -2701.17 510 v -1440.76 -2701.17 510 v -1442.93 -2697.93 810 v -1442.93 -2697.93 510 v -1442.93 -2697.93 810 v -1446.17 -2695.76 810 v -1442.93 -2697.93 510 v -1442.93 -2697.93 510 v -1446.17 -2695.76 810 v -1446.17 -2695.76 510 v -1446.17 -2695.76 810 v -1450 -2695 810 v -1446.17 -2695.76 510 v -1446.17 -2695.76 510 v -1450 -2695 810 v -1450 -2695 510 v -1450 -2695 810 v -1453.83 -2695.76 810 v -1450 -2695 510 v -1450 -2695 510 v -1453.83 -2695.76 810 v -1453.83 -2695.76 510 v -1453.83 -2695.76 810 v -1457.07 -2697.93 810 v -1453.83 -2695.76 510 v -1453.83 -2695.76 510 v -1457.07 -2697.93 810 v -1457.07 -2697.93 510 v -1457.07 -2697.93 810 v -1459.24 -2701.17 810 v -1457.07 -2697.93 510 v -1457.07 -2697.93 510 v -1459.24 -2701.17 810 v -1459.24 -2701.17 510 v -1459.24 -2701.17 810 v -1460 -2705 810 v -1459.24 -2701.17 510 v -1459.24 -2701.17 510 v -1460 -2705 810 v -1460 -2705 510 v -1300 -2690 910 v -1700 -2690 910 v -1300 -2700 910 v -1300 -2700 910 v -1700 -2690 910 v -1700 -2700 910 v -1700 -2690 710 v -1300 -2690 710 v -1700 -2700 710 v -1700 -2700 710 v -1300 -2690 710 v -1300 -2700 710 v -1700 -2690 910 v -1700 -2690 710 v -1700 -2700 910 v -1700 -2700 910 v -1700 -2690 710 v -1700 -2700 710 v -1300 -2690 710 v -1300 -2690 910 v -1300 -2700 710 v -1300 -2700 710 v -1300 -2690 910 v -1300 -2700 910 v -1300 -2700 710 v -1300 -2700 910 v -1700 -2700 710 v -1700 -2700 710 v -1300 -2700 910 v -1700 -2700 910 v -1300 -2690 710 v -1700 -2690 710 v -1300 -2690 910 v -1300 -2690 910 v -1700 -2690 710 v -1700 -2690 910 v -798.346 -313.769 495 v -797.753 -309.913 495 v -798.346 -313.769 5 v -798.346 -313.769 5 v -797.753 -309.913 495 v -797.753 -309.913 5 v -797.753 -309.913 495 v -798.68 -306.123 495 v -797.753 -309.913 5 v -797.753 -309.913 5 v -798.68 -306.123 495 v -798.68 -306.123 5 v -798.68 -306.123 495 v -800.987 -302.976 495 v -798.68 -306.123 5 v -798.68 -306.123 5 v -800.987 -302.976 495 v -800.987 -302.976 5 v -800.987 -302.976 495 v -804.323 -300.952 495 v -800.987 -302.976 5 v -800.987 -302.976 5 v -804.323 -300.952 495 v -804.323 -300.952 5 v -804.323 -300.952 495 v -808.179 -300.359 495 v -804.323 -300.952 5 v -804.323 -300.952 5 v -808.179 -300.359 495 v -808.179 -300.359 5 v -808.179 -300.359 495 v -811.969 -301.286 495 v -808.179 -300.359 5 v -808.179 -300.359 5 v -811.969 -301.286 495 v -811.969 -301.286 5 v -811.969 -301.286 495 v -815.116 -303.593 495 v -811.969 -301.286 5 v -811.969 -301.286 5 v -815.116 -303.593 495 v -815.116 -303.593 5 v -815.116 -303.593 495 v -817.14 -306.929 495 v -815.116 -303.593 5 v -815.116 -303.593 5 v -817.14 -306.929 495 v -817.14 -306.929 5 v -817.14 -306.929 495 v -817.734 -310.785 495 v -817.14 -306.929 5 v -817.14 -306.929 5 v -817.734 -310.785 495 v -817.734 -310.785 5 v -817.734 -310.785 495 v -816.806 -314.575 495 v -817.734 -310.785 5 v -817.734 -310.785 5 v -816.806 -314.575 495 v -816.806 -314.575 5 v -816.806 -314.575 495 v -814.499 -317.722 495 v -816.806 -314.575 5 v -816.806 -314.575 5 v -814.499 -317.722 495 v -814.499 -317.722 5 v -814.499 -317.722 495 v -811.163 -319.746 495 v -814.499 -317.722 5 v -814.499 -317.722 5 v -811.163 -319.746 495 v -811.163 -319.746 5 v -811.163 -319.746 495 v -807.307 -320.34 495 v -811.163 -319.746 5 v -811.163 -319.746 5 v -807.307 -320.34 495 v -807.307 -320.34 5 v -807.307 -320.34 495 v -803.517 -319.412 495 v -807.307 -320.34 5 v -807.307 -320.34 5 v -803.517 -319.412 495 v -803.517 -319.412 5 v -803.517 -319.412 495 v -800.37 -317.105 495 v -803.517 -319.412 5 v -803.517 -319.412 5 v -800.37 -317.105 495 v -800.37 -317.105 5 v -800.37 -317.105 495 v -798.346 -313.769 495 v -800.37 -317.105 5 v -800.37 -317.105 5 v -798.346 -313.769 495 v -798.346 -313.769 5 v -1080.25 -211.163 495 v -1079.66 -207.307 495 v -1080.25 -211.163 5 v -1080.25 -211.163 5 v -1079.66 -207.307 495 v -1079.66 -207.307 5 v -1079.66 -207.307 495 v -1080.59 -203.517 495 v -1079.66 -207.307 5 v -1079.66 -207.307 5 v -1080.59 -203.517 495 v -1080.59 -203.517 5 v -1080.59 -203.517 495 v -1082.9 -200.37 495 v -1080.59 -203.517 5 v -1080.59 -203.517 5 v -1082.9 -200.37 495 v -1082.9 -200.37 5 v -1082.9 -200.37 495 v -1086.23 -198.346 495 v -1082.9 -200.37 5 v -1082.9 -200.37 5 v -1086.23 -198.346 495 v -1086.23 -198.346 5 v -1086.23 -198.346 495 v -1090.09 -197.753 495 v -1086.23 -198.346 5 v -1086.23 -198.346 5 v -1090.09 -197.753 495 v -1090.09 -197.753 5 v -1090.09 -197.753 495 v -1093.88 -198.68 495 v -1090.09 -197.753 5 v -1090.09 -197.753 5 v -1093.88 -198.68 495 v -1093.88 -198.68 5 v -1093.88 -198.68 495 v -1097.02 -200.987 495 v -1093.88 -198.68 5 v -1093.88 -198.68 5 v -1097.02 -200.987 495 v -1097.02 -200.987 5 v -1097.02 -200.987 495 v -1099.05 -204.323 495 v -1097.02 -200.987 5 v -1097.02 -200.987 5 v -1099.05 -204.323 495 v -1099.05 -204.323 5 v -1099.05 -204.323 495 v -1099.64 -208.179 495 v -1099.05 -204.323 5 v -1099.05 -204.323 5 v -1099.64 -208.179 495 v -1099.64 -208.179 5 v -1099.64 -208.179 495 v -1098.71 -211.969 495 v -1099.64 -208.179 5 v -1099.64 -208.179 5 v -1098.71 -211.969 495 v -1098.71 -211.969 5 v -1098.71 -211.969 495 v -1096.41 -215.116 495 v -1098.71 -211.969 5 v -1098.71 -211.969 5 v -1096.41 -215.116 495 v -1096.41 -215.116 5 v -1096.41 -215.116 495 v -1093.07 -217.14 495 v -1096.41 -215.116 5 v -1096.41 -215.116 5 v -1093.07 -217.14 495 v -1093.07 -217.14 5 v -1093.07 -217.14 495 v -1089.21 -217.734 495 v -1093.07 -217.14 5 v -1093.07 -217.14 5 v -1089.21 -217.734 495 v -1089.21 -217.734 5 v -1089.21 -217.734 495 v -1085.42 -216.806 495 v -1089.21 -217.734 5 v -1089.21 -217.734 5 v -1085.42 -216.806 495 v -1085.42 -216.806 5 v -1085.42 -216.806 495 v -1082.28 -214.499 495 v -1085.42 -216.806 5 v -1085.42 -216.806 5 v -1082.28 -214.499 495 v -1082.28 -214.499 5 v -1082.28 -214.499 495 v -1080.25 -211.163 495 v -1082.28 -214.499 5 v -1082.28 -214.499 5 v -1080.25 -211.163 495 v -1080.25 -211.163 5 v -1182.86 -493.071 495 v -1182.27 -489.215 495 v -1182.86 -493.071 5 v -1182.86 -493.071 5 v -1182.27 -489.215 495 v -1182.27 -489.215 5 v -1182.27 -489.215 495 v -1183.19 -485.425 495 v -1182.27 -489.215 5 v -1182.27 -489.215 5 v -1183.19 -485.425 495 v -1183.19 -485.425 5 v -1183.19 -485.425 495 v -1185.5 -482.278 495 v -1183.19 -485.425 5 v -1183.19 -485.425 5 v -1185.5 -482.278 495 v -1185.5 -482.278 5 v -1185.5 -482.278 495 v -1188.84 -480.254 495 v -1185.5 -482.278 5 v -1185.5 -482.278 5 v -1188.84 -480.254 495 v -1188.84 -480.254 5 v -1188.84 -480.254 495 v -1192.69 -479.66 495 v -1188.84 -480.254 5 v -1188.84 -480.254 5 v -1192.69 -479.66 495 v -1192.69 -479.66 5 v -1192.69 -479.66 495 v -1196.48 -480.588 495 v -1192.69 -479.66 5 v -1192.69 -479.66 5 v -1196.48 -480.588 495 v -1196.48 -480.588 5 v -1196.48 -480.588 495 v -1199.63 -482.895 495 v -1196.48 -480.588 5 v -1196.48 -480.588 5 v -1199.63 -482.895 495 v -1199.63 -482.895 5 v -1199.63 -482.895 495 v -1201.65 -486.231 495 v -1199.63 -482.895 5 v -1199.63 -482.895 5 v -1201.65 -486.231 495 v -1201.65 -486.231 5 v -1201.65 -486.231 495 v -1202.25 -490.087 495 v -1201.65 -486.231 5 v -1201.65 -486.231 5 v -1202.25 -490.087 495 v -1202.25 -490.087 5 v -1202.25 -490.087 495 v -1201.32 -493.877 495 v -1202.25 -490.087 5 v -1202.25 -490.087 5 v -1201.32 -493.877 495 v -1201.32 -493.877 5 v -1201.32 -493.877 495 v -1199.01 -497.024 495 v -1201.32 -493.877 5 v -1201.32 -493.877 5 v -1199.01 -497.024 495 v -1199.01 -497.024 5 v -1199.01 -497.024 495 v -1195.68 -499.048 495 v -1199.01 -497.024 5 v -1199.01 -497.024 5 v -1195.68 -499.048 495 v -1195.68 -499.048 5 v -1195.68 -499.048 495 v -1191.82 -499.641 495 v -1195.68 -499.048 5 v -1195.68 -499.048 5 v -1191.82 -499.641 495 v -1191.82 -499.641 5 v -1191.82 -499.641 495 v -1188.03 -498.714 495 v -1191.82 -499.641 5 v -1191.82 -499.641 5 v -1188.03 -498.714 495 v -1188.03 -498.714 5 v -1188.03 -498.714 495 v -1184.88 -496.407 495 v -1188.03 -498.714 5 v -1188.03 -498.714 5 v -1184.88 -496.407 495 v -1184.88 -496.407 5 v -1184.88 -496.407 495 v -1182.86 -493.071 495 v -1184.88 -496.407 5 v -1184.88 -496.407 5 v -1182.86 -493.071 495 v -1182.86 -493.071 5 v -900.952 -595.677 495 v -900.359 -591.821 495 v -900.952 -595.677 5 v -900.952 -595.677 5 v -900.359 -591.821 495 v -900.359 -591.821 5 v -900.359 -591.821 495 v -901.286 -588.031 495 v -900.359 -591.821 5 v -900.359 -591.821 5 v -901.286 -588.031 495 v -901.286 -588.031 5 v -901.286 -588.031 495 v -903.593 -584.884 495 v -901.286 -588.031 5 v -901.286 -588.031 5 v -903.593 -584.884 495 v -903.593 -584.884 5 v -903.593 -584.884 495 v -906.929 -582.86 495 v -903.593 -584.884 5 v -903.593 -584.884 5 v -906.929 -582.86 495 v -906.929 -582.86 5 v -906.929 -582.86 495 v -910.785 -582.266 495 v -906.929 -582.86 5 v -906.929 -582.86 5 v -910.785 -582.266 495 v -910.785 -582.266 5 v -910.785 -582.266 495 v -914.575 -583.194 495 v -910.785 -582.266 5 v -910.785 -582.266 5 v -914.575 -583.194 495 v -914.575 -583.194 5 v -914.575 -583.194 495 v -917.722 -585.501 495 v -914.575 -583.194 5 v -914.575 -583.194 5 v -917.722 -585.501 495 v -917.722 -585.501 5 v -917.722 -585.501 495 v -919.746 -588.837 495 v -917.722 -585.501 5 v -917.722 -585.501 5 v -919.746 -588.837 495 v -919.746 -588.837 5 v -919.746 -588.837 495 v -920.34 -592.693 495 v -919.746 -588.837 5 v -919.746 -588.837 5 v -920.34 -592.693 495 v -920.34 -592.693 5 v -920.34 -592.693 495 v -919.412 -596.483 495 v -920.34 -592.693 5 v -920.34 -592.693 5 v -919.412 -596.483 495 v -919.412 -596.483 5 v -919.412 -596.483 495 v -917.105 -599.63 495 v -919.412 -596.483 5 v -919.412 -596.483 5 v -917.105 -599.63 495 v -917.105 -599.63 5 v -917.105 -599.63 495 v -913.769 -601.654 495 v -917.105 -599.63 5 v -917.105 -599.63 5 v -913.769 -601.654 495 v -913.769 -601.654 5 v -913.769 -601.654 495 v -909.913 -602.247 495 v -913.769 -601.654 5 v -913.769 -601.654 5 v -909.913 -602.247 495 v -909.913 -602.247 5 v -909.913 -602.247 495 v -906.123 -601.32 495 v -909.913 -602.247 5 v -909.913 -602.247 5 v -906.123 -601.32 495 v -906.123 -601.32 5 v -906.123 -601.32 495 v -902.976 -599.013 495 v -906.123 -601.32 5 v -906.123 -601.32 5 v -902.976 -599.013 495 v -902.976 -599.013 5 v -902.976 -599.013 495 v -900.952 -595.677 495 v -902.976 -599.013 5 v -902.976 -599.013 5 v -900.952 -595.677 495 v -900.952 -595.677 5 v -1256.34 -519.534 510 v -880.466 -656.343 510 v -1119.53 -143.657 510 v -1119.53 -143.657 510 v -880.466 -656.343 510 v -743.657 -280.466 510 v -880.466 -656.343 500 v -1256.34 -519.534 500 v -743.657 -280.466 500 v -743.657 -280.466 500 v -1256.34 -519.534 500 v -1119.53 -143.657 500 v -880.466 -656.343 510 v -880.466 -656.343 500 v -743.657 -280.466 510 v -743.657 -280.466 510 v -880.466 -656.343 500 v -743.657 -280.466 500 v -1256.34 -519.534 500 v -1256.34 -519.534 510 v -1119.53 -143.657 500 v -1119.53 -143.657 500 v -1256.34 -519.534 510 v -1119.53 -143.657 510 v -1119.53 -143.657 500 v -1119.53 -143.657 510 v -743.657 -280.466 500 v -743.657 -280.466 500 v -1119.53 -143.657 510 v -743.657 -280.466 510 v -1256.34 -519.534 500 v -880.466 -656.343 500 v -1256.34 -519.534 510 v -1256.34 -519.534 510 v -880.466 -656.343 500 v -880.466 -656.343 510 v -873.504 -227.884 810 v -872.911 -224.028 810 v -873.504 -227.884 510 v -873.504 -227.884 510 v -872.911 -224.028 810 v -872.911 -224.028 510 v -872.911 -224.028 810 v -873.838 -220.238 810 v -872.911 -224.028 510 v -872.911 -224.028 510 v -873.838 -220.238 810 v -873.838 -220.238 510 v -873.838 -220.238 810 v -876.145 -217.091 810 v -873.838 -220.238 510 v -873.838 -220.238 510 v -876.145 -217.091 810 v -876.145 -217.091 510 v -876.145 -217.091 810 v -879.481 -215.067 810 v -876.145 -217.091 510 v -876.145 -217.091 510 v -879.481 -215.067 810 v -879.481 -215.067 510 v -879.481 -215.067 810 v -883.337 -214.474 810 v -879.481 -215.067 510 v -879.481 -215.067 510 v -883.337 -214.474 810 v -883.337 -214.474 510 v -883.337 -214.474 810 v -887.127 -215.401 810 v -883.337 -214.474 510 v -883.337 -214.474 510 v -887.127 -215.401 810 v -887.127 -215.401 510 v -887.127 -215.401 810 v -890.274 -217.708 810 v -887.127 -215.401 510 v -887.127 -215.401 510 v -890.274 -217.708 810 v -890.274 -217.708 510 v -890.274 -217.708 810 v -892.298 -221.044 810 v -890.274 -217.708 510 v -890.274 -217.708 510 v -892.298 -221.044 810 v -892.298 -221.044 510 v -892.298 -221.044 810 v -892.892 -224.9 810 v -892.298 -221.044 510 v -892.298 -221.044 510 v -892.892 -224.9 810 v -892.892 -224.9 510 v -892.892 -224.9 810 v -891.964 -228.69 810 v -892.892 -224.9 510 v -892.892 -224.9 510 v -891.964 -228.69 810 v -891.964 -228.69 510 v -891.964 -228.69 810 v -889.657 -231.837 810 v -891.964 -228.69 510 v -891.964 -228.69 510 v -889.657 -231.837 810 v -889.657 -231.837 510 v -889.657 -231.837 810 v -886.321 -233.861 810 v -889.657 -231.837 510 v -889.657 -231.837 510 v -886.321 -233.861 810 v -886.321 -233.861 510 v -886.321 -233.861 810 v -882.465 -234.454 810 v -886.321 -233.861 510 v -886.321 -233.861 510 v -882.465 -234.454 810 v -882.465 -234.454 510 v -882.465 -234.454 810 v -878.675 -233.527 810 v -882.465 -234.454 510 v -882.465 -234.454 510 v -878.675 -233.527 810 v -878.675 -233.527 510 v -878.675 -233.527 810 v -875.528 -231.22 810 v -878.675 -233.527 510 v -878.675 -233.527 510 v -875.528 -231.22 810 v -875.528 -231.22 510 v -875.528 -231.22 810 v -873.504 -227.884 810 v -875.528 -231.22 510 v -875.528 -231.22 510 v -873.504 -227.884 810 v -873.504 -227.884 510 v -967.474 -193.682 810 v -966.88 -189.826 810 v -967.474 -193.682 510 v -967.474 -193.682 510 v -966.88 -189.826 810 v -966.88 -189.826 510 v -966.88 -189.826 810 v -967.807 -186.036 810 v -966.88 -189.826 510 v -966.88 -189.826 510 v -967.807 -186.036 810 v -967.807 -186.036 510 v -967.807 -186.036 810 v -970.115 -182.889 810 v -967.807 -186.036 510 v -967.807 -186.036 510 v -970.115 -182.889 810 v -970.115 -182.889 510 v -970.115 -182.889 810 v -973.45 -180.865 810 v -970.115 -182.889 510 v -970.115 -182.889 510 v -973.45 -180.865 810 v -973.45 -180.865 510 v -973.45 -180.865 810 v -977.307 -180.272 810 v -973.45 -180.865 510 v -973.45 -180.865 510 v -977.307 -180.272 810 v -977.307 -180.272 510 v -977.307 -180.272 810 v -981.097 -181.199 810 v -977.307 -180.272 510 v -977.307 -180.272 510 v -981.097 -181.199 810 v -981.097 -181.199 510 v -981.097 -181.199 810 v -984.243 -183.506 810 v -981.097 -181.199 510 v -981.097 -181.199 510 v -984.243 -183.506 810 v -984.243 -183.506 510 v -984.243 -183.506 810 v -986.267 -186.842 810 v -984.243 -183.506 510 v -984.243 -183.506 510 v -986.267 -186.842 810 v -986.267 -186.842 510 v -986.267 -186.842 810 v -986.861 -190.698 810 v -986.267 -186.842 510 v -986.267 -186.842 510 v -986.861 -190.698 810 v -986.861 -190.698 510 v -986.861 -190.698 810 v -985.934 -194.488 810 v -986.861 -190.698 510 v -986.861 -190.698 510 v -985.934 -194.488 810 v -985.934 -194.488 510 v -985.934 -194.488 810 v -983.626 -197.635 810 v -985.934 -194.488 510 v -985.934 -194.488 510 v -983.626 -197.635 810 v -983.626 -197.635 510 v -983.626 -197.635 810 v -980.291 -199.659 810 v -983.626 -197.635 510 v -983.626 -197.635 510 v -980.291 -199.659 810 v -980.291 -199.659 510 v -980.291 -199.659 810 v -976.434 -200.252 810 v -980.291 -199.659 510 v -980.291 -199.659 510 v -976.434 -200.252 810 v -976.434 -200.252 510 v -976.434 -200.252 810 v -972.644 -199.325 810 v -976.434 -200.252 510 v -976.434 -200.252 510 v -972.644 -199.325 810 v -972.644 -199.325 510 v -972.644 -199.325 810 v -969.498 -197.018 810 v -972.644 -199.325 510 v -972.644 -199.325 510 v -969.498 -197.018 810 v -969.498 -197.018 510 v -969.498 -197.018 810 v -967.474 -193.682 810 v -969.498 -197.018 510 v -969.498 -197.018 510 v -967.474 -193.682 810 v -967.474 -193.682 510 v -1122.95 -153.054 910 v -747.078 -289.862 910 v -1119.53 -143.657 910 v -1119.53 -143.657 910 v -747.078 -289.862 910 v -743.657 -280.465 910 v -747.078 -289.862 710 v -1122.95 -153.054 710 v -743.657 -280.465 710 v -743.657 -280.465 710 v -1122.95 -153.054 710 v -1119.53 -143.657 710 v -747.078 -289.862 910 v -747.078 -289.862 710 v -743.657 -280.465 910 v -743.657 -280.465 910 v -747.078 -289.862 710 v -743.657 -280.465 710 v -1122.95 -153.054 710 v -1122.95 -153.054 910 v -1119.53 -143.657 710 v -1119.53 -143.657 710 v -1122.95 -153.054 910 v -1119.53 -143.657 910 v -1119.53 -143.657 710 v -1119.53 -143.657 910 v -743.657 -280.465 710 v -743.657 -280.465 710 v -1119.53 -143.657 910 v -743.657 -280.465 910 v -1122.95 -153.054 710 v -747.078 -289.862 710 v -1122.95 -153.054 910 v -1122.95 -153.054 910 v -747.078 -289.862 710 v -747.078 -289.862 910 v -1900.95 -1604.32 495 v -1902.98 -1600.99 495 v -1900.95 -1604.32 5 v -1900.95 -1604.32 5 v -1902.98 -1600.99 495 v -1902.98 -1600.99 5 v -1902.98 -1600.99 495 v -1906.12 -1598.68 495 v -1902.98 -1600.99 5 v -1902.98 -1600.99 5 v -1906.12 -1598.68 495 v -1906.12 -1598.68 5 v -1906.12 -1598.68 495 v -1909.91 -1597.75 495 v -1906.12 -1598.68 5 v -1906.12 -1598.68 5 v -1909.91 -1597.75 495 v -1909.91 -1597.75 5 v -1909.91 -1597.75 495 v -1913.77 -1598.35 495 v -1909.91 -1597.75 5 v -1909.91 -1597.75 5 v -1913.77 -1598.35 495 v -1913.77 -1598.35 5 v -1913.77 -1598.35 495 v -1917.1 -1600.37 495 v -1913.77 -1598.35 5 v -1913.77 -1598.35 5 v -1917.1 -1600.37 495 v -1917.1 -1600.37 5 v -1917.1 -1600.37 495 v -1919.41 -1603.52 495 v -1917.1 -1600.37 5 v -1917.1 -1600.37 5 v -1919.41 -1603.52 495 v -1919.41 -1603.52 5 v -1919.41 -1603.52 495 v -1920.34 -1607.31 495 v -1919.41 -1603.52 5 v -1919.41 -1603.52 5 v -1920.34 -1607.31 495 v -1920.34 -1607.31 5 v -1920.34 -1607.31 495 v -1919.75 -1611.16 495 v -1920.34 -1607.31 5 v -1920.34 -1607.31 5 v -1919.75 -1611.16 495 v -1919.75 -1611.16 5 v -1919.75 -1611.16 495 v -1917.72 -1614.5 495 v -1919.75 -1611.16 5 v -1919.75 -1611.16 5 v -1917.72 -1614.5 495 v -1917.72 -1614.5 5 v -1917.72 -1614.5 495 v -1914.58 -1616.81 495 v -1917.72 -1614.5 5 v -1917.72 -1614.5 5 v -1914.58 -1616.81 495 v -1914.58 -1616.81 5 v -1914.58 -1616.81 495 v -1910.79 -1617.73 495 v -1914.58 -1616.81 5 v -1914.58 -1616.81 5 v -1910.79 -1617.73 495 v -1910.79 -1617.73 5 v -1910.79 -1617.73 495 v -1906.93 -1617.14 495 v -1910.79 -1617.73 5 v -1910.79 -1617.73 5 v -1906.93 -1617.14 495 v -1906.93 -1617.14 5 v -1906.93 -1617.14 495 v -1903.59 -1615.12 495 v -1906.93 -1617.14 5 v -1906.93 -1617.14 5 v -1903.59 -1615.12 495 v -1903.59 -1615.12 5 v -1903.59 -1615.12 495 v -1901.29 -1611.97 495 v -1903.59 -1615.12 5 v -1903.59 -1615.12 5 v -1901.29 -1611.97 495 v -1901.29 -1611.97 5 v -1901.29 -1611.97 495 v -1900.36 -1608.18 495 v -1901.29 -1611.97 5 v -1901.29 -1611.97 5 v -1900.36 -1608.18 495 v -1900.36 -1608.18 5 v -1900.36 -1608.18 495 v -1900.95 -1604.32 495 v -1900.36 -1608.18 5 v -1900.36 -1608.18 5 v -1900.95 -1604.32 495 v -1900.95 -1604.32 5 v -2182.86 -1706.93 495 v -2184.88 -1703.59 495 v -2182.86 -1706.93 5 v -2182.86 -1706.93 5 v -2184.88 -1703.59 495 v -2184.88 -1703.59 5 v -2184.88 -1703.59 495 v -2188.03 -1701.29 495 v -2184.88 -1703.59 5 v -2184.88 -1703.59 5 v -2188.03 -1701.29 495 v -2188.03 -1701.29 5 v -2188.03 -1701.29 495 v -2191.82 -1700.36 495 v -2188.03 -1701.29 5 v -2188.03 -1701.29 5 v -2191.82 -1700.36 495 v -2191.82 -1700.36 5 v -2191.82 -1700.36 495 v -2195.68 -1700.95 495 v -2191.82 -1700.36 5 v -2191.82 -1700.36 5 v -2195.68 -1700.95 495 v -2195.68 -1700.95 5 v -2195.68 -1700.95 495 v -2199.01 -1702.98 495 v -2195.68 -1700.95 5 v -2195.68 -1700.95 5 v -2199.01 -1702.98 495 v -2199.01 -1702.98 5 v -2199.01 -1702.98 495 v -2201.32 -1706.12 495 v -2199.01 -1702.98 5 v -2199.01 -1702.98 5 v -2201.32 -1706.12 495 v -2201.32 -1706.12 5 v -2201.32 -1706.12 495 v -2202.25 -1709.91 495 v -2201.32 -1706.12 5 v -2201.32 -1706.12 5 v -2202.25 -1709.91 495 v -2202.25 -1709.91 5 v -2202.25 -1709.91 495 v -2201.65 -1713.77 495 v -2202.25 -1709.91 5 v -2202.25 -1709.91 5 v -2201.65 -1713.77 495 v -2201.65 -1713.77 5 v -2201.65 -1713.77 495 v -2199.63 -1717.1 495 v -2201.65 -1713.77 5 v -2201.65 -1713.77 5 v -2199.63 -1717.1 495 v -2199.63 -1717.1 5 v -2199.63 -1717.1 495 v -2196.48 -1719.41 495 v -2199.63 -1717.1 5 v -2199.63 -1717.1 5 v -2196.48 -1719.41 495 v -2196.48 -1719.41 5 v -2196.48 -1719.41 495 v -2192.69 -1720.34 495 v -2196.48 -1719.41 5 v -2196.48 -1719.41 5 v -2192.69 -1720.34 495 v -2192.69 -1720.34 5 v -2192.69 -1720.34 495 v -2188.84 -1719.75 495 v -2192.69 -1720.34 5 v -2192.69 -1720.34 5 v -2188.84 -1719.75 495 v -2188.84 -1719.75 5 v -2188.84 -1719.75 495 v -2185.5 -1717.72 495 v -2188.84 -1719.75 5 v -2188.84 -1719.75 5 v -2185.5 -1717.72 495 v -2185.5 -1717.72 5 v -2185.5 -1717.72 495 v -2183.19 -1714.58 495 v -2185.5 -1717.72 5 v -2185.5 -1717.72 5 v -2183.19 -1714.58 495 v -2183.19 -1714.58 5 v -2183.19 -1714.58 495 v -2182.27 -1710.79 495 v -2183.19 -1714.58 5 v -2183.19 -1714.58 5 v -2182.27 -1710.79 495 v -2182.27 -1710.79 5 v -2182.27 -1710.79 495 v -2182.86 -1706.93 495 v -2182.27 -1710.79 5 v -2182.27 -1710.79 5 v -2182.86 -1706.93 495 v -2182.86 -1706.93 5 v -2080.25 -1988.84 495 v -2082.28 -1985.5 495 v -2080.25 -1988.84 5 v -2080.25 -1988.84 5 v -2082.28 -1985.5 495 v -2082.28 -1985.5 5 v -2082.28 -1985.5 495 v -2085.42 -1983.19 495 v -2082.28 -1985.5 5 v -2082.28 -1985.5 5 v -2085.42 -1983.19 495 v -2085.42 -1983.19 5 v -2085.42 -1983.19 495 v -2089.21 -1982.27 495 v -2085.42 -1983.19 5 v -2085.42 -1983.19 5 v -2089.21 -1982.27 495 v -2089.21 -1982.27 5 v -2089.21 -1982.27 495 v -2093.07 -1982.86 495 v -2089.21 -1982.27 5 v -2089.21 -1982.27 5 v -2093.07 -1982.86 495 v -2093.07 -1982.86 5 v -2093.07 -1982.86 495 v -2096.41 -1984.88 495 v -2093.07 -1982.86 5 v -2093.07 -1982.86 5 v -2096.41 -1984.88 495 v -2096.41 -1984.88 5 v -2096.41 -1984.88 495 v -2098.71 -1988.03 495 v -2096.41 -1984.88 5 v -2096.41 -1984.88 5 v -2098.71 -1988.03 495 v -2098.71 -1988.03 5 v -2098.71 -1988.03 495 v -2099.64 -1991.82 495 v -2098.71 -1988.03 5 v -2098.71 -1988.03 5 v -2099.64 -1991.82 495 v -2099.64 -1991.82 5 v -2099.64 -1991.82 495 v -2099.05 -1995.68 495 v -2099.64 -1991.82 5 v -2099.64 -1991.82 5 v -2099.05 -1995.68 495 v -2099.05 -1995.68 5 v -2099.05 -1995.68 495 v -2097.02 -1999.01 495 v -2099.05 -1995.68 5 v -2099.05 -1995.68 5 v -2097.02 -1999.01 495 v -2097.02 -1999.01 5 v -2097.02 -1999.01 495 v -2093.88 -2001.32 495 v -2097.02 -1999.01 5 v -2097.02 -1999.01 5 v -2093.88 -2001.32 495 v -2093.88 -2001.32 5 v -2093.88 -2001.32 495 v -2090.09 -2002.25 495 v -2093.88 -2001.32 5 v -2093.88 -2001.32 5 v -2090.09 -2002.25 495 v -2090.09 -2002.25 5 v -2090.09 -2002.25 495 v -2086.23 -2001.65 495 v -2090.09 -2002.25 5 v -2090.09 -2002.25 5 v -2086.23 -2001.65 495 v -2086.23 -2001.65 5 v -2086.23 -2001.65 495 v -2082.9 -1999.63 495 v -2086.23 -2001.65 5 v -2086.23 -2001.65 5 v -2082.9 -1999.63 495 v -2082.9 -1999.63 5 v -2082.9 -1999.63 495 v -2080.59 -1996.48 495 v -2082.9 -1999.63 5 v -2082.9 -1999.63 5 v -2080.59 -1996.48 495 v -2080.59 -1996.48 5 v -2080.59 -1996.48 495 v -2079.66 -1992.69 495 v -2080.59 -1996.48 5 v -2080.59 -1996.48 5 v -2079.66 -1992.69 495 v -2079.66 -1992.69 5 v -2079.66 -1992.69 495 v -2080.25 -1988.84 495 v -2079.66 -1992.69 5 v -2079.66 -1992.69 5 v -2080.25 -1988.84 495 v -2080.25 -1988.84 5 v -1798.35 -1886.23 495 v -1800.37 -1882.9 495 v -1798.35 -1886.23 5 v -1798.35 -1886.23 5 v -1800.37 -1882.9 495 v -1800.37 -1882.9 5 v -1800.37 -1882.9 495 v -1803.52 -1880.59 495 v -1800.37 -1882.9 5 v -1800.37 -1882.9 5 v -1803.52 -1880.59 495 v -1803.52 -1880.59 5 v -1803.52 -1880.59 495 v -1807.31 -1879.66 495 v -1803.52 -1880.59 5 v -1803.52 -1880.59 5 v -1807.31 -1879.66 495 v -1807.31 -1879.66 5 v -1807.31 -1879.66 495 v -1811.16 -1880.25 495 v -1807.31 -1879.66 5 v -1807.31 -1879.66 5 v -1811.16 -1880.25 495 v -1811.16 -1880.25 5 v -1811.16 -1880.25 495 v -1814.5 -1882.28 495 v -1811.16 -1880.25 5 v -1811.16 -1880.25 5 v -1814.5 -1882.28 495 v -1814.5 -1882.28 5 v -1814.5 -1882.28 495 v -1816.81 -1885.42 495 v -1814.5 -1882.28 5 v -1814.5 -1882.28 5 v -1816.81 -1885.42 495 v -1816.81 -1885.42 5 v -1816.81 -1885.42 495 v -1817.73 -1889.21 495 v -1816.81 -1885.42 5 v -1816.81 -1885.42 5 v -1817.73 -1889.21 495 v -1817.73 -1889.21 5 v -1817.73 -1889.21 495 v -1817.14 -1893.07 495 v -1817.73 -1889.21 5 v -1817.73 -1889.21 5 v -1817.14 -1893.07 495 v -1817.14 -1893.07 5 v -1817.14 -1893.07 495 v -1815.12 -1896.41 495 v -1817.14 -1893.07 5 v -1817.14 -1893.07 5 v -1815.12 -1896.41 495 v -1815.12 -1896.41 5 v -1815.12 -1896.41 495 v -1811.97 -1898.71 495 v -1815.12 -1896.41 5 v -1815.12 -1896.41 5 v -1811.97 -1898.71 495 v -1811.97 -1898.71 5 v -1811.97 -1898.71 495 v -1808.18 -1899.64 495 v -1811.97 -1898.71 5 v -1811.97 -1898.71 5 v -1808.18 -1899.64 495 v -1808.18 -1899.64 5 v -1808.18 -1899.64 495 v -1804.32 -1899.05 495 v -1808.18 -1899.64 5 v -1808.18 -1899.64 5 v -1804.32 -1899.05 495 v -1804.32 -1899.05 5 v -1804.32 -1899.05 495 v -1800.99 -1897.02 495 v -1804.32 -1899.05 5 v -1804.32 -1899.05 5 v -1800.99 -1897.02 495 v -1800.99 -1897.02 5 v -1800.99 -1897.02 495 v -1798.68 -1893.88 495 v -1800.99 -1897.02 5 v -1800.99 -1897.02 5 v -1798.68 -1893.88 495 v -1798.68 -1893.88 5 v -1798.68 -1893.88 495 v -1797.75 -1890.09 495 v -1798.68 -1893.88 5 v -1798.68 -1893.88 5 v -1797.75 -1890.09 495 v -1797.75 -1890.09 5 v -1797.75 -1890.09 495 v -1798.35 -1886.23 495 v -1797.75 -1890.09 5 v -1797.75 -1890.09 5 v -1798.35 -1886.23 495 v -1798.35 -1886.23 5 v -2119.53 -2056.34 510 v -1743.66 -1919.53 510 v -2256.34 -1680.47 510 v -2256.34 -1680.47 510 v -1743.66 -1919.53 510 v -1880.47 -1543.66 510 v -1743.66 -1919.53 500 v -2119.53 -2056.34 500 v -1880.47 -1543.66 500 v -1880.47 -1543.66 500 v -2119.53 -2056.34 500 v -2256.34 -1680.47 500 v -1743.66 -1919.53 510 v -1743.66 -1919.53 500 v -1880.47 -1543.66 510 v -1880.47 -1543.66 510 v -1743.66 -1919.53 500 v -1880.47 -1543.66 500 v -2119.53 -2056.34 500 v -2119.53 -2056.34 510 v -2256.34 -1680.47 500 v -2256.34 -1680.47 500 v -2119.53 -2056.34 510 v -2256.34 -1680.47 510 v -2256.34 -1680.47 500 v -2256.34 -1680.47 510 v -1880.47 -1543.66 500 v -1880.47 -1543.66 500 v -2256.34 -1680.47 510 v -1880.47 -1543.66 510 v -2119.53 -2056.34 500 v -1743.66 -1919.53 500 v -2119.53 -2056.34 510 v -2119.53 -2056.34 510 v -1743.66 -1919.53 500 v -1743.66 -1919.53 510 v -2013.73 -1586.84 810 v -2015.76 -1583.51 810 v -2013.73 -1586.84 510 v -2013.73 -1586.84 510 v -2015.76 -1583.51 810 v -2015.76 -1583.51 510 v -2015.76 -1583.51 810 v -2018.9 -1581.2 810 v -2015.76 -1583.51 510 v -2015.76 -1583.51 510 v -2018.9 -1581.2 810 v -2018.9 -1581.2 510 v -2018.9 -1581.2 810 v -2022.69 -1580.27 810 v -2018.9 -1581.2 510 v -2018.9 -1581.2 510 v -2022.69 -1580.27 810 v -2022.69 -1580.27 510 v -2022.69 -1580.27 810 v -2026.55 -1580.86 810 v -2022.69 -1580.27 510 v -2022.69 -1580.27 510 v -2026.55 -1580.86 810 v -2026.55 -1580.86 510 v -2026.55 -1580.86 810 v -2029.89 -1582.89 810 v -2026.55 -1580.86 510 v -2026.55 -1580.86 510 v -2029.89 -1582.89 810 v -2029.89 -1582.89 510 v -2029.89 -1582.89 810 v -2032.19 -1586.04 810 v -2029.89 -1582.89 510 v -2029.89 -1582.89 510 v -2032.19 -1586.04 810 v -2032.19 -1586.04 510 v -2032.19 -1586.04 810 v -2033.12 -1589.83 810 v -2032.19 -1586.04 510 v -2032.19 -1586.04 510 v -2033.12 -1589.83 810 v -2033.12 -1589.83 510 v -2033.12 -1589.83 810 v -2032.53 -1593.68 810 v -2033.12 -1589.83 510 v -2033.12 -1589.83 510 v -2032.53 -1593.68 810 v -2032.53 -1593.68 510 v -2032.53 -1593.68 810 v -2030.5 -1597.02 810 v -2032.53 -1593.68 510 v -2032.53 -1593.68 510 v -2030.5 -1597.02 810 v -2030.5 -1597.02 510 v -2030.5 -1597.02 810 v -2027.36 -1599.33 810 v -2030.5 -1597.02 510 v -2030.5 -1597.02 510 v -2027.36 -1599.33 810 v -2027.36 -1599.33 510 v -2027.36 -1599.33 810 v -2023.57 -1600.25 810 v -2027.36 -1599.33 510 v -2027.36 -1599.33 510 v -2023.57 -1600.25 810 v -2023.57 -1600.25 510 v -2023.57 -1600.25 810 v -2019.71 -1599.66 810 v -2023.57 -1600.25 510 v -2023.57 -1600.25 510 v -2019.71 -1599.66 810 v -2019.71 -1599.66 510 v -2019.71 -1599.66 810 v -2016.37 -1597.63 810 v -2019.71 -1599.66 510 v -2019.71 -1599.66 510 v -2016.37 -1597.63 810 v -2016.37 -1597.63 510 v -2016.37 -1597.63 810 v -2014.07 -1594.49 810 v -2016.37 -1597.63 510 v -2016.37 -1597.63 510 v -2014.07 -1594.49 810 v -2014.07 -1594.49 510 v -2014.07 -1594.49 810 v -2013.14 -1590.7 810 v -2014.07 -1594.49 510 v -2014.07 -1594.49 510 v -2013.14 -1590.7 810 v -2013.14 -1590.7 510 v -2013.14 -1590.7 810 v -2013.73 -1586.84 810 v -2013.14 -1590.7 510 v -2013.14 -1590.7 510 v -2013.73 -1586.84 810 v -2013.73 -1586.84 510 v -2107.7 -1621.04 810 v -2109.73 -1617.71 810 v -2107.7 -1621.04 510 v -2107.7 -1621.04 510 v -2109.73 -1617.71 810 v -2109.73 -1617.71 510 v -2109.73 -1617.71 810 v -2112.87 -1615.4 810 v -2109.73 -1617.71 510 v -2109.73 -1617.71 510 v -2112.87 -1615.4 810 v -2112.87 -1615.4 510 v -2112.87 -1615.4 810 v -2116.66 -1614.47 810 v -2112.87 -1615.4 510 v -2112.87 -1615.4 510 v -2116.66 -1614.47 810 v -2116.66 -1614.47 510 v -2116.66 -1614.47 810 v -2120.52 -1615.07 810 v -2116.66 -1614.47 510 v -2116.66 -1614.47 510 v -2120.52 -1615.07 810 v -2120.52 -1615.07 510 v -2120.52 -1615.07 810 v -2123.85 -1617.09 810 v -2120.52 -1615.07 510 v -2120.52 -1615.07 510 v -2123.85 -1617.09 810 v -2123.85 -1617.09 510 v -2123.85 -1617.09 810 v -2126.16 -1620.24 810 v -2123.85 -1617.09 510 v -2123.85 -1617.09 510 v -2126.16 -1620.24 810 v -2126.16 -1620.24 510 v -2126.16 -1620.24 810 v -2127.09 -1624.03 810 v -2126.16 -1620.24 510 v -2126.16 -1620.24 510 v -2127.09 -1624.03 810 v -2127.09 -1624.03 510 v -2127.09 -1624.03 810 v -2126.5 -1627.88 810 v -2127.09 -1624.03 510 v -2127.09 -1624.03 510 v -2126.5 -1627.88 810 v -2126.5 -1627.88 510 v -2126.5 -1627.88 810 v -2124.47 -1631.22 810 v -2126.5 -1627.88 510 v -2126.5 -1627.88 510 v -2124.47 -1631.22 810 v -2124.47 -1631.22 510 v -2124.47 -1631.22 810 v -2121.32 -1633.53 810 v -2124.47 -1631.22 510 v -2124.47 -1631.22 510 v -2121.32 -1633.53 810 v -2121.32 -1633.53 510 v -2121.32 -1633.53 810 v -2117.53 -1634.45 810 v -2121.32 -1633.53 510 v -2121.32 -1633.53 510 v -2117.53 -1634.45 810 v -2117.53 -1634.45 510 v -2117.53 -1634.45 810 v -2113.68 -1633.86 810 v -2117.53 -1634.45 510 v -2117.53 -1634.45 510 v -2113.68 -1633.86 810 v -2113.68 -1633.86 510 v -2113.68 -1633.86 810 v -2110.34 -1631.84 810 v -2113.68 -1633.86 510 v -2113.68 -1633.86 510 v -2110.34 -1631.84 810 v -2110.34 -1631.84 510 v -2110.34 -1631.84 810 v -2108.04 -1628.69 810 v -2110.34 -1631.84 510 v -2110.34 -1631.84 510 v -2108.04 -1628.69 810 v -2108.04 -1628.69 510 v -2108.04 -1628.69 810 v -2107.11 -1624.9 810 v -2108.04 -1628.69 510 v -2108.04 -1628.69 510 v -2107.11 -1624.9 810 v -2107.11 -1624.9 510 v -2107.11 -1624.9 810 v -2107.7 -1621.04 810 v -2107.11 -1624.9 510 v -2107.11 -1624.9 510 v -2107.7 -1621.04 810 v -2107.7 -1621.04 510 v -2252.92 -1689.86 910 v -1877.05 -1553.05 910 v -2256.34 -1680.47 910 v -2256.34 -1680.47 910 v -1877.05 -1553.05 910 v -1880.47 -1543.66 910 v -1877.05 -1553.05 710 v -2252.92 -1689.86 710 v -1880.47 -1543.66 710 v -1880.47 -1543.66 710 v -2252.92 -1689.86 710 v -2256.34 -1680.47 710 v -1877.05 -1553.05 910 v -1877.05 -1553.05 710 v -1880.47 -1543.66 910 v -1880.47 -1543.66 910 v -1877.05 -1553.05 710 v -1880.47 -1543.66 710 v -2252.92 -1689.86 710 v -2252.92 -1689.86 910 v -2256.34 -1680.47 710 v -2256.34 -1680.47 710 v -2252.92 -1689.86 910 v -2256.34 -1680.47 910 v -2256.34 -1680.47 710 v -2256.34 -1680.47 910 v -1880.47 -1543.66 710 v -1880.47 -1543.66 710 v -2256.34 -1680.47 910 v -1880.47 -1543.66 910 v -2252.92 -1689.86 710 v -1877.05 -1553.05 710 v -2252.92 -1689.86 910 v -2252.92 -1689.86 910 v -1877.05 -1553.05 710 v -1877.05 -1553.05 910 v -2880 1350 695 v -2881.52 1357.65 695 v -2880 1350 5 v -2880 1350 5 v -2881.52 1357.65 695 v -2881.52 1357.65 5 v -2881.52 1357.65 695 v -2885.86 1364.14 695 v -2881.52 1357.65 5 v -2881.52 1357.65 5 v -2885.86 1364.14 695 v -2885.86 1364.14 5 v -2885.86 1364.14 695 v -2892.35 1368.48 695 v -2885.86 1364.14 5 v -2885.86 1364.14 5 v -2892.35 1368.48 695 v -2892.35 1368.48 5 v -2892.35 1368.48 695 v -2900 1370 695 v -2892.35 1368.48 5 v -2892.35 1368.48 5 v -2900 1370 695 v -2900 1370 5 v -2900 1370 695 v -2907.65 1368.48 695 v -2900 1370 5 v -2900 1370 5 v -2907.65 1368.48 695 v -2907.65 1368.48 5 v -2907.65 1368.48 695 v -2914.14 1364.14 695 v -2907.65 1368.48 5 v -2907.65 1368.48 5 v -2914.14 1364.14 695 v -2914.14 1364.14 5 v -2914.14 1364.14 695 v -2918.48 1357.65 695 v -2914.14 1364.14 5 v -2914.14 1364.14 5 v -2918.48 1357.65 695 v -2918.48 1357.65 5 v -2918.48 1357.65 695 v -2920 1350 695 v -2918.48 1357.65 5 v -2918.48 1357.65 5 v -2920 1350 695 v -2920 1350 5 v -2920 1350 695 v -2918.48 1342.35 695 v -2920 1350 5 v -2920 1350 5 v -2918.48 1342.35 695 v -2918.48 1342.35 5 v -2918.48 1342.35 695 v -2914.14 1335.86 695 v -2918.48 1342.35 5 v -2918.48 1342.35 5 v -2914.14 1335.86 695 v -2914.14 1335.86 5 v -2914.14 1335.86 695 v -2907.65 1331.52 695 v -2914.14 1335.86 5 v -2914.14 1335.86 5 v -2907.65 1331.52 695 v -2907.65 1331.52 5 v -2907.65 1331.52 695 v -2900 1330 695 v -2907.65 1331.52 5 v -2907.65 1331.52 5 v -2900 1330 695 v -2900 1330 5 v -2900 1330 695 v -2892.35 1331.52 695 v -2900 1330 5 v -2900 1330 5 v -2892.35 1331.52 695 v -2892.35 1331.52 5 v -2892.35 1331.52 695 v -2885.86 1335.86 695 v -2892.35 1331.52 5 v -2892.35 1331.52 5 v -2885.86 1335.86 695 v -2885.86 1335.86 5 v -2885.86 1335.86 695 v -2881.52 1342.35 695 v -2885.86 1335.86 5 v -2885.86 1335.86 5 v -2881.52 1342.35 695 v -2881.52 1342.35 5 v -2881.52 1342.35 695 v -2880 1350 695 v -2881.52 1342.35 5 v -2881.52 1342.35 5 v -2880 1350 695 v -2880 1350 5 v 20 1350 695 v 18.4776 1357.65 695 v 20 1350 5 v 20 1350 5 v 18.4776 1357.65 695 v 18.4776 1357.65 5 v 18.4776 1357.65 695 v 14.1421 1364.14 695 v 18.4776 1357.65 5 v 18.4776 1357.65 5 v 14.1421 1364.14 695 v 14.1421 1364.14 5 v 14.1421 1364.14 695 v 7.65367 1368.48 695 v 14.1421 1364.14 5 v 14.1421 1364.14 5 v 7.65367 1368.48 695 v 7.65367 1368.48 5 v 7.65367 1368.48 695 v -1e-006 1370 695 v 7.65367 1368.48 5 v 7.65367 1368.48 5 v -1e-006 1370 695 v -1e-006 1370 5 v -1e-006 1370 695 v -7.65367 1368.48 695 v -1e-006 1370 5 v -1e-006 1370 5 v -7.65367 1368.48 695 v -7.65367 1368.48 5 v -7.65367 1368.48 695 v -14.1421 1364.14 695 v -7.65367 1368.48 5 v -7.65367 1368.48 5 v -14.1421 1364.14 695 v -14.1421 1364.14 5 v -14.1421 1364.14 695 v -18.4776 1357.65 695 v -14.1421 1364.14 5 v -14.1421 1364.14 5 v -18.4776 1357.65 695 v -18.4776 1357.65 5 v -18.4776 1357.65 695 v -20 1350 695 v -18.4776 1357.65 5 v -18.4776 1357.65 5 v -20 1350 695 v -20 1350 5 v -20 1350 695 v -18.4776 1342.35 695 v -20 1350 5 v -20 1350 5 v -18.4776 1342.35 695 v -18.4776 1342.35 5 v -18.4776 1342.35 695 v -14.1421 1335.86 695 v -18.4776 1342.35 5 v -18.4776 1342.35 5 v -14.1421 1335.86 695 v -14.1421 1335.86 5 v -14.1421 1335.86 695 v -7.65367 1331.52 695 v -14.1421 1335.86 5 v -14.1421 1335.86 5 v -7.65367 1331.52 695 v -7.65367 1331.52 5 v -7.65367 1331.52 695 v 0 1330 695 v -7.65367 1331.52 5 v -7.65367 1331.52 5 v 0 1330 695 v 0 1330 5 v 0 1330 695 v 7.65367 1331.52 695 v 0 1330 5 v 0 1330 5 v 7.65367 1331.52 695 v 7.65367 1331.52 5 v 7.65367 1331.52 695 v 14.1421 1335.86 695 v 7.65367 1331.52 5 v 7.65367 1331.52 5 v 14.1421 1335.86 695 v 14.1421 1335.86 5 v 14.1421 1335.86 695 v 18.4776 1342.35 695 v 14.1421 1335.86 5 v 14.1421 1335.86 5 v 18.4776 1342.35 695 v 18.4776 1342.35 5 v 18.4776 1342.35 695 v 20 1350 695 v 18.4776 1342.35 5 v 18.4776 1342.35 5 v 20 1350 695 v 20 1350 5 v 20 2650 695 v 18.4776 2657.65 695 v 20 2650 5 v 20 2650 5 v 18.4776 2657.65 695 v 18.4776 2657.65 5 v 18.4776 2657.65 695 v 14.1421 2664.14 695 v 18.4776 2657.65 5 v 18.4776 2657.65 5 v 14.1421 2664.14 695 v 14.1421 2664.14 5 v 14.1421 2664.14 695 v 7.65367 2668.48 695 v 14.1421 2664.14 5 v 14.1421 2664.14 5 v 7.65367 2668.48 695 v 7.65367 2668.48 5 v 7.65367 2668.48 695 v -1e-006 2670 695 v 7.65367 2668.48 5 v 7.65367 2668.48 5 v -1e-006 2670 695 v -1e-006 2670 5 v -1e-006 2670 695 v -7.65367 2668.48 695 v -1e-006 2670 5 v -1e-006 2670 5 v -7.65367 2668.48 695 v -7.65367 2668.48 5 v -7.65367 2668.48 695 v -14.1421 2664.14 695 v -7.65367 2668.48 5 v -7.65367 2668.48 5 v -14.1421 2664.14 695 v -14.1421 2664.14 5 v -14.1421 2664.14 695 v -18.4776 2657.65 695 v -14.1421 2664.14 5 v -14.1421 2664.14 5 v -18.4776 2657.65 695 v -18.4776 2657.65 5 v -18.4776 2657.65 695 v -20 2650 695 v -18.4776 2657.65 5 v -18.4776 2657.65 5 v -20 2650 695 v -20 2650 5 v -20 2650 695 v -18.4776 2642.35 695 v -20 2650 5 v -20 2650 5 v -18.4776 2642.35 695 v -18.4776 2642.35 5 v -18.4776 2642.35 695 v -14.1421 2635.86 695 v -18.4776 2642.35 5 v -18.4776 2642.35 5 v -14.1421 2635.86 695 v -14.1421 2635.86 5 v -14.1421 2635.86 695 v -7.65367 2631.52 695 v -14.1421 2635.86 5 v -14.1421 2635.86 5 v -7.65367 2631.52 695 v -7.65367 2631.52 5 v -7.65367 2631.52 695 v 0 2630 695 v -7.65367 2631.52 5 v -7.65367 2631.52 5 v 0 2630 695 v 0 2630 5 v 0 2630 695 v 7.65367 2631.52 695 v 0 2630 5 v 0 2630 5 v 7.65367 2631.52 695 v 7.65367 2631.52 5 v 7.65367 2631.52 695 v 14.1421 2635.86 695 v 7.65367 2631.52 5 v 7.65367 2631.52 5 v 14.1421 2635.86 695 v 14.1421 2635.86 5 v 14.1421 2635.86 695 v 18.4776 2642.35 695 v 14.1421 2635.86 5 v 14.1421 2635.86 5 v 18.4776 2642.35 695 v 18.4776 2642.35 5 v 18.4776 2642.35 695 v 20 2650 695 v 18.4776 2642.35 5 v 18.4776 2642.35 5 v 20 2650 695 v 20 2650 5 v -2880 2650 695 v -2881.52 2657.65 695 v -2880 2650 5 v -2880 2650 5 v -2881.52 2657.65 695 v -2881.52 2657.65 5 v -2881.52 2657.65 695 v -2885.86 2664.14 695 v -2881.52 2657.65 5 v -2881.52 2657.65 5 v -2885.86 2664.14 695 v -2885.86 2664.14 5 v -2885.86 2664.14 695 v -2892.35 2668.48 695 v -2885.86 2664.14 5 v -2885.86 2664.14 5 v -2892.35 2668.48 695 v -2892.35 2668.48 5 v -2892.35 2668.48 695 v -2900 2670 695 v -2892.35 2668.48 5 v -2892.35 2668.48 5 v -2900 2670 695 v -2900 2670 5 v -2900 2670 695 v -2907.65 2668.48 695 v -2900 2670 5 v -2900 2670 5 v -2907.65 2668.48 695 v -2907.65 2668.48 5 v -2907.65 2668.48 695 v -2914.14 2664.14 695 v -2907.65 2668.48 5 v -2907.65 2668.48 5 v -2914.14 2664.14 695 v -2914.14 2664.14 5 v -2914.14 2664.14 695 v -2918.48 2657.65 695 v -2914.14 2664.14 5 v -2914.14 2664.14 5 v -2918.48 2657.65 695 v -2918.48 2657.65 5 v -2918.48 2657.65 695 v -2920 2650 695 v -2918.48 2657.65 5 v -2918.48 2657.65 5 v -2920 2650 695 v -2920 2650 5 v -2920 2650 695 v -2918.48 2642.35 695 v -2920 2650 5 v -2920 2650 5 v -2918.48 2642.35 695 v -2918.48 2642.35 5 v -2918.48 2642.35 695 v -2914.14 2635.86 695 v -2918.48 2642.35 5 v -2918.48 2642.35 5 v -2914.14 2635.86 695 v -2914.14 2635.86 5 v -2914.14 2635.86 695 v -2907.65 2631.52 695 v -2914.14 2635.86 5 v -2914.14 2635.86 5 v -2907.65 2631.52 695 v -2907.65 2631.52 5 v -2907.65 2631.52 695 v -2900 2630 695 v -2907.65 2631.52 5 v -2907.65 2631.52 5 v -2900 2630 695 v -2900 2630 5 v -2900 2630 695 v -2892.35 2631.52 695 v -2900 2630 5 v -2900 2630 5 v -2892.35 2631.52 695 v -2892.35 2631.52 5 v -2892.35 2631.52 695 v -2885.86 2635.86 695 v -2892.35 2631.52 5 v -2892.35 2631.52 5 v -2885.86 2635.86 695 v -2885.86 2635.86 5 v -2885.86 2635.86 695 v -2881.52 2642.35 695 v -2885.86 2635.86 5 v -2885.86 2635.86 5 v -2881.52 2642.35 695 v -2881.52 2642.35 5 v -2881.52 2642.35 695 v -2880 2650 695 v -2881.52 2642.35 5 v -2881.52 2642.35 5 v -2880 2650 695 v -2880 2650 5 v -2900 1250 730 v -100 1250 730 v -2900 2750 730 v -2900 2750 730 v -100 1250 730 v -100 2750 730 v -100 1250 680 v -2900 1250 680 v -100 2750 680 v -100 2750 680 v -2900 1250 680 v -2900 2750 680 v -100 1250 730 v -100 1250 680 v -100 2750 730 v -100 2750 730 v -100 1250 680 v -100 2750 680 v -2900 1250 680 v -2900 1250 730 v -2900 2750 680 v -2900 2750 680 v -2900 1250 730 v -2900 2750 730 v -2900 2750 680 v -2900 2750 730 v -100 2750 680 v -100 2750 680 v -2900 2750 730 v -100 2750 730 v -2900 1250 680 v -100 1250 680 v -2900 1250 730 v -2900 1250 730 v -100 1250 680 v -100 1250 730 v -281.015 2712.06 495 v -279.138 2715.48 495 v -281.015 2712.06 5 v -281.015 2712.06 5 v -279.138 2715.48 495 v -279.138 2715.48 5 v -279.138 2715.48 495 v -278.713 2719.36 495 v -279.138 2715.48 5 v -279.138 2715.48 5 v -278.713 2719.36 495 v -278.713 2719.36 5 v -278.713 2719.36 495 v -279.805 2723.11 495 v -278.713 2719.36 5 v -278.713 2719.36 5 v -279.805 2723.11 495 v -279.805 2723.11 5 v -279.805 2723.11 495 v -282.247 2726.15 495 v -279.805 2723.11 5 v -279.805 2723.11 5 v -282.247 2726.15 495 v -282.247 2726.15 5 v -282.247 2726.15 495 v -285.668 2728.03 495 v -282.247 2726.15 5 v -282.247 2726.15 5 v -285.668 2728.03 495 v -285.668 2728.03 5 v -285.668 2728.03 495 v -289.547 2728.45 495 v -285.668 2728.03 5 v -285.668 2728.03 5 v -289.547 2728.45 495 v -289.547 2728.45 5 v -289.547 2728.45 495 v -293.293 2727.36 495 v -289.547 2728.45 5 v -289.547 2728.45 5 v -293.293 2727.36 495 v -293.293 2727.36 5 v -293.293 2727.36 495 v -296.336 2724.92 495 v -293.293 2727.36 5 v -293.293 2727.36 5 v -296.336 2724.92 495 v -296.336 2724.92 5 v -296.336 2724.92 495 v -298.212 2721.5 495 v -296.336 2724.92 5 v -296.336 2724.92 5 v -298.212 2721.5 495 v -298.212 2721.5 5 v -298.212 2721.5 495 v -298.637 2717.62 495 v -298.212 2721.5 5 v -298.212 2721.5 5 v -298.637 2717.62 495 v -298.637 2717.62 5 v -298.637 2717.62 495 v -297.545 2713.87 495 v -298.637 2717.62 5 v -298.637 2717.62 5 v -297.545 2713.87 495 v -297.545 2713.87 5 v -297.545 2713.87 495 v -295.103 2710.83 495 v -297.545 2713.87 5 v -297.545 2713.87 5 v -295.103 2710.83 495 v -295.103 2710.83 5 v -295.103 2710.83 495 v -291.682 2708.95 495 v -295.103 2710.83 5 v -295.103 2710.83 5 v -291.682 2708.95 495 v -291.682 2708.95 5 v -291.682 2708.95 495 v -287.804 2708.53 495 v -291.682 2708.95 5 v -291.682 2708.95 5 v -287.804 2708.53 495 v -287.804 2708.53 5 v -287.804 2708.53 495 v -284.058 2709.62 495 v -287.804 2708.53 5 v -287.804 2708.53 5 v -284.058 2709.62 495 v -284.058 2709.62 5 v -284.058 2709.62 495 v -281.015 2712.06 495 v -284.058 2709.62 5 v -284.058 2709.62 5 v -281.015 2712.06 495 v -281.015 2712.06 5 v -510.828 2904.9 495 v -508.951 2908.32 495 v -510.828 2904.9 5 v -510.828 2904.9 5 v -508.951 2908.32 495 v -508.951 2908.32 5 v -508.951 2908.32 495 v -508.527 2912.2 495 v -508.951 2908.32 5 v -508.951 2908.32 5 v -508.527 2912.2 495 v -508.527 2912.2 5 v -508.527 2912.2 495 v -509.618 2915.94 495 v -508.527 2912.2 5 v -508.527 2912.2 5 v -509.618 2915.94 495 v -509.618 2915.94 5 v -509.618 2915.94 495 v -512.061 2918.99 495 v -509.618 2915.94 5 v -509.618 2915.94 5 v -512.061 2918.99 495 v -512.061 2918.99 5 v -512.061 2918.99 495 v -515.481 2920.86 495 v -512.061 2918.99 5 v -512.061 2918.99 5 v -515.481 2920.86 495 v -515.481 2920.86 5 v -515.481 2920.86 495 v -519.36 2921.29 495 v -515.481 2920.86 5 v -515.481 2920.86 5 v -519.36 2921.29 495 v -519.36 2921.29 5 v -519.36 2921.29 495 v -523.106 2920.19 495 v -519.36 2921.29 5 v -519.36 2921.29 5 v -523.106 2920.19 495 v -523.106 2920.19 5 v -523.106 2920.19 495 v -526.149 2917.75 495 v -523.106 2920.19 5 v -523.106 2920.19 5 v -526.149 2917.75 495 v -526.149 2917.75 5 v -526.149 2917.75 495 v -528.026 2914.33 495 v -526.149 2917.75 5 v -526.149 2917.75 5 v -528.026 2914.33 495 v -528.026 2914.33 5 v -528.026 2914.33 495 v -528.451 2910.45 495 v -528.026 2914.33 5 v -528.026 2914.33 5 v -528.451 2910.45 495 v -528.451 2910.45 5 v -528.451 2910.45 495 v -527.359 2906.71 495 v -528.451 2910.45 5 v -528.451 2910.45 5 v -527.359 2906.71 495 v -527.359 2906.71 5 v -527.359 2906.71 495 v -524.916 2903.66 495 v -527.359 2906.71 5 v -527.359 2906.71 5 v -524.916 2903.66 495 v -524.916 2903.66 5 v -524.916 2903.66 495 v -521.496 2901.79 495 v -524.916 2903.66 5 v -524.916 2903.66 5 v -521.496 2901.79 495 v -521.496 2901.79 5 v -521.496 2901.79 495 v -517.617 2901.36 495 v -521.496 2901.79 5 v -521.496 2901.79 5 v -517.617 2901.36 495 v -517.617 2901.36 5 v -517.617 2901.36 495 v -513.871 2902.45 495 v -517.617 2901.36 5 v -517.617 2901.36 5 v -513.871 2902.45 495 v -513.871 2902.45 5 v -513.871 2902.45 495 v -510.828 2904.9 495 v -513.871 2902.45 5 v -513.871 2902.45 5 v -510.828 2904.9 495 v -510.828 2904.9 5 v -703.664 2675.08 495 v -701.788 2678.5 495 v -703.664 2675.08 5 v -703.664 2675.08 5 v -701.788 2678.5 495 v -701.788 2678.5 5 v -701.788 2678.5 495 v -701.363 2682.38 495 v -701.788 2678.5 5 v -701.788 2678.5 5 v -701.363 2682.38 495 v -701.363 2682.38 5 v -701.363 2682.38 495 v -702.455 2686.13 495 v -701.363 2682.38 5 v -701.363 2682.38 5 v -702.455 2686.13 495 v -702.455 2686.13 5 v -702.455 2686.13 495 v -704.897 2689.17 495 v -702.455 2686.13 5 v -702.455 2686.13 5 v -704.897 2689.17 495 v -704.897 2689.17 5 v -704.897 2689.17 495 v -708.318 2691.05 495 v -704.897 2689.17 5 v -704.897 2689.17 5 v -708.318 2691.05 495 v -708.318 2691.05 5 v -708.318 2691.05 495 v -712.196 2691.47 495 v -708.318 2691.05 5 v -708.318 2691.05 5 v -712.196 2691.47 495 v -712.196 2691.47 5 v -712.196 2691.47 495 v -715.942 2690.38 495 v -712.196 2691.47 5 v -712.196 2691.47 5 v -715.942 2690.38 495 v -715.942 2690.38 5 v -715.942 2690.38 495 v -718.985 2687.94 495 v -715.942 2690.38 5 v -715.942 2690.38 5 v -718.985 2687.94 495 v -718.985 2687.94 5 v -718.985 2687.94 495 v -720.862 2684.52 495 v -718.985 2687.94 5 v -718.985 2687.94 5 v -720.862 2684.52 495 v -720.862 2684.52 5 v -720.862 2684.52 495 v -721.287 2680.64 495 v -720.862 2684.52 5 v -720.862 2684.52 5 v -721.287 2680.64 495 v -721.287 2680.64 5 v -721.287 2680.64 495 v -720.195 2676.89 495 v -721.287 2680.64 5 v -721.287 2680.64 5 v -720.195 2676.89 495 v -720.195 2676.89 5 v -720.195 2676.89 495 v -717.753 2673.85 495 v -720.195 2676.89 5 v -720.195 2676.89 5 v -717.753 2673.85 495 v -717.753 2673.85 5 v -717.753 2673.85 495 v -714.332 2671.97 495 v -717.753 2673.85 5 v -717.753 2673.85 5 v -714.332 2671.97 495 v -714.332 2671.97 5 v -714.332 2671.97 495 v -710.453 2671.55 495 v -714.332 2671.97 5 v -714.332 2671.97 5 v -710.453 2671.55 495 v -710.453 2671.55 5 v -710.453 2671.55 495 v -706.707 2672.64 495 v -710.453 2671.55 5 v -710.453 2671.55 5 v -706.707 2672.64 495 v -706.707 2672.64 5 v -706.707 2672.64 495 v -703.664 2675.08 495 v -706.707 2672.64 5 v -706.707 2672.64 5 v -703.664 2675.08 495 v -703.664 2675.08 5 v -473.851 2482.25 495 v -471.974 2485.67 495 v -473.851 2482.25 5 v -473.851 2482.25 5 v -471.974 2485.67 495 v -471.974 2485.67 5 v -471.974 2485.67 495 v -471.55 2489.55 495 v -471.974 2485.67 5 v -471.974 2485.67 5 v -471.55 2489.55 495 v -471.55 2489.55 5 v -471.55 2489.55 495 v -472.641 2493.29 495 v -471.55 2489.55 5 v -471.55 2489.55 5 v -472.641 2493.29 495 v -472.641 2493.29 5 v -472.641 2493.29 495 v -475.084 2496.34 495 v -472.641 2493.29 5 v -472.641 2493.29 5 v -475.084 2496.34 495 v -475.084 2496.34 5 v -475.084 2496.34 495 v -478.504 2498.21 495 v -475.084 2496.34 5 v -475.084 2496.34 5 v -478.504 2498.21 495 v -478.504 2498.21 5 v -478.504 2498.21 495 v -482.383 2498.64 495 v -478.504 2498.21 5 v -478.504 2498.21 5 v -482.383 2498.64 495 v -482.383 2498.64 5 v -482.383 2498.64 495 v -486.129 2497.55 495 v -482.383 2498.64 5 v -482.383 2498.64 5 v -486.129 2497.55 495 v -486.129 2497.55 5 v -486.129 2497.55 495 v -489.172 2495.1 495 v -486.129 2497.55 5 v -486.129 2497.55 5 v -489.172 2495.1 495 v -489.172 2495.1 5 v -489.172 2495.1 495 v -491.049 2491.68 495 v -489.172 2495.1 5 v -489.172 2495.1 5 v -491.049 2491.68 495 v -491.049 2491.68 5 v -491.049 2491.68 495 v -491.473 2487.8 495 v -491.049 2491.68 5 v -491.049 2491.68 5 v -491.473 2487.8 495 v -491.473 2487.8 5 v -491.473 2487.8 495 v -490.382 2484.06 495 v -491.473 2487.8 5 v -491.473 2487.8 5 v -490.382 2484.06 495 v -490.382 2484.06 5 v -490.382 2484.06 495 v -487.939 2481.01 495 v -490.382 2484.06 5 v -490.382 2484.06 5 v -487.939 2481.01 495 v -487.939 2481.01 5 v -487.939 2481.01 495 v -484.519 2479.14 495 v -487.939 2481.01 5 v -487.939 2481.01 5 v -484.519 2479.14 495 v -484.519 2479.14 5 v -484.519 2479.14 495 v -480.64 2478.71 495 v -484.519 2479.14 5 v -484.519 2479.14 5 v -480.64 2478.71 495 v -480.64 2478.71 5 v -480.64 2478.71 495 v -476.894 2479.81 495 v -480.64 2478.71 5 v -480.64 2478.71 5 v -476.894 2479.81 495 v -476.894 2479.81 5 v -476.894 2479.81 495 v -473.851 2482.25 495 v -476.894 2479.81 5 v -476.894 2479.81 5 v -473.851 2482.25 495 v -473.851 2482.25 5 v -781.766 2675.35 510 v -475.349 2418.23 510 v -524.651 2981.77 510 v -524.651 2981.77 510 v -475.349 2418.23 510 v -218.234 2724.65 510 v -475.349 2418.23 500 v -781.766 2675.35 500 v -218.234 2724.65 500 v -218.234 2724.65 500 v -781.766 2675.35 500 v -524.651 2981.77 500 v -475.349 2418.23 510 v -475.349 2418.23 500 v -218.234 2724.65 510 v -218.234 2724.65 510 v -475.349 2418.23 500 v -218.234 2724.65 500 v -781.766 2675.35 500 v -781.766 2675.35 510 v -524.651 2981.77 500 v -524.651 2981.77 500 v -781.766 2675.35 510 v -524.651 2981.77 510 v -524.651 2981.77 500 v -524.651 2981.77 510 v -218.234 2724.65 500 v -218.234 2724.65 500 v -524.651 2981.77 510 v -218.234 2724.65 510 v -781.766 2675.35 500 v -475.349 2418.23 500 v -781.766 2675.35 510 v -781.766 2675.35 510 v -475.349 2418.23 500 v -475.349 2418.23 510 v -322.266 2818.47 810 v -320.389 2821.89 810 v -322.266 2818.47 510 v -322.266 2818.47 510 v -320.389 2821.89 810 v -320.389 2821.89 510 v -320.389 2821.89 810 v -319.964 2825.77 810 v -320.389 2821.89 510 v -320.389 2821.89 510 v -319.964 2825.77 810 v -319.964 2825.77 510 v -319.964 2825.77 810 v -321.056 2829.52 810 v -319.964 2825.77 510 v -319.964 2825.77 510 v -321.056 2829.52 810 v -321.056 2829.52 510 v -321.056 2829.52 810 v -323.498 2832.56 810 v -321.056 2829.52 510 v -321.056 2829.52 510 v -323.498 2832.56 810 v -323.498 2832.56 510 v -323.498 2832.56 810 v -326.919 2834.44 810 v -323.498 2832.56 510 v -323.498 2832.56 510 v -326.919 2834.44 810 v -326.919 2834.44 510 v -326.919 2834.44 810 v -330.798 2834.86 810 v -326.919 2834.44 510 v -326.919 2834.44 510 v -330.798 2834.86 810 v -330.798 2834.86 510 v -330.798 2834.86 810 v -334.544 2833.77 810 v -330.798 2834.86 510 v -330.798 2834.86 510 v -334.544 2833.77 810 v -334.544 2833.77 510 v -334.544 2833.77 810 v -337.587 2831.33 810 v -334.544 2833.77 510 v -334.544 2833.77 510 v -337.587 2831.33 810 v -337.587 2831.33 510 v -337.587 2831.33 810 v -339.464 2827.91 810 v -337.587 2831.33 510 v -337.587 2831.33 510 v -339.464 2827.91 810 v -339.464 2827.91 510 v -339.464 2827.91 810 v -339.888 2824.03 810 v -339.464 2827.91 510 v -339.464 2827.91 510 v -339.888 2824.03 810 v -339.888 2824.03 510 v -339.888 2824.03 810 v -338.796 2820.28 810 v -339.888 2824.03 510 v -339.888 2824.03 510 v -338.796 2820.28 810 v -338.796 2820.28 510 v -338.796 2820.28 810 v -336.354 2817.24 810 v -338.796 2820.28 510 v -338.796 2820.28 510 v -336.354 2817.24 810 v -336.354 2817.24 510 v -336.354 2817.24 810 v -332.933 2815.36 810 v -336.354 2817.24 510 v -336.354 2817.24 510 v -332.933 2815.36 810 v -332.933 2815.36 510 v -332.933 2815.36 810 v -329.055 2814.94 810 v -332.933 2815.36 510 v -332.933 2815.36 510 v -329.055 2814.94 810 v -329.055 2814.94 510 v -329.055 2814.94 810 v -325.309 2816.03 810 v -329.055 2814.94 510 v -329.055 2814.94 510 v -325.309 2816.03 810 v -325.309 2816.03 510 v -325.309 2816.03 810 v -322.266 2818.47 810 v -325.309 2816.03 510 v -325.309 2816.03 510 v -322.266 2818.47 810 v -322.266 2818.47 510 v -398.87 2882.75 810 v -396.994 2886.17 810 v -398.87 2882.75 510 v -398.87 2882.75 510 v -396.994 2886.17 810 v -396.994 2886.17 510 v -396.994 2886.17 810 v -396.569 2890.05 810 v -396.994 2886.17 510 v -396.994 2886.17 510 v -396.569 2890.05 810 v -396.569 2890.05 510 v -396.569 2890.05 810 v -397.661 2893.8 810 v -396.569 2890.05 510 v -396.569 2890.05 510 v -397.661 2893.8 810 v -397.661 2893.8 510 v -397.661 2893.8 810 v -400.103 2896.84 810 v -397.661 2893.8 510 v -397.661 2893.8 510 v -400.103 2896.84 810 v -400.103 2896.84 510 v -400.103 2896.84 810 v -403.524 2898.72 810 v -400.103 2896.84 510 v -400.103 2896.84 510 v -403.524 2898.72 810 v -403.524 2898.72 510 v -403.524 2898.72 810 v -407.402 2899.14 810 v -403.524 2898.72 510 v -403.524 2898.72 510 v -407.402 2899.14 810 v -407.402 2899.14 510 v -407.402 2899.14 810 v -411.148 2898.05 810 v -407.402 2899.14 510 v -407.402 2899.14 510 v -411.148 2898.05 810 v -411.148 2898.05 510 v -411.148 2898.05 810 v -414.191 2895.61 810 v -411.148 2898.05 510 v -411.148 2898.05 510 v -414.191 2895.61 810 v -414.191 2895.61 510 v -414.191 2895.61 810 v -416.068 2892.19 810 v -414.191 2895.61 510 v -414.191 2895.61 510 v -416.068 2892.19 810 v -416.068 2892.19 510 v -416.068 2892.19 810 v -416.493 2888.31 810 v -416.068 2892.19 510 v -416.068 2892.19 510 v -416.493 2888.31 810 v -416.493 2888.31 510 v -416.493 2888.31 810 v -415.401 2884.56 810 v -416.493 2888.31 510 v -416.493 2888.31 510 v -415.401 2884.56 810 v -415.401 2884.56 510 v -415.401 2884.56 810 v -412.959 2881.52 810 v -415.401 2884.56 510 v -415.401 2884.56 510 v -412.959 2881.52 810 v -412.959 2881.52 510 v -412.959 2881.52 810 v -409.538 2879.64 810 v -412.959 2881.52 510 v -412.959 2881.52 510 v -409.538 2879.64 810 v -409.538 2879.64 510 v -409.538 2879.64 810 v -405.659 2879.22 810 v -409.538 2879.64 510 v -409.538 2879.64 510 v -405.659 2879.22 810 v -405.659 2879.22 510 v -405.659 2879.22 810 v -401.913 2880.31 810 v -405.659 2879.22 510 v -405.659 2879.22 510 v -401.913 2880.31 810 v -401.913 2880.31 510 v -401.913 2880.31 810 v -398.87 2882.75 810 v -401.913 2880.31 510 v -401.913 2880.31 510 v -398.87 2882.75 810 v -398.87 2882.75 510 v -531.079 2974.11 910 v -224.661 2716.99 910 v -524.651 2981.77 910 v -524.651 2981.77 910 v -224.661 2716.99 910 v -218.234 2724.65 910 v -224.661 2716.99 710 v -531.079 2974.11 710 v -218.234 2724.65 710 v -218.234 2724.65 710 v -531.079 2974.11 710 v -524.651 2981.77 710 v -224.661 2716.99 910 v -224.661 2716.99 710 v -218.234 2724.65 910 v -218.234 2724.65 910 v -224.661 2716.99 710 v -218.234 2724.65 710 v -531.079 2974.11 710 v -531.079 2974.11 910 v -524.651 2981.77 710 v -524.651 2981.77 710 v -531.079 2974.11 910 v -524.651 2981.77 910 v -524.651 2981.77 710 v -524.651 2981.77 910 v -218.234 2724.65 710 v -218.234 2724.65 710 v -524.651 2981.77 910 v -218.234 2724.65 910 v -531.079 2974.11 710 v -224.661 2716.99 710 v -531.079 2974.11 910 v -531.079 2974.11 910 v -224.661 2716.99 710 v -224.661 2716.99 910 v -1281.01 2712.06 495 v -1279.14 2715.48 495 v -1281.01 2712.06 5 v -1281.01 2712.06 5 v -1279.14 2715.48 495 v -1279.14 2715.48 5 v -1279.14 2715.48 495 v -1278.71 2719.36 495 v -1279.14 2715.48 5 v -1279.14 2715.48 5 v -1278.71 2719.36 495 v -1278.71 2719.36 5 v -1278.71 2719.36 495 v -1279.81 2723.11 495 v -1278.71 2719.36 5 v -1278.71 2719.36 5 v -1279.81 2723.11 495 v -1279.81 2723.11 5 v -1279.81 2723.11 495 v -1282.25 2726.15 495 v -1279.81 2723.11 5 v -1279.81 2723.11 5 v -1282.25 2726.15 495 v -1282.25 2726.15 5 v -1282.25 2726.15 495 v -1285.67 2728.03 495 v -1282.25 2726.15 5 v -1282.25 2726.15 5 v -1285.67 2728.03 495 v -1285.67 2728.03 5 v -1285.67 2728.03 495 v -1289.55 2728.45 495 v -1285.67 2728.03 5 v -1285.67 2728.03 5 v -1289.55 2728.45 495 v -1289.55 2728.45 5 v -1289.55 2728.45 495 v -1293.29 2727.36 495 v -1289.55 2728.45 5 v -1289.55 2728.45 5 v -1293.29 2727.36 495 v -1293.29 2727.36 5 v -1293.29 2727.36 495 v -1296.34 2724.92 495 v -1293.29 2727.36 5 v -1293.29 2727.36 5 v -1296.34 2724.92 495 v -1296.34 2724.92 5 v -1296.34 2724.92 495 v -1298.21 2721.5 495 v -1296.34 2724.92 5 v -1296.34 2724.92 5 v -1298.21 2721.5 495 v -1298.21 2721.5 5 v -1298.21 2721.5 495 v -1298.64 2717.62 495 v -1298.21 2721.5 5 v -1298.21 2721.5 5 v -1298.64 2717.62 495 v -1298.64 2717.62 5 v -1298.64 2717.62 495 v -1297.55 2713.87 495 v -1298.64 2717.62 5 v -1298.64 2717.62 5 v -1297.55 2713.87 495 v -1297.55 2713.87 5 v -1297.55 2713.87 495 v -1295.1 2710.83 495 v -1297.55 2713.87 5 v -1297.55 2713.87 5 v -1295.1 2710.83 495 v -1295.1 2710.83 5 v -1295.1 2710.83 495 v -1291.68 2708.95 495 v -1295.1 2710.83 5 v -1295.1 2710.83 5 v -1291.68 2708.95 495 v -1291.68 2708.95 5 v -1291.68 2708.95 495 v -1287.8 2708.53 495 v -1291.68 2708.95 5 v -1291.68 2708.95 5 v -1287.8 2708.53 495 v -1287.8 2708.53 5 v -1287.8 2708.53 495 v -1284.06 2709.62 495 v -1287.8 2708.53 5 v -1287.8 2708.53 5 v -1284.06 2709.62 495 v -1284.06 2709.62 5 v -1284.06 2709.62 495 v -1281.01 2712.06 495 v -1284.06 2709.62 5 v -1284.06 2709.62 5 v -1281.01 2712.06 495 v -1281.01 2712.06 5 v -1510.83 2904.9 495 v -1508.95 2908.32 495 v -1510.83 2904.9 5 v -1510.83 2904.9 5 v -1508.95 2908.32 495 v -1508.95 2908.32 5 v -1508.95 2908.32 495 v -1508.53 2912.2 495 v -1508.95 2908.32 5 v -1508.95 2908.32 5 v -1508.53 2912.2 495 v -1508.53 2912.2 5 v -1508.53 2912.2 495 v -1509.62 2915.94 495 v -1508.53 2912.2 5 v -1508.53 2912.2 5 v -1509.62 2915.94 495 v -1509.62 2915.94 5 v -1509.62 2915.94 495 v -1512.06 2918.99 495 v -1509.62 2915.94 5 v -1509.62 2915.94 5 v -1512.06 2918.99 495 v -1512.06 2918.99 5 v -1512.06 2918.99 495 v -1515.48 2920.86 495 v -1512.06 2918.99 5 v -1512.06 2918.99 5 v -1515.48 2920.86 495 v -1515.48 2920.86 5 v -1515.48 2920.86 495 v -1519.36 2921.29 495 v -1515.48 2920.86 5 v -1515.48 2920.86 5 v -1519.36 2921.29 495 v -1519.36 2921.29 5 v -1519.36 2921.29 495 v -1523.11 2920.19 495 v -1519.36 2921.29 5 v -1519.36 2921.29 5 v -1523.11 2920.19 495 v -1523.11 2920.19 5 v -1523.11 2920.19 495 v -1526.15 2917.75 495 v -1523.11 2920.19 5 v -1523.11 2920.19 5 v -1526.15 2917.75 495 v -1526.15 2917.75 5 v -1526.15 2917.75 495 v -1528.03 2914.33 495 v -1526.15 2917.75 5 v -1526.15 2917.75 5 v -1528.03 2914.33 495 v -1528.03 2914.33 5 v -1528.03 2914.33 495 v -1528.45 2910.45 495 v -1528.03 2914.33 5 v -1528.03 2914.33 5 v -1528.45 2910.45 495 v -1528.45 2910.45 5 v -1528.45 2910.45 495 v -1527.36 2906.71 495 v -1528.45 2910.45 5 v -1528.45 2910.45 5 v -1527.36 2906.71 495 v -1527.36 2906.71 5 v -1527.36 2906.71 495 v -1524.92 2903.66 495 v -1527.36 2906.71 5 v -1527.36 2906.71 5 v -1524.92 2903.66 495 v -1524.92 2903.66 5 v -1524.92 2903.66 495 v -1521.5 2901.79 495 v -1524.92 2903.66 5 v -1524.92 2903.66 5 v -1521.5 2901.79 495 v -1521.5 2901.79 5 v -1521.5 2901.79 495 v -1517.62 2901.36 495 v -1521.5 2901.79 5 v -1521.5 2901.79 5 v -1517.62 2901.36 495 v -1517.62 2901.36 5 v -1517.62 2901.36 495 v -1513.87 2902.45 495 v -1517.62 2901.36 5 v -1517.62 2901.36 5 v -1513.87 2902.45 495 v -1513.87 2902.45 5 v -1513.87 2902.45 495 v -1510.83 2904.9 495 v -1513.87 2902.45 5 v -1513.87 2902.45 5 v -1510.83 2904.9 495 v -1510.83 2904.9 5 v -1703.66 2675.08 495 v -1701.79 2678.5 495 v -1703.66 2675.08 5 v -1703.66 2675.08 5 v -1701.79 2678.5 495 v -1701.79 2678.5 5 v -1701.79 2678.5 495 v -1701.36 2682.38 495 v -1701.79 2678.5 5 v -1701.79 2678.5 5 v -1701.36 2682.38 495 v -1701.36 2682.38 5 v -1701.36 2682.38 495 v -1702.45 2686.13 495 v -1701.36 2682.38 5 v -1701.36 2682.38 5 v -1702.45 2686.13 495 v -1702.45 2686.13 5 v -1702.45 2686.13 495 v -1704.9 2689.17 495 v -1702.45 2686.13 5 v -1702.45 2686.13 5 v -1704.9 2689.17 495 v -1704.9 2689.17 5 v -1704.9 2689.17 495 v -1708.32 2691.05 495 v -1704.9 2689.17 5 v -1704.9 2689.17 5 v -1708.32 2691.05 495 v -1708.32 2691.05 5 v -1708.32 2691.05 495 v -1712.2 2691.47 495 v -1708.32 2691.05 5 v -1708.32 2691.05 5 v -1712.2 2691.47 495 v -1712.2 2691.47 5 v -1712.2 2691.47 495 v -1715.94 2690.38 495 v -1712.2 2691.47 5 v -1712.2 2691.47 5 v -1715.94 2690.38 495 v -1715.94 2690.38 5 v -1715.94 2690.38 495 v -1718.99 2687.94 495 v -1715.94 2690.38 5 v -1715.94 2690.38 5 v -1718.99 2687.94 495 v -1718.99 2687.94 5 v -1718.99 2687.94 495 v -1720.86 2684.52 495 v -1718.99 2687.94 5 v -1718.99 2687.94 5 v -1720.86 2684.52 495 v -1720.86 2684.52 5 v -1720.86 2684.52 495 v -1721.29 2680.64 495 v -1720.86 2684.52 5 v -1720.86 2684.52 5 v -1721.29 2680.64 495 v -1721.29 2680.64 5 v -1721.29 2680.64 495 v -1720.19 2676.89 495 v -1721.29 2680.64 5 v -1721.29 2680.64 5 v -1720.19 2676.89 495 v -1720.19 2676.89 5 v -1720.19 2676.89 495 v -1717.75 2673.85 495 v -1720.19 2676.89 5 v -1720.19 2676.89 5 v -1717.75 2673.85 495 v -1717.75 2673.85 5 v -1717.75 2673.85 495 v -1714.33 2671.97 495 v -1717.75 2673.85 5 v -1717.75 2673.85 5 v -1714.33 2671.97 495 v -1714.33 2671.97 5 v -1714.33 2671.97 495 v -1710.45 2671.55 495 v -1714.33 2671.97 5 v -1714.33 2671.97 5 v -1710.45 2671.55 495 v -1710.45 2671.55 5 v -1710.45 2671.55 495 v -1706.71 2672.64 495 v -1710.45 2671.55 5 v -1710.45 2671.55 5 v -1706.71 2672.64 495 v -1706.71 2672.64 5 v -1706.71 2672.64 495 v -1703.66 2675.08 495 v -1706.71 2672.64 5 v -1706.71 2672.64 5 v -1703.66 2675.08 495 v -1703.66 2675.08 5 v -1473.85 2482.25 495 v -1471.97 2485.67 495 v -1473.85 2482.25 5 v -1473.85 2482.25 5 v -1471.97 2485.67 495 v -1471.97 2485.67 5 v -1471.97 2485.67 495 v -1471.55 2489.55 495 v -1471.97 2485.67 5 v -1471.97 2485.67 5 v -1471.55 2489.55 495 v -1471.55 2489.55 5 v -1471.55 2489.55 495 v -1472.64 2493.29 495 v -1471.55 2489.55 5 v -1471.55 2489.55 5 v -1472.64 2493.29 495 v -1472.64 2493.29 5 v -1472.64 2493.29 495 v -1475.08 2496.34 495 v -1472.64 2493.29 5 v -1472.64 2493.29 5 v -1475.08 2496.34 495 v -1475.08 2496.34 5 v -1475.08 2496.34 495 v -1478.5 2498.21 495 v -1475.08 2496.34 5 v -1475.08 2496.34 5 v -1478.5 2498.21 495 v -1478.5 2498.21 5 v -1478.5 2498.21 495 v -1482.38 2498.64 495 v -1478.5 2498.21 5 v -1478.5 2498.21 5 v -1482.38 2498.64 495 v -1482.38 2498.64 5 v -1482.38 2498.64 495 v -1486.13 2497.55 495 v -1482.38 2498.64 5 v -1482.38 2498.64 5 v -1486.13 2497.55 495 v -1486.13 2497.55 5 v -1486.13 2497.55 495 v -1489.17 2495.1 495 v -1486.13 2497.55 5 v -1486.13 2497.55 5 v -1489.17 2495.1 495 v -1489.17 2495.1 5 v -1489.17 2495.1 495 v -1491.05 2491.68 495 v -1489.17 2495.1 5 v -1489.17 2495.1 5 v -1491.05 2491.68 495 v -1491.05 2491.68 5 v -1491.05 2491.68 495 v -1491.47 2487.8 495 v -1491.05 2491.68 5 v -1491.05 2491.68 5 v -1491.47 2487.8 495 v -1491.47 2487.8 5 v -1491.47 2487.8 495 v -1490.38 2484.06 495 v -1491.47 2487.8 5 v -1491.47 2487.8 5 v -1490.38 2484.06 495 v -1490.38 2484.06 5 v -1490.38 2484.06 495 v -1487.94 2481.01 495 v -1490.38 2484.06 5 v -1490.38 2484.06 5 v -1487.94 2481.01 495 v -1487.94 2481.01 5 v -1487.94 2481.01 495 v -1484.52 2479.14 495 v -1487.94 2481.01 5 v -1487.94 2481.01 5 v -1484.52 2479.14 495 v -1484.52 2479.14 5 v -1484.52 2479.14 495 v -1480.64 2478.71 495 v -1484.52 2479.14 5 v -1484.52 2479.14 5 v -1480.64 2478.71 495 v -1480.64 2478.71 5 v -1480.64 2478.71 495 v -1476.89 2479.81 495 v -1480.64 2478.71 5 v -1480.64 2478.71 5 v -1476.89 2479.81 495 v -1476.89 2479.81 5 v -1476.89 2479.81 495 v -1473.85 2482.25 495 v -1476.89 2479.81 5 v -1476.89 2479.81 5 v -1473.85 2482.25 495 v -1473.85 2482.25 5 v -1781.77 2675.35 510 v -1475.35 2418.23 510 v -1524.65 2981.77 510 v -1524.65 2981.77 510 v -1475.35 2418.23 510 v -1218.23 2724.65 510 v -1475.35 2418.23 500 v -1781.77 2675.35 500 v -1218.23 2724.65 500 v -1218.23 2724.65 500 v -1781.77 2675.35 500 v -1524.65 2981.77 500 v -1475.35 2418.23 510 v -1475.35 2418.23 500 v -1218.23 2724.65 510 v -1218.23 2724.65 510 v -1475.35 2418.23 500 v -1218.23 2724.65 500 v -1781.77 2675.35 500 v -1781.77 2675.35 510 v -1524.65 2981.77 500 v -1524.65 2981.77 500 v -1781.77 2675.35 510 v -1524.65 2981.77 510 v -1524.65 2981.77 500 v -1524.65 2981.77 510 v -1218.23 2724.65 500 v -1218.23 2724.65 500 v -1524.65 2981.77 510 v -1218.23 2724.65 510 v -1781.77 2675.35 500 v -1475.35 2418.23 500 v -1781.77 2675.35 510 v -1781.77 2675.35 510 v -1475.35 2418.23 500 v -1475.35 2418.23 510 v -1322.27 2818.47 810 v -1320.39 2821.89 810 v -1322.27 2818.47 510 v -1322.27 2818.47 510 v -1320.39 2821.89 810 v -1320.39 2821.89 510 v -1320.39 2821.89 810 v -1319.96 2825.77 810 v -1320.39 2821.89 510 v -1320.39 2821.89 510 v -1319.96 2825.77 810 v -1319.96 2825.77 510 v -1319.96 2825.77 810 v -1321.06 2829.52 810 v -1319.96 2825.77 510 v -1319.96 2825.77 510 v -1321.06 2829.52 810 v -1321.06 2829.52 510 v -1321.06 2829.52 810 v -1323.5 2832.56 810 v -1321.06 2829.52 510 v -1321.06 2829.52 510 v -1323.5 2832.56 810 v -1323.5 2832.56 510 v -1323.5 2832.56 810 v -1326.92 2834.44 810 v -1323.5 2832.56 510 v -1323.5 2832.56 510 v -1326.92 2834.44 810 v -1326.92 2834.44 510 v -1326.92 2834.44 810 v -1330.8 2834.86 810 v -1326.92 2834.44 510 v -1326.92 2834.44 510 v -1330.8 2834.86 810 v -1330.8 2834.86 510 v -1330.8 2834.86 810 v -1334.54 2833.77 810 v -1330.8 2834.86 510 v -1330.8 2834.86 510 v -1334.54 2833.77 810 v -1334.54 2833.77 510 v -1334.54 2833.77 810 v -1337.59 2831.33 810 v -1334.54 2833.77 510 v -1334.54 2833.77 510 v -1337.59 2831.33 810 v -1337.59 2831.33 510 v -1337.59 2831.33 810 v -1339.46 2827.91 810 v -1337.59 2831.33 510 v -1337.59 2831.33 510 v -1339.46 2827.91 810 v -1339.46 2827.91 510 v -1339.46 2827.91 810 v -1339.89 2824.03 810 v -1339.46 2827.91 510 v -1339.46 2827.91 510 v -1339.89 2824.03 810 v -1339.89 2824.03 510 v -1339.89 2824.03 810 v -1338.8 2820.28 810 v -1339.89 2824.03 510 v -1339.89 2824.03 510 v -1338.8 2820.28 810 v -1338.8 2820.28 510 v -1338.8 2820.28 810 v -1336.35 2817.24 810 v -1338.8 2820.28 510 v -1338.8 2820.28 510 v -1336.35 2817.24 810 v -1336.35 2817.24 510 v -1336.35 2817.24 810 v -1332.93 2815.36 810 v -1336.35 2817.24 510 v -1336.35 2817.24 510 v -1332.93 2815.36 810 v -1332.93 2815.36 510 v -1332.93 2815.36 810 v -1329.05 2814.94 810 v -1332.93 2815.36 510 v -1332.93 2815.36 510 v -1329.05 2814.94 810 v -1329.05 2814.94 510 v -1329.05 2814.94 810 v -1325.31 2816.03 810 v -1329.05 2814.94 510 v -1329.05 2814.94 510 v -1325.31 2816.03 810 v -1325.31 2816.03 510 v -1325.31 2816.03 810 v -1322.27 2818.47 810 v -1325.31 2816.03 510 v -1325.31 2816.03 510 v -1322.27 2818.47 810 v -1322.27 2818.47 510 v -1398.87 2882.75 810 v -1396.99 2886.17 810 v -1398.87 2882.75 510 v -1398.87 2882.75 510 v -1396.99 2886.17 810 v -1396.99 2886.17 510 v -1396.99 2886.17 810 v -1396.57 2890.05 810 v -1396.99 2886.17 510 v -1396.99 2886.17 510 v -1396.57 2890.05 810 v -1396.57 2890.05 510 v -1396.57 2890.05 810 v -1397.66 2893.8 810 v -1396.57 2890.05 510 v -1396.57 2890.05 510 v -1397.66 2893.8 810 v -1397.66 2893.8 510 v -1397.66 2893.8 810 v -1400.1 2896.84 810 v -1397.66 2893.8 510 v -1397.66 2893.8 510 v -1400.1 2896.84 810 v -1400.1 2896.84 510 v -1400.1 2896.84 810 v -1403.52 2898.72 810 v -1400.1 2896.84 510 v -1400.1 2896.84 510 v -1403.52 2898.72 810 v -1403.52 2898.72 510 v -1403.52 2898.72 810 v -1407.4 2899.14 810 v -1403.52 2898.72 510 v -1403.52 2898.72 510 v -1407.4 2899.14 810 v -1407.4 2899.14 510 v -1407.4 2899.14 810 v -1411.15 2898.05 810 v -1407.4 2899.14 510 v -1407.4 2899.14 510 v -1411.15 2898.05 810 v -1411.15 2898.05 510 v -1411.15 2898.05 810 v -1414.19 2895.61 810 v -1411.15 2898.05 510 v -1411.15 2898.05 510 v -1414.19 2895.61 810 v -1414.19 2895.61 510 v -1414.19 2895.61 810 v -1416.07 2892.19 810 v -1414.19 2895.61 510 v -1414.19 2895.61 510 v -1416.07 2892.19 810 v -1416.07 2892.19 510 v -1416.07 2892.19 810 v -1416.49 2888.31 810 v -1416.07 2892.19 510 v -1416.07 2892.19 510 v -1416.49 2888.31 810 v -1416.49 2888.31 510 v -1416.49 2888.31 810 v -1415.4 2884.56 810 v -1416.49 2888.31 510 v -1416.49 2888.31 510 v -1415.4 2884.56 810 v -1415.4 2884.56 510 v -1415.4 2884.56 810 v -1412.96 2881.52 810 v -1415.4 2884.56 510 v -1415.4 2884.56 510 v -1412.96 2881.52 810 v -1412.96 2881.52 510 v -1412.96 2881.52 810 v -1409.54 2879.64 810 v -1412.96 2881.52 510 v -1412.96 2881.52 510 v -1409.54 2879.64 810 v -1409.54 2879.64 510 v -1409.54 2879.64 810 v -1405.66 2879.22 810 v -1409.54 2879.64 510 v -1409.54 2879.64 510 v -1405.66 2879.22 810 v -1405.66 2879.22 510 v -1405.66 2879.22 810 v -1401.91 2880.31 810 v -1405.66 2879.22 510 v -1405.66 2879.22 510 v -1401.91 2880.31 810 v -1401.91 2880.31 510 v -1401.91 2880.31 810 v -1398.87 2882.75 810 v -1401.91 2880.31 510 v -1401.91 2880.31 510 v -1398.87 2882.75 810 v -1398.87 2882.75 510 v -1531.08 2974.11 910 v -1224.66 2716.99 910 v -1524.65 2981.77 910 v -1524.65 2981.77 910 v -1224.66 2716.99 910 v -1218.23 2724.65 910 v -1224.66 2716.99 710 v -1531.08 2974.11 710 v -1218.23 2724.65 710 v -1218.23 2724.65 710 v -1531.08 2974.11 710 v -1524.65 2981.77 710 v -1224.66 2716.99 910 v -1224.66 2716.99 710 v -1218.23 2724.65 910 v -1218.23 2724.65 910 v -1224.66 2716.99 710 v -1218.23 2724.65 710 v -1531.08 2974.11 710 v -1531.08 2974.11 910 v -1524.65 2981.77 710 v -1524.65 2981.77 710 v -1531.08 2974.11 910 v -1524.65 2981.77 910 v -1524.65 2981.77 710 v -1524.65 2981.77 910 v -1218.23 2724.65 710 v -1218.23 2724.65 710 v -1524.65 2981.77 910 v -1218.23 2724.65 910 v -1531.08 2974.11 710 v -1224.66 2716.99 710 v -1531.08 2974.11 910 v -1531.08 2974.11 910 v -1224.66 2716.99 710 v -1224.66 2716.99 910 v -2281.01 2712.06 495 v -2279.14 2715.48 495 v -2281.01 2712.06 5 v -2281.01 2712.06 5 v -2279.14 2715.48 495 v -2279.14 2715.48 5 v -2279.14 2715.48 495 v -2278.71 2719.36 495 v -2279.14 2715.48 5 v -2279.14 2715.48 5 v -2278.71 2719.36 495 v -2278.71 2719.36 5 v -2278.71 2719.36 495 v -2279.81 2723.11 495 v -2278.71 2719.36 5 v -2278.71 2719.36 5 v -2279.81 2723.11 495 v -2279.81 2723.11 5 v -2279.81 2723.11 495 v -2282.25 2726.15 495 v -2279.81 2723.11 5 v -2279.81 2723.11 5 v -2282.25 2726.15 495 v -2282.25 2726.15 5 v -2282.25 2726.15 495 v -2285.67 2728.03 495 v -2282.25 2726.15 5 v -2282.25 2726.15 5 v -2285.67 2728.03 495 v -2285.67 2728.03 5 v -2285.67 2728.03 495 v -2289.55 2728.45 495 v -2285.67 2728.03 5 v -2285.67 2728.03 5 v -2289.55 2728.45 495 v -2289.55 2728.45 5 v -2289.55 2728.45 495 v -2293.29 2727.36 495 v -2289.55 2728.45 5 v -2289.55 2728.45 5 v -2293.29 2727.36 495 v -2293.29 2727.36 5 v -2293.29 2727.36 495 v -2296.34 2724.92 495 v -2293.29 2727.36 5 v -2293.29 2727.36 5 v -2296.34 2724.92 495 v -2296.34 2724.92 5 v -2296.34 2724.92 495 v -2298.21 2721.5 495 v -2296.34 2724.92 5 v -2296.34 2724.92 5 v -2298.21 2721.5 495 v -2298.21 2721.5 5 v -2298.21 2721.5 495 v -2298.64 2717.62 495 v -2298.21 2721.5 5 v -2298.21 2721.5 5 v -2298.64 2717.62 495 v -2298.64 2717.62 5 v -2298.64 2717.62 495 v -2297.55 2713.87 495 v -2298.64 2717.62 5 v -2298.64 2717.62 5 v -2297.55 2713.87 495 v -2297.55 2713.87 5 v -2297.55 2713.87 495 v -2295.1 2710.83 495 v -2297.55 2713.87 5 v -2297.55 2713.87 5 v -2295.1 2710.83 495 v -2295.1 2710.83 5 v -2295.1 2710.83 495 v -2291.68 2708.95 495 v -2295.1 2710.83 5 v -2295.1 2710.83 5 v -2291.68 2708.95 495 v -2291.68 2708.95 5 v -2291.68 2708.95 495 v -2287.8 2708.53 495 v -2291.68 2708.95 5 v -2291.68 2708.95 5 v -2287.8 2708.53 495 v -2287.8 2708.53 5 v -2287.8 2708.53 495 v -2284.06 2709.62 495 v -2287.8 2708.53 5 v -2287.8 2708.53 5 v -2284.06 2709.62 495 v -2284.06 2709.62 5 v -2284.06 2709.62 495 v -2281.01 2712.06 495 v -2284.06 2709.62 5 v -2284.06 2709.62 5 v -2281.01 2712.06 495 v -2281.01 2712.06 5 v -2510.83 2904.9 495 v -2508.95 2908.32 495 v -2510.83 2904.9 5 v -2510.83 2904.9 5 v -2508.95 2908.32 495 v -2508.95 2908.32 5 v -2508.95 2908.32 495 v -2508.53 2912.2 495 v -2508.95 2908.32 5 v -2508.95 2908.32 5 v -2508.53 2912.2 495 v -2508.53 2912.2 5 v -2508.53 2912.2 495 v -2509.62 2915.94 495 v -2508.53 2912.2 5 v -2508.53 2912.2 5 v -2509.62 2915.94 495 v -2509.62 2915.94 5 v -2509.62 2915.94 495 v -2512.06 2918.99 495 v -2509.62 2915.94 5 v -2509.62 2915.94 5 v -2512.06 2918.99 495 v -2512.06 2918.99 5 v -2512.06 2918.99 495 v -2515.48 2920.86 495 v -2512.06 2918.99 5 v -2512.06 2918.99 5 v -2515.48 2920.86 495 v -2515.48 2920.86 5 v -2515.48 2920.86 495 v -2519.36 2921.29 495 v -2515.48 2920.86 5 v -2515.48 2920.86 5 v -2519.36 2921.29 495 v -2519.36 2921.29 5 v -2519.36 2921.29 495 v -2523.11 2920.19 495 v -2519.36 2921.29 5 v -2519.36 2921.29 5 v -2523.11 2920.19 495 v -2523.11 2920.19 5 v -2523.11 2920.19 495 v -2526.15 2917.75 495 v -2523.11 2920.19 5 v -2523.11 2920.19 5 v -2526.15 2917.75 495 v -2526.15 2917.75 5 v -2526.15 2917.75 495 v -2528.03 2914.33 495 v -2526.15 2917.75 5 v -2526.15 2917.75 5 v -2528.03 2914.33 495 v -2528.03 2914.33 5 v -2528.03 2914.33 495 v -2528.45 2910.45 495 v -2528.03 2914.33 5 v -2528.03 2914.33 5 v -2528.45 2910.45 495 v -2528.45 2910.45 5 v -2528.45 2910.45 495 v -2527.36 2906.71 495 v -2528.45 2910.45 5 v -2528.45 2910.45 5 v -2527.36 2906.71 495 v -2527.36 2906.71 5 v -2527.36 2906.71 495 v -2524.92 2903.66 495 v -2527.36 2906.71 5 v -2527.36 2906.71 5 v -2524.92 2903.66 495 v -2524.92 2903.66 5 v -2524.92 2903.66 495 v -2521.5 2901.79 495 v -2524.92 2903.66 5 v -2524.92 2903.66 5 v -2521.5 2901.79 495 v -2521.5 2901.79 5 v -2521.5 2901.79 495 v -2517.62 2901.36 495 v -2521.5 2901.79 5 v -2521.5 2901.79 5 v -2517.62 2901.36 495 v -2517.62 2901.36 5 v -2517.62 2901.36 495 v -2513.87 2902.45 495 v -2517.62 2901.36 5 v -2517.62 2901.36 5 v -2513.87 2902.45 495 v -2513.87 2902.45 5 v -2513.87 2902.45 495 v -2510.83 2904.9 495 v -2513.87 2902.45 5 v -2513.87 2902.45 5 v -2510.83 2904.9 495 v -2510.83 2904.9 5 v -2703.66 2675.08 495 v -2701.79 2678.5 495 v -2703.66 2675.08 5 v -2703.66 2675.08 5 v -2701.79 2678.5 495 v -2701.79 2678.5 5 v -2701.79 2678.5 495 v -2701.36 2682.38 495 v -2701.79 2678.5 5 v -2701.79 2678.5 5 v -2701.36 2682.38 495 v -2701.36 2682.38 5 v -2701.36 2682.38 495 v -2702.45 2686.13 495 v -2701.36 2682.38 5 v -2701.36 2682.38 5 v -2702.45 2686.13 495 v -2702.45 2686.13 5 v -2702.45 2686.13 495 v -2704.9 2689.17 495 v -2702.45 2686.13 5 v -2702.45 2686.13 5 v -2704.9 2689.17 495 v -2704.9 2689.17 5 v -2704.9 2689.17 495 v -2708.32 2691.05 495 v -2704.9 2689.17 5 v -2704.9 2689.17 5 v -2708.32 2691.05 495 v -2708.32 2691.05 5 v -2708.32 2691.05 495 v -2712.2 2691.47 495 v -2708.32 2691.05 5 v -2708.32 2691.05 5 v -2712.2 2691.47 495 v -2712.2 2691.47 5 v -2712.2 2691.47 495 v -2715.94 2690.38 495 v -2712.2 2691.47 5 v -2712.2 2691.47 5 v -2715.94 2690.38 495 v -2715.94 2690.38 5 v -2715.94 2690.38 495 v -2718.99 2687.94 495 v -2715.94 2690.38 5 v -2715.94 2690.38 5 v -2718.99 2687.94 495 v -2718.99 2687.94 5 v -2718.99 2687.94 495 v -2720.86 2684.52 495 v -2718.99 2687.94 5 v -2718.99 2687.94 5 v -2720.86 2684.52 495 v -2720.86 2684.52 5 v -2720.86 2684.52 495 v -2721.29 2680.64 495 v -2720.86 2684.52 5 v -2720.86 2684.52 5 v -2721.29 2680.64 495 v -2721.29 2680.64 5 v -2721.29 2680.64 495 v -2720.19 2676.89 495 v -2721.29 2680.64 5 v -2721.29 2680.64 5 v -2720.19 2676.89 495 v -2720.19 2676.89 5 v -2720.19 2676.89 495 v -2717.75 2673.85 495 v -2720.19 2676.89 5 v -2720.19 2676.89 5 v -2717.75 2673.85 495 v -2717.75 2673.85 5 v -2717.75 2673.85 495 v -2714.33 2671.97 495 v -2717.75 2673.85 5 v -2717.75 2673.85 5 v -2714.33 2671.97 495 v -2714.33 2671.97 5 v -2714.33 2671.97 495 v -2710.45 2671.55 495 v -2714.33 2671.97 5 v -2714.33 2671.97 5 v -2710.45 2671.55 495 v -2710.45 2671.55 5 v -2710.45 2671.55 495 v -2706.71 2672.64 495 v -2710.45 2671.55 5 v -2710.45 2671.55 5 v -2706.71 2672.64 495 v -2706.71 2672.64 5 v -2706.71 2672.64 495 v -2703.66 2675.08 495 v -2706.71 2672.64 5 v -2706.71 2672.64 5 v -2703.66 2675.08 495 v -2703.66 2675.08 5 v -2473.85 2482.25 495 v -2471.97 2485.67 495 v -2473.85 2482.25 5 v -2473.85 2482.25 5 v -2471.97 2485.67 495 v -2471.97 2485.67 5 v -2471.97 2485.67 495 v -2471.55 2489.55 495 v -2471.97 2485.67 5 v -2471.97 2485.67 5 v -2471.55 2489.55 495 v -2471.55 2489.55 5 v -2471.55 2489.55 495 v -2472.64 2493.29 495 v -2471.55 2489.55 5 v -2471.55 2489.55 5 v -2472.64 2493.29 495 v -2472.64 2493.29 5 v -2472.64 2493.29 495 v -2475.08 2496.34 495 v -2472.64 2493.29 5 v -2472.64 2493.29 5 v -2475.08 2496.34 495 v -2475.08 2496.34 5 v -2475.08 2496.34 495 v -2478.5 2498.21 495 v -2475.08 2496.34 5 v -2475.08 2496.34 5 v -2478.5 2498.21 495 v -2478.5 2498.21 5 v -2478.5 2498.21 495 v -2482.38 2498.64 495 v -2478.5 2498.21 5 v -2478.5 2498.21 5 v -2482.38 2498.64 495 v -2482.38 2498.64 5 v -2482.38 2498.64 495 v -2486.13 2497.55 495 v -2482.38 2498.64 5 v -2482.38 2498.64 5 v -2486.13 2497.55 495 v -2486.13 2497.55 5 v -2486.13 2497.55 495 v -2489.17 2495.1 495 v -2486.13 2497.55 5 v -2486.13 2497.55 5 v -2489.17 2495.1 495 v -2489.17 2495.1 5 v -2489.17 2495.1 495 v -2491.05 2491.68 495 v -2489.17 2495.1 5 v -2489.17 2495.1 5 v -2491.05 2491.68 495 v -2491.05 2491.68 5 v -2491.05 2491.68 495 v -2491.47 2487.8 495 v -2491.05 2491.68 5 v -2491.05 2491.68 5 v -2491.47 2487.8 495 v -2491.47 2487.8 5 v -2491.47 2487.8 495 v -2490.38 2484.06 495 v -2491.47 2487.8 5 v -2491.47 2487.8 5 v -2490.38 2484.06 495 v -2490.38 2484.06 5 v -2490.38 2484.06 495 v -2487.94 2481.01 495 v -2490.38 2484.06 5 v -2490.38 2484.06 5 v -2487.94 2481.01 495 v -2487.94 2481.01 5 v -2487.94 2481.01 495 v -2484.52 2479.14 495 v -2487.94 2481.01 5 v -2487.94 2481.01 5 v -2484.52 2479.14 495 v -2484.52 2479.14 5 v -2484.52 2479.14 495 v -2480.64 2478.71 495 v -2484.52 2479.14 5 v -2484.52 2479.14 5 v -2480.64 2478.71 495 v -2480.64 2478.71 5 v -2480.64 2478.71 495 v -2476.89 2479.81 495 v -2480.64 2478.71 5 v -2480.64 2478.71 5 v -2476.89 2479.81 495 v -2476.89 2479.81 5 v -2476.89 2479.81 495 v -2473.85 2482.25 495 v -2476.89 2479.81 5 v -2476.89 2479.81 5 v -2473.85 2482.25 495 v -2473.85 2482.25 5 v -2781.77 2675.35 510 v -2475.35 2418.23 510 v -2524.65 2981.77 510 v -2524.65 2981.77 510 v -2475.35 2418.23 510 v -2218.23 2724.65 510 v -2475.35 2418.23 500 v -2781.77 2675.35 500 v -2218.23 2724.65 500 v -2218.23 2724.65 500 v -2781.77 2675.35 500 v -2524.65 2981.77 500 v -2475.35 2418.23 510 v -2475.35 2418.23 500 v -2218.23 2724.65 510 v -2218.23 2724.65 510 v -2475.35 2418.23 500 v -2218.23 2724.65 500 v -2781.77 2675.35 500 v -2781.77 2675.35 510 v -2524.65 2981.77 500 v -2524.65 2981.77 500 v -2781.77 2675.35 510 v -2524.65 2981.77 510 v -2524.65 2981.77 500 v -2524.65 2981.77 510 v -2218.23 2724.65 500 v -2218.23 2724.65 500 v -2524.65 2981.77 510 v -2218.23 2724.65 510 v -2781.77 2675.35 500 v -2475.35 2418.23 500 v -2781.77 2675.35 510 v -2781.77 2675.35 510 v -2475.35 2418.23 500 v -2475.35 2418.23 510 v -2322.27 2818.47 810 v -2320.39 2821.89 810 v -2322.27 2818.47 510 v -2322.27 2818.47 510 v -2320.39 2821.89 810 v -2320.39 2821.89 510 v -2320.39 2821.89 810 v -2319.96 2825.77 810 v -2320.39 2821.89 510 v -2320.39 2821.89 510 v -2319.96 2825.77 810 v -2319.96 2825.77 510 v -2319.96 2825.77 810 v -2321.06 2829.52 810 v -2319.96 2825.77 510 v -2319.96 2825.77 510 v -2321.06 2829.52 810 v -2321.06 2829.52 510 v -2321.06 2829.52 810 v -2323.5 2832.56 810 v -2321.06 2829.52 510 v -2321.06 2829.52 510 v -2323.5 2832.56 810 v -2323.5 2832.56 510 v -2323.5 2832.56 810 v -2326.92 2834.44 810 v -2323.5 2832.56 510 v -2323.5 2832.56 510 v -2326.92 2834.44 810 v -2326.92 2834.44 510 v -2326.92 2834.44 810 v -2330.8 2834.86 810 v -2326.92 2834.44 510 v -2326.92 2834.44 510 v -2330.8 2834.86 810 v -2330.8 2834.86 510 v -2330.8 2834.86 810 v -2334.54 2833.77 810 v -2330.8 2834.86 510 v -2330.8 2834.86 510 v -2334.54 2833.77 810 v -2334.54 2833.77 510 v -2334.54 2833.77 810 v -2337.59 2831.33 810 v -2334.54 2833.77 510 v -2334.54 2833.77 510 v -2337.59 2831.33 810 v -2337.59 2831.33 510 v -2337.59 2831.33 810 v -2339.46 2827.91 810 v -2337.59 2831.33 510 v -2337.59 2831.33 510 v -2339.46 2827.91 810 v -2339.46 2827.91 510 v -2339.46 2827.91 810 v -2339.89 2824.03 810 v -2339.46 2827.91 510 v -2339.46 2827.91 510 v -2339.89 2824.03 810 v -2339.89 2824.03 510 v -2339.89 2824.03 810 v -2338.8 2820.28 810 v -2339.89 2824.03 510 v -2339.89 2824.03 510 v -2338.8 2820.28 810 v -2338.8 2820.28 510 v -2338.8 2820.28 810 v -2336.35 2817.24 810 v -2338.8 2820.28 510 v -2338.8 2820.28 510 v -2336.35 2817.24 810 v -2336.35 2817.24 510 v -2336.35 2817.24 810 v -2332.93 2815.36 810 v -2336.35 2817.24 510 v -2336.35 2817.24 510 v -2332.93 2815.36 810 v -2332.93 2815.36 510 v -2332.93 2815.36 810 v -2329.05 2814.94 810 v -2332.93 2815.36 510 v -2332.93 2815.36 510 v -2329.05 2814.94 810 v -2329.05 2814.94 510 v -2329.05 2814.94 810 v -2325.31 2816.03 810 v -2329.05 2814.94 510 v -2329.05 2814.94 510 v -2325.31 2816.03 810 v -2325.31 2816.03 510 v -2325.31 2816.03 810 v -2322.27 2818.47 810 v -2325.31 2816.03 510 v -2325.31 2816.03 510 v -2322.27 2818.47 810 v -2322.27 2818.47 510 v -2398.87 2882.75 810 v -2396.99 2886.17 810 v -2398.87 2882.75 510 v -2398.87 2882.75 510 v -2396.99 2886.17 810 v -2396.99 2886.17 510 v -2396.99 2886.17 810 v -2396.57 2890.05 810 v -2396.99 2886.17 510 v -2396.99 2886.17 510 v -2396.57 2890.05 810 v -2396.57 2890.05 510 v -2396.57 2890.05 810 v -2397.66 2893.8 810 v -2396.57 2890.05 510 v -2396.57 2890.05 510 v -2397.66 2893.8 810 v -2397.66 2893.8 510 v -2397.66 2893.8 810 v -2400.1 2896.84 810 v -2397.66 2893.8 510 v -2397.66 2893.8 510 v -2400.1 2896.84 810 v -2400.1 2896.84 510 v -2400.1 2896.84 810 v -2403.52 2898.72 810 v -2400.1 2896.84 510 v -2400.1 2896.84 510 v -2403.52 2898.72 810 v -2403.52 2898.72 510 v -2403.52 2898.72 810 v -2407.4 2899.14 810 v -2403.52 2898.72 510 v -2403.52 2898.72 510 v -2407.4 2899.14 810 v -2407.4 2899.14 510 v -2407.4 2899.14 810 v -2411.15 2898.05 810 v -2407.4 2899.14 510 v -2407.4 2899.14 510 v -2411.15 2898.05 810 v -2411.15 2898.05 510 v -2411.15 2898.05 810 v -2414.19 2895.61 810 v -2411.15 2898.05 510 v -2411.15 2898.05 510 v -2414.19 2895.61 810 v -2414.19 2895.61 510 v -2414.19 2895.61 810 v -2416.07 2892.19 810 v -2414.19 2895.61 510 v -2414.19 2895.61 510 v -2416.07 2892.19 810 v -2416.07 2892.19 510 v -2416.07 2892.19 810 v -2416.49 2888.31 810 v -2416.07 2892.19 510 v -2416.07 2892.19 510 v -2416.49 2888.31 810 v -2416.49 2888.31 510 v -2416.49 2888.31 810 v -2415.4 2884.56 810 v -2416.49 2888.31 510 v -2416.49 2888.31 510 v -2415.4 2884.56 810 v -2415.4 2884.56 510 v -2415.4 2884.56 810 v -2412.96 2881.52 810 v -2415.4 2884.56 510 v -2415.4 2884.56 510 v -2412.96 2881.52 810 v -2412.96 2881.52 510 v -2412.96 2881.52 810 v -2409.54 2879.64 810 v -2412.96 2881.52 510 v -2412.96 2881.52 510 v -2409.54 2879.64 810 v -2409.54 2879.64 510 v -2409.54 2879.64 810 v -2405.66 2879.22 810 v -2409.54 2879.64 510 v -2409.54 2879.64 510 v -2405.66 2879.22 810 v -2405.66 2879.22 510 v -2405.66 2879.22 810 v -2401.91 2880.31 810 v -2405.66 2879.22 510 v -2405.66 2879.22 510 v -2401.91 2880.31 810 v -2401.91 2880.31 510 v -2401.91 2880.31 810 v -2398.87 2882.75 810 v -2401.91 2880.31 510 v -2401.91 2880.31 510 v -2398.87 2882.75 810 v -2398.87 2882.75 510 v -2531.08 2974.11 910 v -2224.66 2716.99 910 v -2524.65 2981.77 910 v -2524.65 2981.77 910 v -2224.66 2716.99 910 v -2218.23 2724.65 910 v -2224.66 2716.99 710 v -2531.08 2974.11 710 v -2218.23 2724.65 710 v -2218.23 2724.65 710 v -2531.08 2974.11 710 v -2524.65 2981.77 710 v -2224.66 2716.99 910 v -2224.66 2716.99 710 v -2218.23 2724.65 910 v -2218.23 2724.65 910 v -2224.66 2716.99 710 v -2218.23 2724.65 710 v -2531.08 2974.11 710 v -2531.08 2974.11 910 v -2524.65 2981.77 710 v -2524.65 2981.77 710 v -2531.08 2974.11 910 v -2524.65 2981.77 910 v -2524.65 2981.77 710 v -2524.65 2981.77 910 v -2218.23 2724.65 710 v -2218.23 2724.65 710 v -2524.65 2981.77 910 v -2218.23 2724.65 910 v -2531.08 2974.11 710 v -2224.66 2716.99 710 v -2531.08 2974.11 910 v -2531.08 2974.11 910 v -2224.66 2716.99 710 v -2224.66 2716.99 910 v -660 1150 495 v -659.239 1146.17 495 v -660 1150 5 v -660 1150 5 v -659.239 1146.17 495 v -659.239 1146.17 5 v -659.239 1146.17 495 v -657.071 1142.93 495 v -659.239 1146.17 5 v -659.239 1146.17 5 v -657.071 1142.93 495 v -657.071 1142.93 5 v -657.071 1142.93 495 v -653.827 1140.76 495 v -657.071 1142.93 5 v -657.071 1142.93 5 v -653.827 1140.76 495 v -653.827 1140.76 5 v -653.827 1140.76 495 v -650 1140 495 v -653.827 1140.76 5 v -653.827 1140.76 5 v -650 1140 495 v -650 1140 5 v -650 1140 495 v -646.173 1140.76 495 v -650 1140 5 v -650 1140 5 v -646.173 1140.76 495 v -646.173 1140.76 5 v -646.173 1140.76 495 v -642.929 1142.93 495 v -646.173 1140.76 5 v -646.173 1140.76 5 v -642.929 1142.93 495 v -642.929 1142.93 5 v -642.929 1142.93 495 v -640.761 1146.17 495 v -642.929 1142.93 5 v -642.929 1142.93 5 v -640.761 1146.17 495 v -640.761 1146.17 5 v -640.761 1146.17 495 v -640 1150 495 v -640.761 1146.17 5 v -640.761 1146.17 5 v -640 1150 495 v -640 1150 5 v -640 1150 495 v -640.761 1153.83 495 v -640 1150 5 v -640 1150 5 v -640.761 1153.83 495 v -640.761 1153.83 5 v -640.761 1153.83 495 v -642.929 1157.07 495 v -640.761 1153.83 5 v -640.761 1153.83 5 v -642.929 1157.07 495 v -642.929 1157.07 5 v -642.929 1157.07 495 v -646.173 1159.24 495 v -642.929 1157.07 5 v -642.929 1157.07 5 v -646.173 1159.24 495 v -646.173 1159.24 5 v -646.173 1159.24 495 v -650 1160 495 v -646.173 1159.24 5 v -646.173 1159.24 5 v -650 1160 495 v -650 1160 5 v -650 1160 495 v -653.827 1159.24 495 v -650 1160 5 v -650 1160 5 v -653.827 1159.24 495 v -653.827 1159.24 5 v -653.827 1159.24 495 v -657.071 1157.07 495 v -653.827 1159.24 5 v -653.827 1159.24 5 v -657.071 1157.07 495 v -657.071 1157.07 5 v -657.071 1157.07 495 v -659.239 1153.83 495 v -657.071 1157.07 5 v -657.071 1157.07 5 v -659.239 1153.83 495 v -659.239 1153.83 5 v -659.239 1153.83 495 v -660 1150 495 v -659.239 1153.83 5 v -659.239 1153.83 5 v -660 1150 495 v -660 1150 5 v -360 1150 495 v -359.239 1146.17 495 v -360 1150 5 v -360 1150 5 v -359.239 1146.17 495 v -359.239 1146.17 5 v -359.239 1146.17 495 v -357.071 1142.93 495 v -359.239 1146.17 5 v -359.239 1146.17 5 v -357.071 1142.93 495 v -357.071 1142.93 5 v -357.071 1142.93 495 v -353.827 1140.76 495 v -357.071 1142.93 5 v -357.071 1142.93 5 v -353.827 1140.76 495 v -353.827 1140.76 5 v -353.827 1140.76 495 v -350 1140 495 v -353.827 1140.76 5 v -353.827 1140.76 5 v -350 1140 495 v -350 1140 5 v -350 1140 495 v -346.173 1140.76 495 v -350 1140 5 v -350 1140 5 v -346.173 1140.76 495 v -346.173 1140.76 5 v -346.173 1140.76 495 v -342.929 1142.93 495 v -346.173 1140.76 5 v -346.173 1140.76 5 v -342.929 1142.93 495 v -342.929 1142.93 5 v -342.929 1142.93 495 v -340.761 1146.17 495 v -342.929 1142.93 5 v -342.929 1142.93 5 v -340.761 1146.17 495 v -340.761 1146.17 5 v -340.761 1146.17 495 v -340 1150 495 v -340.761 1146.17 5 v -340.761 1146.17 5 v -340 1150 495 v -340 1150 5 v -340 1150 495 v -340.761 1153.83 495 v -340 1150 5 v -340 1150 5 v -340.761 1153.83 495 v -340.761 1153.83 5 v -340.761 1153.83 495 v -342.929 1157.07 495 v -340.761 1153.83 5 v -340.761 1153.83 5 v -342.929 1157.07 495 v -342.929 1157.07 5 v -342.929 1157.07 495 v -346.173 1159.24 495 v -342.929 1157.07 5 v -342.929 1157.07 5 v -346.173 1159.24 495 v -346.173 1159.24 5 v -346.173 1159.24 495 v -350 1160 495 v -346.173 1159.24 5 v -346.173 1159.24 5 v -350 1160 495 v -350 1160 5 v -350 1160 495 v -353.827 1159.24 495 v -350 1160 5 v -350 1160 5 v -353.827 1159.24 495 v -353.827 1159.24 5 v -353.827 1159.24 495 v -357.071 1157.07 495 v -353.827 1159.24 5 v -353.827 1159.24 5 v -357.071 1157.07 495 v -357.071 1157.07 5 v -357.071 1157.07 495 v -359.239 1153.83 495 v -357.071 1157.07 5 v -357.071 1157.07 5 v -359.239 1153.83 495 v -359.239 1153.83 5 v -359.239 1153.83 495 v -360 1150 495 v -359.239 1153.83 5 v -359.239 1153.83 5 v -360 1150 495 v -360 1150 5 v -360 1450 495 v -359.239 1446.17 495 v -360 1450 5 v -360 1450 5 v -359.239 1446.17 495 v -359.239 1446.17 5 v -359.239 1446.17 495 v -357.071 1442.93 495 v -359.239 1446.17 5 v -359.239 1446.17 5 v -357.071 1442.93 495 v -357.071 1442.93 5 v -357.071 1442.93 495 v -353.827 1440.76 495 v -357.071 1442.93 5 v -357.071 1442.93 5 v -353.827 1440.76 495 v -353.827 1440.76 5 v -353.827 1440.76 495 v -350 1440 495 v -353.827 1440.76 5 v -353.827 1440.76 5 v -350 1440 495 v -350 1440 5 v -350 1440 495 v -346.173 1440.76 495 v -350 1440 5 v -350 1440 5 v -346.173 1440.76 495 v -346.173 1440.76 5 v -346.173 1440.76 495 v -342.929 1442.93 495 v -346.173 1440.76 5 v -346.173 1440.76 5 v -342.929 1442.93 495 v -342.929 1442.93 5 v -342.929 1442.93 495 v -340.761 1446.17 495 v -342.929 1442.93 5 v -342.929 1442.93 5 v -340.761 1446.17 495 v -340.761 1446.17 5 v -340.761 1446.17 495 v -340 1450 495 v -340.761 1446.17 5 v -340.761 1446.17 5 v -340 1450 495 v -340 1450 5 v -340 1450 495 v -340.761 1453.83 495 v -340 1450 5 v -340 1450 5 v -340.761 1453.83 495 v -340.761 1453.83 5 v -340.761 1453.83 495 v -342.929 1457.07 495 v -340.761 1453.83 5 v -340.761 1453.83 5 v -342.929 1457.07 495 v -342.929 1457.07 5 v -342.929 1457.07 495 v -346.173 1459.24 495 v -342.929 1457.07 5 v -342.929 1457.07 5 v -346.173 1459.24 495 v -346.173 1459.24 5 v -346.173 1459.24 495 v -350 1460 495 v -346.173 1459.24 5 v -346.173 1459.24 5 v -350 1460 495 v -350 1460 5 v -350 1460 495 v -353.827 1459.24 495 v -350 1460 5 v -350 1460 5 v -353.827 1459.24 495 v -353.827 1459.24 5 v -353.827 1459.24 495 v -357.071 1457.07 495 v -353.827 1459.24 5 v -353.827 1459.24 5 v -357.071 1457.07 495 v -357.071 1457.07 5 v -357.071 1457.07 495 v -359.239 1453.83 495 v -357.071 1457.07 5 v -357.071 1457.07 5 v -359.239 1453.83 495 v -359.239 1453.83 5 v -359.239 1453.83 495 v -360 1450 495 v -359.239 1453.83 5 v -359.239 1453.83 5 v -360 1450 495 v -360 1450 5 v -660 1450 495 v -659.239 1446.17 495 v -660 1450 5 v -660 1450 5 v -659.239 1446.17 495 v -659.239 1446.17 5 v -659.239 1446.17 495 v -657.071 1442.93 495 v -659.239 1446.17 5 v -659.239 1446.17 5 v -657.071 1442.93 495 v -657.071 1442.93 5 v -657.071 1442.93 495 v -653.827 1440.76 495 v -657.071 1442.93 5 v -657.071 1442.93 5 v -653.827 1440.76 495 v -653.827 1440.76 5 v -653.827 1440.76 495 v -650 1440 495 v -653.827 1440.76 5 v -653.827 1440.76 5 v -650 1440 495 v -650 1440 5 v -650 1440 495 v -646.173 1440.76 495 v -650 1440 5 v -650 1440 5 v -646.173 1440.76 495 v -646.173 1440.76 5 v -646.173 1440.76 495 v -642.929 1442.93 495 v -646.173 1440.76 5 v -646.173 1440.76 5 v -642.929 1442.93 495 v -642.929 1442.93 5 v -642.929 1442.93 495 v -640.761 1446.17 495 v -642.929 1442.93 5 v -642.929 1442.93 5 v -640.761 1446.17 495 v -640.761 1446.17 5 v -640.761 1446.17 495 v -640 1450 495 v -640.761 1446.17 5 v -640.761 1446.17 5 v -640 1450 495 v -640 1450 5 v -640 1450 495 v -640.761 1453.83 495 v -640 1450 5 v -640 1450 5 v -640.761 1453.83 495 v -640.761 1453.83 5 v -640.761 1453.83 495 v -642.929 1457.07 495 v -640.761 1453.83 5 v -640.761 1453.83 5 v -642.929 1457.07 495 v -642.929 1457.07 5 v -642.929 1457.07 495 v -646.173 1459.24 495 v -642.929 1457.07 5 v -642.929 1457.07 5 v -646.173 1459.24 495 v -646.173 1459.24 5 v -646.173 1459.24 495 v -650 1460 495 v -646.173 1459.24 5 v -646.173 1459.24 5 v -650 1460 495 v -650 1460 5 v -650 1460 495 v -653.827 1459.24 495 v -650 1460 5 v -650 1460 5 v -653.827 1459.24 495 v -653.827 1459.24 5 v -653.827 1459.24 495 v -657.071 1457.07 495 v -653.827 1459.24 5 v -653.827 1459.24 5 v -657.071 1457.07 495 v -657.071 1457.07 5 v -657.071 1457.07 495 v -659.239 1453.83 495 v -657.071 1457.07 5 v -657.071 1457.07 5 v -659.239 1453.83 495 v -659.239 1453.83 5 v -659.239 1453.83 495 v -660 1450 495 v -659.239 1453.83 5 v -659.239 1453.83 5 v -660 1450 495 v -660 1450 5 v -300 1500 510 v -700 1500 510 v -300 1100 510 v -300 1100 510 v -700 1500 510 v -700 1100 510 v -700 1500 500 v -300 1500 500 v -700 1100 500 v -700 1100 500 v -300 1500 500 v -300 1100 500 v -700 1500 510 v -700 1500 500 v -700 1100 510 v -700 1100 510 v -700 1500 500 v -700 1100 500 v -300 1500 500 v -300 1500 510 v -300 1100 500 v -300 1100 500 v -300 1500 510 v -300 1100 510 v -300 1100 500 v -300 1100 510 v -700 1100 500 v -700 1100 500 v -300 1100 510 v -700 1100 510 v -300 1500 500 v -700 1500 500 v -300 1500 510 v -300 1500 510 v -700 1500 500 v -700 1500 510 v -560 1095 810 v -559.239 1091.17 810 v -560 1095 510 v -560 1095 510 v -559.239 1091.17 810 v -559.239 1091.17 510 v -559.239 1091.17 810 v -557.071 1087.93 810 v -559.239 1091.17 510 v -559.239 1091.17 510 v -557.071 1087.93 810 v -557.071 1087.93 510 v -557.071 1087.93 810 v -553.827 1085.76 810 v -557.071 1087.93 510 v -557.071 1087.93 510 v -553.827 1085.76 810 v -553.827 1085.76 510 v -553.827 1085.76 810 v -550 1085 810 v -553.827 1085.76 510 v -553.827 1085.76 510 v -550 1085 810 v -550 1085 510 v -550 1085 810 v -546.173 1085.76 810 v -550 1085 510 v -550 1085 510 v -546.173 1085.76 810 v -546.173 1085.76 510 v -546.173 1085.76 810 v -542.929 1087.93 810 v -546.173 1085.76 510 v -546.173 1085.76 510 v -542.929 1087.93 810 v -542.929 1087.93 510 v -542.929 1087.93 810 v -540.761 1091.17 810 v -542.929 1087.93 510 v -542.929 1087.93 510 v -540.761 1091.17 810 v -540.761 1091.17 510 v -540.761 1091.17 810 v -540 1095 810 v -540.761 1091.17 510 v -540.761 1091.17 510 v -540 1095 810 v -540 1095 510 v -540 1095 810 v -540.761 1098.83 810 v -540 1095 510 v -540 1095 510 v -540.761 1098.83 810 v -540.761 1098.83 510 v -540.761 1098.83 810 v -542.929 1102.07 810 v -540.761 1098.83 510 v -540.761 1098.83 510 v -542.929 1102.07 810 v -542.929 1102.07 510 v -542.929 1102.07 810 v -546.173 1104.24 810 v -542.929 1102.07 510 v -542.929 1102.07 510 v -546.173 1104.24 810 v -546.173 1104.24 510 v -546.173 1104.24 810 v -550 1105 810 v -546.173 1104.24 510 v -546.173 1104.24 510 v -550 1105 810 v -550 1105 510 v -550 1105 810 v -553.827 1104.24 810 v -550 1105 510 v -550 1105 510 v -553.827 1104.24 810 v -553.827 1104.24 510 v -553.827 1104.24 810 v -557.071 1102.07 810 v -553.827 1104.24 510 v -553.827 1104.24 510 v -557.071 1102.07 810 v -557.071 1102.07 510 v -557.071 1102.07 810 v -559.239 1098.83 810 v -557.071 1102.07 510 v -557.071 1102.07 510 v -559.239 1098.83 810 v -559.239 1098.83 510 v -559.239 1098.83 810 v -560 1095 810 v -559.239 1098.83 510 v -559.239 1098.83 510 v -560 1095 810 v -560 1095 510 v -460 1095 810 v -459.239 1091.17 810 v -460 1095 510 v -460 1095 510 v -459.239 1091.17 810 v -459.239 1091.17 510 v -459.239 1091.17 810 v -457.071 1087.93 810 v -459.239 1091.17 510 v -459.239 1091.17 510 v -457.071 1087.93 810 v -457.071 1087.93 510 v -457.071 1087.93 810 v -453.827 1085.76 810 v -457.071 1087.93 510 v -457.071 1087.93 510 v -453.827 1085.76 810 v -453.827 1085.76 510 v -453.827 1085.76 810 v -450 1085 810 v -453.827 1085.76 510 v -453.827 1085.76 510 v -450 1085 810 v -450 1085 510 v -450 1085 810 v -446.173 1085.76 810 v -450 1085 510 v -450 1085 510 v -446.173 1085.76 810 v -446.173 1085.76 510 v -446.173 1085.76 810 v -442.929 1087.93 810 v -446.173 1085.76 510 v -446.173 1085.76 510 v -442.929 1087.93 810 v -442.929 1087.93 510 v -442.929 1087.93 810 v -440.761 1091.17 810 v -442.929 1087.93 510 v -442.929 1087.93 510 v -440.761 1091.17 810 v -440.761 1091.17 510 v -440.761 1091.17 810 v -440 1095 810 v -440.761 1091.17 510 v -440.761 1091.17 510 v -440 1095 810 v -440 1095 510 v -440 1095 810 v -440.761 1098.83 810 v -440 1095 510 v -440 1095 510 v -440.761 1098.83 810 v -440.761 1098.83 510 v -440.761 1098.83 810 v -442.929 1102.07 810 v -440.761 1098.83 510 v -440.761 1098.83 510 v -442.929 1102.07 810 v -442.929 1102.07 510 v -442.929 1102.07 810 v -446.173 1104.24 810 v -442.929 1102.07 510 v -442.929 1102.07 510 v -446.173 1104.24 810 v -446.173 1104.24 510 v -446.173 1104.24 810 v -450 1105 810 v -446.173 1104.24 510 v -446.173 1104.24 510 v -450 1105 810 v -450 1105 510 v -450 1105 810 v -453.827 1104.24 810 v -450 1105 510 v -450 1105 510 v -453.827 1104.24 810 v -453.827 1104.24 510 v -453.827 1104.24 810 v -457.071 1102.07 810 v -453.827 1104.24 510 v -453.827 1104.24 510 v -457.071 1102.07 810 v -457.071 1102.07 510 v -457.071 1102.07 810 v -459.239 1098.83 810 v -457.071 1102.07 510 v -457.071 1102.07 510 v -459.239 1098.83 810 v -459.239 1098.83 510 v -459.239 1098.83 810 v -460 1095 810 v -459.239 1098.83 510 v -459.239 1098.83 510 v -460 1095 810 v -460 1095 510 v -300 1110 910 v -700 1110 910 v -300 1100 910 v -300 1100 910 v -700 1110 910 v -700 1100 910 v -700 1110 710 v -300 1110 710 v -700 1100 710 v -700 1100 710 v -300 1110 710 v -300 1100 710 v -700 1110 910 v -700 1110 710 v -700 1100 910 v -700 1100 910 v -700 1110 710 v -700 1100 710 v -300 1110 710 v -300 1110 910 v -300 1100 710 v -300 1100 710 v -300 1110 910 v -300 1100 910 v -300 1100 710 v -300 1100 910 v -700 1100 710 v -700 1100 710 v -300 1100 910 v -700 1100 910 v -300 1110 710 v -700 1110 710 v -300 1110 910 v -300 1110 910 v -700 1110 710 v -700 1110 910 v -1660 1150 495 v -1659.24 1146.17 495 v -1660 1150 5 v -1660 1150 5 v -1659.24 1146.17 495 v -1659.24 1146.17 5 v -1659.24 1146.17 495 v -1657.07 1142.93 495 v -1659.24 1146.17 5 v -1659.24 1146.17 5 v -1657.07 1142.93 495 v -1657.07 1142.93 5 v -1657.07 1142.93 495 v -1653.83 1140.76 495 v -1657.07 1142.93 5 v -1657.07 1142.93 5 v -1653.83 1140.76 495 v -1653.83 1140.76 5 v -1653.83 1140.76 495 v -1650 1140 495 v -1653.83 1140.76 5 v -1653.83 1140.76 5 v -1650 1140 495 v -1650 1140 5 v -1650 1140 495 v -1646.17 1140.76 495 v -1650 1140 5 v -1650 1140 5 v -1646.17 1140.76 495 v -1646.17 1140.76 5 v -1646.17 1140.76 495 v -1642.93 1142.93 495 v -1646.17 1140.76 5 v -1646.17 1140.76 5 v -1642.93 1142.93 495 v -1642.93 1142.93 5 v -1642.93 1142.93 495 v -1640.76 1146.17 495 v -1642.93 1142.93 5 v -1642.93 1142.93 5 v -1640.76 1146.17 495 v -1640.76 1146.17 5 v -1640.76 1146.17 495 v -1640 1150 495 v -1640.76 1146.17 5 v -1640.76 1146.17 5 v -1640 1150 495 v -1640 1150 5 v -1640 1150 495 v -1640.76 1153.83 495 v -1640 1150 5 v -1640 1150 5 v -1640.76 1153.83 495 v -1640.76 1153.83 5 v -1640.76 1153.83 495 v -1642.93 1157.07 495 v -1640.76 1153.83 5 v -1640.76 1153.83 5 v -1642.93 1157.07 495 v -1642.93 1157.07 5 v -1642.93 1157.07 495 v -1646.17 1159.24 495 v -1642.93 1157.07 5 v -1642.93 1157.07 5 v -1646.17 1159.24 495 v -1646.17 1159.24 5 v -1646.17 1159.24 495 v -1650 1160 495 v -1646.17 1159.24 5 v -1646.17 1159.24 5 v -1650 1160 495 v -1650 1160 5 v -1650 1160 495 v -1653.83 1159.24 495 v -1650 1160 5 v -1650 1160 5 v -1653.83 1159.24 495 v -1653.83 1159.24 5 v -1653.83 1159.24 495 v -1657.07 1157.07 495 v -1653.83 1159.24 5 v -1653.83 1159.24 5 v -1657.07 1157.07 495 v -1657.07 1157.07 5 v -1657.07 1157.07 495 v -1659.24 1153.83 495 v -1657.07 1157.07 5 v -1657.07 1157.07 5 v -1659.24 1153.83 495 v -1659.24 1153.83 5 v -1659.24 1153.83 495 v -1660 1150 495 v -1659.24 1153.83 5 v -1659.24 1153.83 5 v -1660 1150 495 v -1660 1150 5 v -1360 1150 495 v -1359.24 1146.17 495 v -1360 1150 5 v -1360 1150 5 v -1359.24 1146.17 495 v -1359.24 1146.17 5 v -1359.24 1146.17 495 v -1357.07 1142.93 495 v -1359.24 1146.17 5 v -1359.24 1146.17 5 v -1357.07 1142.93 495 v -1357.07 1142.93 5 v -1357.07 1142.93 495 v -1353.83 1140.76 495 v -1357.07 1142.93 5 v -1357.07 1142.93 5 v -1353.83 1140.76 495 v -1353.83 1140.76 5 v -1353.83 1140.76 495 v -1350 1140 495 v -1353.83 1140.76 5 v -1353.83 1140.76 5 v -1350 1140 495 v -1350 1140 5 v -1350 1140 495 v -1346.17 1140.76 495 v -1350 1140 5 v -1350 1140 5 v -1346.17 1140.76 495 v -1346.17 1140.76 5 v -1346.17 1140.76 495 v -1342.93 1142.93 495 v -1346.17 1140.76 5 v -1346.17 1140.76 5 v -1342.93 1142.93 495 v -1342.93 1142.93 5 v -1342.93 1142.93 495 v -1340.76 1146.17 495 v -1342.93 1142.93 5 v -1342.93 1142.93 5 v -1340.76 1146.17 495 v -1340.76 1146.17 5 v -1340.76 1146.17 495 v -1340 1150 495 v -1340.76 1146.17 5 v -1340.76 1146.17 5 v -1340 1150 495 v -1340 1150 5 v -1340 1150 495 v -1340.76 1153.83 495 v -1340 1150 5 v -1340 1150 5 v -1340.76 1153.83 495 v -1340.76 1153.83 5 v -1340.76 1153.83 495 v -1342.93 1157.07 495 v -1340.76 1153.83 5 v -1340.76 1153.83 5 v -1342.93 1157.07 495 v -1342.93 1157.07 5 v -1342.93 1157.07 495 v -1346.17 1159.24 495 v -1342.93 1157.07 5 v -1342.93 1157.07 5 v -1346.17 1159.24 495 v -1346.17 1159.24 5 v -1346.17 1159.24 495 v -1350 1160 495 v -1346.17 1159.24 5 v -1346.17 1159.24 5 v -1350 1160 495 v -1350 1160 5 v -1350 1160 495 v -1353.83 1159.24 495 v -1350 1160 5 v -1350 1160 5 v -1353.83 1159.24 495 v -1353.83 1159.24 5 v -1353.83 1159.24 495 v -1357.07 1157.07 495 v -1353.83 1159.24 5 v -1353.83 1159.24 5 v -1357.07 1157.07 495 v -1357.07 1157.07 5 v -1357.07 1157.07 495 v -1359.24 1153.83 495 v -1357.07 1157.07 5 v -1357.07 1157.07 5 v -1359.24 1153.83 495 v -1359.24 1153.83 5 v -1359.24 1153.83 495 v -1360 1150 495 v -1359.24 1153.83 5 v -1359.24 1153.83 5 v -1360 1150 495 v -1360 1150 5 v -1360 1450 495 v -1359.24 1446.17 495 v -1360 1450 5 v -1360 1450 5 v -1359.24 1446.17 495 v -1359.24 1446.17 5 v -1359.24 1446.17 495 v -1357.07 1442.93 495 v -1359.24 1446.17 5 v -1359.24 1446.17 5 v -1357.07 1442.93 495 v -1357.07 1442.93 5 v -1357.07 1442.93 495 v -1353.83 1440.76 495 v -1357.07 1442.93 5 v -1357.07 1442.93 5 v -1353.83 1440.76 495 v -1353.83 1440.76 5 v -1353.83 1440.76 495 v -1350 1440 495 v -1353.83 1440.76 5 v -1353.83 1440.76 5 v -1350 1440 495 v -1350 1440 5 v -1350 1440 495 v -1346.17 1440.76 495 v -1350 1440 5 v -1350 1440 5 v -1346.17 1440.76 495 v -1346.17 1440.76 5 v -1346.17 1440.76 495 v -1342.93 1442.93 495 v -1346.17 1440.76 5 v -1346.17 1440.76 5 v -1342.93 1442.93 495 v -1342.93 1442.93 5 v -1342.93 1442.93 495 v -1340.76 1446.17 495 v -1342.93 1442.93 5 v -1342.93 1442.93 5 v -1340.76 1446.17 495 v -1340.76 1446.17 5 v -1340.76 1446.17 495 v -1340 1450 495 v -1340.76 1446.17 5 v -1340.76 1446.17 5 v -1340 1450 495 v -1340 1450 5 v -1340 1450 495 v -1340.76 1453.83 495 v -1340 1450 5 v -1340 1450 5 v -1340.76 1453.83 495 v -1340.76 1453.83 5 v -1340.76 1453.83 495 v -1342.93 1457.07 495 v -1340.76 1453.83 5 v -1340.76 1453.83 5 v -1342.93 1457.07 495 v -1342.93 1457.07 5 v -1342.93 1457.07 495 v -1346.17 1459.24 495 v -1342.93 1457.07 5 v -1342.93 1457.07 5 v -1346.17 1459.24 495 v -1346.17 1459.24 5 v -1346.17 1459.24 495 v -1350 1460 495 v -1346.17 1459.24 5 v -1346.17 1459.24 5 v -1350 1460 495 v -1350 1460 5 v -1350 1460 495 v -1353.83 1459.24 495 v -1350 1460 5 v -1350 1460 5 v -1353.83 1459.24 495 v -1353.83 1459.24 5 v -1353.83 1459.24 495 v -1357.07 1457.07 495 v -1353.83 1459.24 5 v -1353.83 1459.24 5 v -1357.07 1457.07 495 v -1357.07 1457.07 5 v -1357.07 1457.07 495 v -1359.24 1453.83 495 v -1357.07 1457.07 5 v -1357.07 1457.07 5 v -1359.24 1453.83 495 v -1359.24 1453.83 5 v -1359.24 1453.83 495 v -1360 1450 495 v -1359.24 1453.83 5 v -1359.24 1453.83 5 v -1360 1450 495 v -1360 1450 5 v -1660 1450 495 v -1659.24 1446.17 495 v -1660 1450 5 v -1660 1450 5 v -1659.24 1446.17 495 v -1659.24 1446.17 5 v -1659.24 1446.17 495 v -1657.07 1442.93 495 v -1659.24 1446.17 5 v -1659.24 1446.17 5 v -1657.07 1442.93 495 v -1657.07 1442.93 5 v -1657.07 1442.93 495 v -1653.83 1440.76 495 v -1657.07 1442.93 5 v -1657.07 1442.93 5 v -1653.83 1440.76 495 v -1653.83 1440.76 5 v -1653.83 1440.76 495 v -1650 1440 495 v -1653.83 1440.76 5 v -1653.83 1440.76 5 v -1650 1440 495 v -1650 1440 5 v -1650 1440 495 v -1646.17 1440.76 495 v -1650 1440 5 v -1650 1440 5 v -1646.17 1440.76 495 v -1646.17 1440.76 5 v -1646.17 1440.76 495 v -1642.93 1442.93 495 v -1646.17 1440.76 5 v -1646.17 1440.76 5 v -1642.93 1442.93 495 v -1642.93 1442.93 5 v -1642.93 1442.93 495 v -1640.76 1446.17 495 v -1642.93 1442.93 5 v -1642.93 1442.93 5 v -1640.76 1446.17 495 v -1640.76 1446.17 5 v -1640.76 1446.17 495 v -1640 1450 495 v -1640.76 1446.17 5 v -1640.76 1446.17 5 v -1640 1450 495 v -1640 1450 5 v -1640 1450 495 v -1640.76 1453.83 495 v -1640 1450 5 v -1640 1450 5 v -1640.76 1453.83 495 v -1640.76 1453.83 5 v -1640.76 1453.83 495 v -1642.93 1457.07 495 v -1640.76 1453.83 5 v -1640.76 1453.83 5 v -1642.93 1457.07 495 v -1642.93 1457.07 5 v -1642.93 1457.07 495 v -1646.17 1459.24 495 v -1642.93 1457.07 5 v -1642.93 1457.07 5 v -1646.17 1459.24 495 v -1646.17 1459.24 5 v -1646.17 1459.24 495 v -1650 1460 495 v -1646.17 1459.24 5 v -1646.17 1459.24 5 v -1650 1460 495 v -1650 1460 5 v -1650 1460 495 v -1653.83 1459.24 495 v -1650 1460 5 v -1650 1460 5 v -1653.83 1459.24 495 v -1653.83 1459.24 5 v -1653.83 1459.24 495 v -1657.07 1457.07 495 v -1653.83 1459.24 5 v -1653.83 1459.24 5 v -1657.07 1457.07 495 v -1657.07 1457.07 5 v -1657.07 1457.07 495 v -1659.24 1453.83 495 v -1657.07 1457.07 5 v -1657.07 1457.07 5 v -1659.24 1453.83 495 v -1659.24 1453.83 5 v -1659.24 1453.83 495 v -1660 1450 495 v -1659.24 1453.83 5 v -1659.24 1453.83 5 v -1660 1450 495 v -1660 1450 5 v -1300 1500 510 v -1700 1500 510 v -1300 1100 510 v -1300 1100 510 v -1700 1500 510 v -1700 1100 510 v -1700 1500 500 v -1300 1500 500 v -1700 1100 500 v -1700 1100 500 v -1300 1500 500 v -1300 1100 500 v -1700 1500 510 v -1700 1500 500 v -1700 1100 510 v -1700 1100 510 v -1700 1500 500 v -1700 1100 500 v -1300 1500 500 v -1300 1500 510 v -1300 1100 500 v -1300 1100 500 v -1300 1500 510 v -1300 1100 510 v -1300 1100 500 v -1300 1100 510 v -1700 1100 500 v -1700 1100 500 v -1300 1100 510 v -1700 1100 510 v -1300 1500 500 v -1700 1500 500 v -1300 1500 510 v -1300 1500 510 v -1700 1500 500 v -1700 1500 510 v -1560 1095 810 v -1559.24 1091.17 810 v -1560 1095 510 v -1560 1095 510 v -1559.24 1091.17 810 v -1559.24 1091.17 510 v -1559.24 1091.17 810 v -1557.07 1087.93 810 v -1559.24 1091.17 510 v -1559.24 1091.17 510 v -1557.07 1087.93 810 v -1557.07 1087.93 510 v -1557.07 1087.93 810 v -1553.83 1085.76 810 v -1557.07 1087.93 510 v -1557.07 1087.93 510 v -1553.83 1085.76 810 v -1553.83 1085.76 510 v -1553.83 1085.76 810 v -1550 1085 810 v -1553.83 1085.76 510 v -1553.83 1085.76 510 v -1550 1085 810 v -1550 1085 510 v -1550 1085 810 v -1546.17 1085.76 810 v -1550 1085 510 v -1550 1085 510 v -1546.17 1085.76 810 v -1546.17 1085.76 510 v -1546.17 1085.76 810 v -1542.93 1087.93 810 v -1546.17 1085.76 510 v -1546.17 1085.76 510 v -1542.93 1087.93 810 v -1542.93 1087.93 510 v -1542.93 1087.93 810 v -1540.76 1091.17 810 v -1542.93 1087.93 510 v -1542.93 1087.93 510 v -1540.76 1091.17 810 v -1540.76 1091.17 510 v -1540.76 1091.17 810 v -1540 1095 810 v -1540.76 1091.17 510 v -1540.76 1091.17 510 v -1540 1095 810 v -1540 1095 510 v -1540 1095 810 v -1540.76 1098.83 810 v -1540 1095 510 v -1540 1095 510 v -1540.76 1098.83 810 v -1540.76 1098.83 510 v -1540.76 1098.83 810 v -1542.93 1102.07 810 v -1540.76 1098.83 510 v -1540.76 1098.83 510 v -1542.93 1102.07 810 v -1542.93 1102.07 510 v -1542.93 1102.07 810 v -1546.17 1104.24 810 v -1542.93 1102.07 510 v -1542.93 1102.07 510 v -1546.17 1104.24 810 v -1546.17 1104.24 510 v -1546.17 1104.24 810 v -1550 1105 810 v -1546.17 1104.24 510 v -1546.17 1104.24 510 v -1550 1105 810 v -1550 1105 510 v -1550 1105 810 v -1553.83 1104.24 810 v -1550 1105 510 v -1550 1105 510 v -1553.83 1104.24 810 v -1553.83 1104.24 510 v -1553.83 1104.24 810 v -1557.07 1102.07 810 v -1553.83 1104.24 510 v -1553.83 1104.24 510 v -1557.07 1102.07 810 v -1557.07 1102.07 510 v -1557.07 1102.07 810 v -1559.24 1098.83 810 v -1557.07 1102.07 510 v -1557.07 1102.07 510 v -1559.24 1098.83 810 v -1559.24 1098.83 510 v -1559.24 1098.83 810 v -1560 1095 810 v -1559.24 1098.83 510 v -1559.24 1098.83 510 v -1560 1095 810 v -1560 1095 510 v -1460 1095 810 v -1459.24 1091.17 810 v -1460 1095 510 v -1460 1095 510 v -1459.24 1091.17 810 v -1459.24 1091.17 510 v -1459.24 1091.17 810 v -1457.07 1087.93 810 v -1459.24 1091.17 510 v -1459.24 1091.17 510 v -1457.07 1087.93 810 v -1457.07 1087.93 510 v -1457.07 1087.93 810 v -1453.83 1085.76 810 v -1457.07 1087.93 510 v -1457.07 1087.93 510 v -1453.83 1085.76 810 v -1453.83 1085.76 510 v -1453.83 1085.76 810 v -1450 1085 810 v -1453.83 1085.76 510 v -1453.83 1085.76 510 v -1450 1085 810 v -1450 1085 510 v -1450 1085 810 v -1446.17 1085.76 810 v -1450 1085 510 v -1450 1085 510 v -1446.17 1085.76 810 v -1446.17 1085.76 510 v -1446.17 1085.76 810 v -1442.93 1087.93 810 v -1446.17 1085.76 510 v -1446.17 1085.76 510 v -1442.93 1087.93 810 v -1442.93 1087.93 510 v -1442.93 1087.93 810 v -1440.76 1091.17 810 v -1442.93 1087.93 510 v -1442.93 1087.93 510 v -1440.76 1091.17 810 v -1440.76 1091.17 510 v -1440.76 1091.17 810 v -1440 1095 810 v -1440.76 1091.17 510 v -1440.76 1091.17 510 v -1440 1095 810 v -1440 1095 510 v -1440 1095 810 v -1440.76 1098.83 810 v -1440 1095 510 v -1440 1095 510 v -1440.76 1098.83 810 v -1440.76 1098.83 510 v -1440.76 1098.83 810 v -1442.93 1102.07 810 v -1440.76 1098.83 510 v -1440.76 1098.83 510 v -1442.93 1102.07 810 v -1442.93 1102.07 510 v -1442.93 1102.07 810 v -1446.17 1104.24 810 v -1442.93 1102.07 510 v -1442.93 1102.07 510 v -1446.17 1104.24 810 v -1446.17 1104.24 510 v -1446.17 1104.24 810 v -1450 1105 810 v -1446.17 1104.24 510 v -1446.17 1104.24 510 v -1450 1105 810 v -1450 1105 510 v -1450 1105 810 v -1453.83 1104.24 810 v -1450 1105 510 v -1450 1105 510 v -1453.83 1104.24 810 v -1453.83 1104.24 510 v -1453.83 1104.24 810 v -1457.07 1102.07 810 v -1453.83 1104.24 510 v -1453.83 1104.24 510 v -1457.07 1102.07 810 v -1457.07 1102.07 510 v -1457.07 1102.07 810 v -1459.24 1098.83 810 v -1457.07 1102.07 510 v -1457.07 1102.07 510 v -1459.24 1098.83 810 v -1459.24 1098.83 510 v -1459.24 1098.83 810 v -1460 1095 810 v -1459.24 1098.83 510 v -1459.24 1098.83 510 v -1460 1095 810 v -1460 1095 510 v -1300 1110 910 v -1700 1110 910 v -1300 1100 910 v -1300 1100 910 v -1700 1110 910 v -1700 1100 910 v -1700 1110 710 v -1300 1110 710 v -1700 1100 710 v -1700 1100 710 v -1300 1110 710 v -1300 1100 710 v -1700 1110 910 v -1700 1110 710 v -1700 1100 910 v -1700 1100 910 v -1700 1110 710 v -1700 1100 710 v -1300 1110 710 v -1300 1110 910 v -1300 1100 710 v -1300 1100 710 v -1300 1110 910 v -1300 1100 910 v -1300 1100 710 v -1300 1100 910 v -1700 1100 710 v -1700 1100 710 v -1300 1100 910 v -1700 1100 910 v -1300 1110 710 v -1700 1110 710 v -1300 1110 910 v -1300 1110 910 v -1700 1110 710 v -1700 1110 910 v -2660 1150 495 v -2659.24 1146.17 495 v -2660 1150 5 v -2660 1150 5 v -2659.24 1146.17 495 v -2659.24 1146.17 5 v -2659.24 1146.17 495 v -2657.07 1142.93 495 v -2659.24 1146.17 5 v -2659.24 1146.17 5 v -2657.07 1142.93 495 v -2657.07 1142.93 5 v -2657.07 1142.93 495 v -2653.83 1140.76 495 v -2657.07 1142.93 5 v -2657.07 1142.93 5 v -2653.83 1140.76 495 v -2653.83 1140.76 5 v -2653.83 1140.76 495 v -2650 1140 495 v -2653.83 1140.76 5 v -2653.83 1140.76 5 v -2650 1140 495 v -2650 1140 5 v -2650 1140 495 v -2646.17 1140.76 495 v -2650 1140 5 v -2650 1140 5 v -2646.17 1140.76 495 v -2646.17 1140.76 5 v -2646.17 1140.76 495 v -2642.93 1142.93 495 v -2646.17 1140.76 5 v -2646.17 1140.76 5 v -2642.93 1142.93 495 v -2642.93 1142.93 5 v -2642.93 1142.93 495 v -2640.76 1146.17 495 v -2642.93 1142.93 5 v -2642.93 1142.93 5 v -2640.76 1146.17 495 v -2640.76 1146.17 5 v -2640.76 1146.17 495 v -2640 1150 495 v -2640.76 1146.17 5 v -2640.76 1146.17 5 v -2640 1150 495 v -2640 1150 5 v -2640 1150 495 v -2640.76 1153.83 495 v -2640 1150 5 v -2640 1150 5 v -2640.76 1153.83 495 v -2640.76 1153.83 5 v -2640.76 1153.83 495 v -2642.93 1157.07 495 v -2640.76 1153.83 5 v -2640.76 1153.83 5 v -2642.93 1157.07 495 v -2642.93 1157.07 5 v -2642.93 1157.07 495 v -2646.17 1159.24 495 v -2642.93 1157.07 5 v -2642.93 1157.07 5 v -2646.17 1159.24 495 v -2646.17 1159.24 5 v -2646.17 1159.24 495 v -2650 1160 495 v -2646.17 1159.24 5 v -2646.17 1159.24 5 v -2650 1160 495 v -2650 1160 5 v -2650 1160 495 v -2653.83 1159.24 495 v -2650 1160 5 v -2650 1160 5 v -2653.83 1159.24 495 v -2653.83 1159.24 5 v -2653.83 1159.24 495 v -2657.07 1157.07 495 v -2653.83 1159.24 5 v -2653.83 1159.24 5 v -2657.07 1157.07 495 v -2657.07 1157.07 5 v -2657.07 1157.07 495 v -2659.24 1153.83 495 v -2657.07 1157.07 5 v -2657.07 1157.07 5 v -2659.24 1153.83 495 v -2659.24 1153.83 5 v -2659.24 1153.83 495 v -2660 1150 495 v -2659.24 1153.83 5 v -2659.24 1153.83 5 v -2660 1150 495 v -2660 1150 5 v -2360 1150 495 v -2359.24 1146.17 495 v -2360 1150 5 v -2360 1150 5 v -2359.24 1146.17 495 v -2359.24 1146.17 5 v -2359.24 1146.17 495 v -2357.07 1142.93 495 v -2359.24 1146.17 5 v -2359.24 1146.17 5 v -2357.07 1142.93 495 v -2357.07 1142.93 5 v -2357.07 1142.93 495 v -2353.83 1140.76 495 v -2357.07 1142.93 5 v -2357.07 1142.93 5 v -2353.83 1140.76 495 v -2353.83 1140.76 5 v -2353.83 1140.76 495 v -2350 1140 495 v -2353.83 1140.76 5 v -2353.83 1140.76 5 v -2350 1140 495 v -2350 1140 5 v -2350 1140 495 v -2346.17 1140.76 495 v -2350 1140 5 v -2350 1140 5 v -2346.17 1140.76 495 v -2346.17 1140.76 5 v -2346.17 1140.76 495 v -2342.93 1142.93 495 v -2346.17 1140.76 5 v -2346.17 1140.76 5 v -2342.93 1142.93 495 v -2342.93 1142.93 5 v -2342.93 1142.93 495 v -2340.76 1146.17 495 v -2342.93 1142.93 5 v -2342.93 1142.93 5 v -2340.76 1146.17 495 v -2340.76 1146.17 5 v -2340.76 1146.17 495 v -2340 1150 495 v -2340.76 1146.17 5 v -2340.76 1146.17 5 v -2340 1150 495 v -2340 1150 5 v -2340 1150 495 v -2340.76 1153.83 495 v -2340 1150 5 v -2340 1150 5 v -2340.76 1153.83 495 v -2340.76 1153.83 5 v -2340.76 1153.83 495 v -2342.93 1157.07 495 v -2340.76 1153.83 5 v -2340.76 1153.83 5 v -2342.93 1157.07 495 v -2342.93 1157.07 5 v -2342.93 1157.07 495 v -2346.17 1159.24 495 v -2342.93 1157.07 5 v -2342.93 1157.07 5 v -2346.17 1159.24 495 v -2346.17 1159.24 5 v -2346.17 1159.24 495 v -2350 1160 495 v -2346.17 1159.24 5 v -2346.17 1159.24 5 v -2350 1160 495 v -2350 1160 5 v -2350 1160 495 v -2353.83 1159.24 495 v -2350 1160 5 v -2350 1160 5 v -2353.83 1159.24 495 v -2353.83 1159.24 5 v -2353.83 1159.24 495 v -2357.07 1157.07 495 v -2353.83 1159.24 5 v -2353.83 1159.24 5 v -2357.07 1157.07 495 v -2357.07 1157.07 5 v -2357.07 1157.07 495 v -2359.24 1153.83 495 v -2357.07 1157.07 5 v -2357.07 1157.07 5 v -2359.24 1153.83 495 v -2359.24 1153.83 5 v -2359.24 1153.83 495 v -2360 1150 495 v -2359.24 1153.83 5 v -2359.24 1153.83 5 v -2360 1150 495 v -2360 1150 5 v -2360 1450 495 v -2359.24 1446.17 495 v -2360 1450 5 v -2360 1450 5 v -2359.24 1446.17 495 v -2359.24 1446.17 5 v -2359.24 1446.17 495 v -2357.07 1442.93 495 v -2359.24 1446.17 5 v -2359.24 1446.17 5 v -2357.07 1442.93 495 v -2357.07 1442.93 5 v -2357.07 1442.93 495 v -2353.83 1440.76 495 v -2357.07 1442.93 5 v -2357.07 1442.93 5 v -2353.83 1440.76 495 v -2353.83 1440.76 5 v -2353.83 1440.76 495 v -2350 1440 495 v -2353.83 1440.76 5 v -2353.83 1440.76 5 v -2350 1440 495 v -2350 1440 5 v -2350 1440 495 v -2346.17 1440.76 495 v -2350 1440 5 v -2350 1440 5 v -2346.17 1440.76 495 v -2346.17 1440.76 5 v -2346.17 1440.76 495 v -2342.93 1442.93 495 v -2346.17 1440.76 5 v -2346.17 1440.76 5 v -2342.93 1442.93 495 v -2342.93 1442.93 5 v -2342.93 1442.93 495 v -2340.76 1446.17 495 v -2342.93 1442.93 5 v -2342.93 1442.93 5 v -2340.76 1446.17 495 v -2340.76 1446.17 5 v -2340.76 1446.17 495 v -2340 1450 495 v -2340.76 1446.17 5 v -2340.76 1446.17 5 v -2340 1450 495 v -2340 1450 5 v -2340 1450 495 v -2340.76 1453.83 495 v -2340 1450 5 v -2340 1450 5 v -2340.76 1453.83 495 v -2340.76 1453.83 5 v -2340.76 1453.83 495 v -2342.93 1457.07 495 v -2340.76 1453.83 5 v -2340.76 1453.83 5 v -2342.93 1457.07 495 v -2342.93 1457.07 5 v -2342.93 1457.07 495 v -2346.17 1459.24 495 v -2342.93 1457.07 5 v -2342.93 1457.07 5 v -2346.17 1459.24 495 v -2346.17 1459.24 5 v -2346.17 1459.24 495 v -2350 1460 495 v -2346.17 1459.24 5 v -2346.17 1459.24 5 v -2350 1460 495 v -2350 1460 5 v -2350 1460 495 v -2353.83 1459.24 495 v -2350 1460 5 v -2350 1460 5 v -2353.83 1459.24 495 v -2353.83 1459.24 5 v -2353.83 1459.24 495 v -2357.07 1457.07 495 v -2353.83 1459.24 5 v -2353.83 1459.24 5 v -2357.07 1457.07 495 v -2357.07 1457.07 5 v -2357.07 1457.07 495 v -2359.24 1453.83 495 v -2357.07 1457.07 5 v -2357.07 1457.07 5 v -2359.24 1453.83 495 v -2359.24 1453.83 5 v -2359.24 1453.83 495 v -2360 1450 495 v -2359.24 1453.83 5 v -2359.24 1453.83 5 v -2360 1450 495 v -2360 1450 5 v -2660 1450 495 v -2659.24 1446.17 495 v -2660 1450 5 v -2660 1450 5 v -2659.24 1446.17 495 v -2659.24 1446.17 5 v -2659.24 1446.17 495 v -2657.07 1442.93 495 v -2659.24 1446.17 5 v -2659.24 1446.17 5 v -2657.07 1442.93 495 v -2657.07 1442.93 5 v -2657.07 1442.93 495 v -2653.83 1440.76 495 v -2657.07 1442.93 5 v -2657.07 1442.93 5 v -2653.83 1440.76 495 v -2653.83 1440.76 5 v -2653.83 1440.76 495 v -2650 1440 495 v -2653.83 1440.76 5 v -2653.83 1440.76 5 v -2650 1440 495 v -2650 1440 5 v -2650 1440 495 v -2646.17 1440.76 495 v -2650 1440 5 v -2650 1440 5 v -2646.17 1440.76 495 v -2646.17 1440.76 5 v -2646.17 1440.76 495 v -2642.93 1442.93 495 v -2646.17 1440.76 5 v -2646.17 1440.76 5 v -2642.93 1442.93 495 v -2642.93 1442.93 5 v -2642.93 1442.93 495 v -2640.76 1446.17 495 v -2642.93 1442.93 5 v -2642.93 1442.93 5 v -2640.76 1446.17 495 v -2640.76 1446.17 5 v -2640.76 1446.17 495 v -2640 1450 495 v -2640.76 1446.17 5 v -2640.76 1446.17 5 v -2640 1450 495 v -2640 1450 5 v -2640 1450 495 v -2640.76 1453.83 495 v -2640 1450 5 v -2640 1450 5 v -2640.76 1453.83 495 v -2640.76 1453.83 5 v -2640.76 1453.83 495 v -2642.93 1457.07 495 v -2640.76 1453.83 5 v -2640.76 1453.83 5 v -2642.93 1457.07 495 v -2642.93 1457.07 5 v -2642.93 1457.07 495 v -2646.17 1459.24 495 v -2642.93 1457.07 5 v -2642.93 1457.07 5 v -2646.17 1459.24 495 v -2646.17 1459.24 5 v -2646.17 1459.24 495 v -2650 1460 495 v -2646.17 1459.24 5 v -2646.17 1459.24 5 v -2650 1460 495 v -2650 1460 5 v -2650 1460 495 v -2653.83 1459.24 495 v -2650 1460 5 v -2650 1460 5 v -2653.83 1459.24 495 v -2653.83 1459.24 5 v -2653.83 1459.24 495 v -2657.07 1457.07 495 v -2653.83 1459.24 5 v -2653.83 1459.24 5 v -2657.07 1457.07 495 v -2657.07 1457.07 5 v -2657.07 1457.07 495 v -2659.24 1453.83 495 v -2657.07 1457.07 5 v -2657.07 1457.07 5 v -2659.24 1453.83 495 v -2659.24 1453.83 5 v -2659.24 1453.83 495 v -2660 1450 495 v -2659.24 1453.83 5 v -2659.24 1453.83 5 v -2660 1450 495 v -2660 1450 5 v -2300 1500 510 v -2700 1500 510 v -2300 1100 510 v -2300 1100 510 v -2700 1500 510 v -2700 1100 510 v -2700 1500 500 v -2300 1500 500 v -2700 1100 500 v -2700 1100 500 v -2300 1500 500 v -2300 1100 500 v -2700 1500 510 v -2700 1500 500 v -2700 1100 510 v -2700 1100 510 v -2700 1500 500 v -2700 1100 500 v -2300 1500 500 v -2300 1500 510 v -2300 1100 500 v -2300 1100 500 v -2300 1500 510 v -2300 1100 510 v -2300 1100 500 v -2300 1100 510 v -2700 1100 500 v -2700 1100 500 v -2300 1100 510 v -2700 1100 510 v -2300 1500 500 v -2700 1500 500 v -2300 1500 510 v -2300 1500 510 v -2700 1500 500 v -2700 1500 510 v -2560 1095 810 v -2559.24 1091.17 810 v -2560 1095 510 v -2560 1095 510 v -2559.24 1091.17 810 v -2559.24 1091.17 510 v -2559.24 1091.17 810 v -2557.07 1087.93 810 v -2559.24 1091.17 510 v -2559.24 1091.17 510 v -2557.07 1087.93 810 v -2557.07 1087.93 510 v -2557.07 1087.93 810 v -2553.83 1085.76 810 v -2557.07 1087.93 510 v -2557.07 1087.93 510 v -2553.83 1085.76 810 v -2553.83 1085.76 510 v -2553.83 1085.76 810 v -2550 1085 810 v -2553.83 1085.76 510 v -2553.83 1085.76 510 v -2550 1085 810 v -2550 1085 510 v -2550 1085 810 v -2546.17 1085.76 810 v -2550 1085 510 v -2550 1085 510 v -2546.17 1085.76 810 v -2546.17 1085.76 510 v -2546.17 1085.76 810 v -2542.93 1087.93 810 v -2546.17 1085.76 510 v -2546.17 1085.76 510 v -2542.93 1087.93 810 v -2542.93 1087.93 510 v -2542.93 1087.93 810 v -2540.76 1091.17 810 v -2542.93 1087.93 510 v -2542.93 1087.93 510 v -2540.76 1091.17 810 v -2540.76 1091.17 510 v -2540.76 1091.17 810 v -2540 1095 810 v -2540.76 1091.17 510 v -2540.76 1091.17 510 v -2540 1095 810 v -2540 1095 510 v -2540 1095 810 v -2540.76 1098.83 810 v -2540 1095 510 v -2540 1095 510 v -2540.76 1098.83 810 v -2540.76 1098.83 510 v -2540.76 1098.83 810 v -2542.93 1102.07 810 v -2540.76 1098.83 510 v -2540.76 1098.83 510 v -2542.93 1102.07 810 v -2542.93 1102.07 510 v -2542.93 1102.07 810 v -2546.17 1104.24 810 v -2542.93 1102.07 510 v -2542.93 1102.07 510 v -2546.17 1104.24 810 v -2546.17 1104.24 510 v -2546.17 1104.24 810 v -2550 1105 810 v -2546.17 1104.24 510 v -2546.17 1104.24 510 v -2550 1105 810 v -2550 1105 510 v -2550 1105 810 v -2553.83 1104.24 810 v -2550 1105 510 v -2550 1105 510 v -2553.83 1104.24 810 v -2553.83 1104.24 510 v -2553.83 1104.24 810 v -2557.07 1102.07 810 v -2553.83 1104.24 510 v -2553.83 1104.24 510 v -2557.07 1102.07 810 v -2557.07 1102.07 510 v -2557.07 1102.07 810 v -2559.24 1098.83 810 v -2557.07 1102.07 510 v -2557.07 1102.07 510 v -2559.24 1098.83 810 v -2559.24 1098.83 510 v -2559.24 1098.83 810 v -2560 1095 810 v -2559.24 1098.83 510 v -2559.24 1098.83 510 v -2560 1095 810 v -2560 1095 510 v -2460 1095 810 v -2459.24 1091.17 810 v -2460 1095 510 v -2460 1095 510 v -2459.24 1091.17 810 v -2459.24 1091.17 510 v -2459.24 1091.17 810 v -2457.07 1087.93 810 v -2459.24 1091.17 510 v -2459.24 1091.17 510 v -2457.07 1087.93 810 v -2457.07 1087.93 510 v -2457.07 1087.93 810 v -2453.83 1085.76 810 v -2457.07 1087.93 510 v -2457.07 1087.93 510 v -2453.83 1085.76 810 v -2453.83 1085.76 510 v -2453.83 1085.76 810 v -2450 1085 810 v -2453.83 1085.76 510 v -2453.83 1085.76 510 v -2450 1085 810 v -2450 1085 510 v -2450 1085 810 v -2446.17 1085.76 810 v -2450 1085 510 v -2450 1085 510 v -2446.17 1085.76 810 v -2446.17 1085.76 510 v -2446.17 1085.76 810 v -2442.93 1087.93 810 v -2446.17 1085.76 510 v -2446.17 1085.76 510 v -2442.93 1087.93 810 v -2442.93 1087.93 510 v -2442.93 1087.93 810 v -2440.76 1091.17 810 v -2442.93 1087.93 510 v -2442.93 1087.93 510 v -2440.76 1091.17 810 v -2440.76 1091.17 510 v -2440.76 1091.17 810 v -2440 1095 810 v -2440.76 1091.17 510 v -2440.76 1091.17 510 v -2440 1095 810 v -2440 1095 510 v -2440 1095 810 v -2440.76 1098.83 810 v -2440 1095 510 v -2440 1095 510 v -2440.76 1098.83 810 v -2440.76 1098.83 510 v -2440.76 1098.83 810 v -2442.93 1102.07 810 v -2440.76 1098.83 510 v -2440.76 1098.83 510 v -2442.93 1102.07 810 v -2442.93 1102.07 510 v -2442.93 1102.07 810 v -2446.17 1104.24 810 v -2442.93 1102.07 510 v -2442.93 1102.07 510 v -2446.17 1104.24 810 v -2446.17 1104.24 510 v -2446.17 1104.24 810 v -2450 1105 810 v -2446.17 1104.24 510 v -2446.17 1104.24 510 v -2450 1105 810 v -2450 1105 510 v -2450 1105 810 v -2453.83 1104.24 810 v -2450 1105 510 v -2450 1105 510 v -2453.83 1104.24 810 v -2453.83 1104.24 510 v -2453.83 1104.24 810 v -2457.07 1102.07 810 v -2453.83 1104.24 510 v -2453.83 1104.24 510 v -2457.07 1102.07 810 v -2457.07 1102.07 510 v -2457.07 1102.07 810 v -2459.24 1098.83 810 v -2457.07 1102.07 510 v -2457.07 1102.07 510 v -2459.24 1098.83 810 v -2459.24 1098.83 510 v -2459.24 1098.83 810 v -2460 1095 810 v -2459.24 1098.83 510 v -2459.24 1098.83 510 v -2460 1095 810 v -2460 1095 510 v -2300 1110 910 v -2700 1110 910 v -2300 1100 910 v -2300 1100 910 v -2700 1110 910 v -2700 1100 910 v -2700 1110 710 v -2300 1110 710 v -2700 1100 710 v -2700 1100 710 v -2300 1110 710 v -2300 1100 710 v -2700 1110 910 v -2700 1110 710 v -2700 1100 910 v -2700 1100 910 v -2700 1110 710 v -2700 1100 710 v -2300 1110 710 v -2300 1110 910 v -2300 1100 710 v -2300 1100 710 v -2300 1110 910 v -2300 1100 910 v -2300 1100 710 v -2300 1100 910 v -2700 1100 710 v -2700 1100 710 v -2300 1100 910 v -2700 1100 910 v -2300 1110 710 v -2700 1110 710 v -2300 1110 910 v -2300 1110 910 v -2700 1110 710 v -2700 1110 910 f 1 2 3 f 4 5 6 f 7 8 9 f 10 11 12 f 13 14 15 f 16 17 18 f 19 20 21 f 22 23 24 f 25 26 27 f 28 29 30 f 31 32 33 f 34 35 36 f 37 38 39 f 40 41 42 f 43 44 45 f 46 47 48 f 49 50 51 f 52 53 54 f 55 56 57 f 58 59 60 f 61 62 63 f 64 65 66 f 67 68 69 f 70 71 72 f 73 74 75 f 76 77 78 f 79 80 81 f 82 83 84 f 85 86 87 f 88 89 90 f 91 92 93 f 94 95 96 f 97 98 99 f 100 101 102 f 103 104 105 f 106 107 108 f 109 110 111 f 112 113 114 f 115 116 117 f 118 119 120 f 121 122 123 f 124 125 126 f 127 128 129 f 130 131 132 f 133 134 135 f 136 137 138 f 139 140 141 f 142 143 144 f 145 146 147 f 148 149 150 f 151 152 153 f 154 155 156 f 157 158 159 f 160 161 162 f 163 164 165 f 166 167 168 f 169 170 171 f 172 173 174 f 175 176 177 f 178 179 180 f 181 182 183 f 184 185 186 f 187 188 189 f 190 191 192 f 193 194 195 f 196 197 198 f 199 200 201 f 202 203 204 f 205 206 207 f 208 209 210 f 211 212 213 f 214 215 216 f 217 218 219 f 220 221 222 f 223 224 225 f 226 227 228 f 229 230 231 f 232 233 234 f 235 236 237 f 238 239 240 f 241 242 243 f 244 245 246 f 247 248 249 f 250 251 252 f 253 254 255 f 256 257 258 f 259 260 261 f 262 263 264 f 265 266 267 f 268 269 270 f 271 272 273 f 274 275 276 f 277 278 279 f 280 281 282 f 283 284 285 f 286 287 288 f 289 290 291 f 292 293 294 f 295 296 297 f 298 299 300 f 301 302 303 f 304 305 306 f 307 308 309 f 310 311 312 f 313 314 315 f 316 317 318 f 319 320 321 f 322 323 324 f 325 326 327 f 328 329 330 f 331 332 333 f 334 335 336 f 337 338 339 f 340 341 342 f 343 344 345 f 346 347 348 f 349 350 351 f 352 353 354 f 355 356 357 f 358 359 360 f 361 362 363 f 364 365 366 f 367 368 369 f 370 371 372 f 373 374 375 f 376 377 378 f 379 380 381 f 382 383 384 f 385 386 387 f 388 389 390 f 391 392 393 f 394 395 396 f 397 398 399 f 400 401 402 f 403 404 405 f 406 407 408 f 409 410 411 f 412 413 414 f 415 416 417 f 418 419 420 f 421 422 423 f 424 425 426 f 427 428 429 f 430 431 432 f 433 434 435 f 436 437 438 f 439 440 441 f 442 443 444 f 445 446 447 f 448 449 450 f 451 452 453 f 454 455 456 f 457 458 459 f 460 461 462 f 463 464 465 f 466 467 468 f 469 470 471 f 472 473 474 f 475 476 477 f 478 479 480 f 481 482 483 f 484 485 486 f 487 488 489 f 490 491 492 f 493 494 495 f 496 497 498 f 499 500 501 f 502 503 504 f 505 506 507 f 508 509 510 f 511 512 513 f 514 515 516 f 517 518 519 f 520 521 522 f 523 524 525 f 526 527 528 f 529 530 531 f 532 533 534 f 535 536 537 f 538 539 540 f 541 542 543 f 544 545 546 f 547 548 549 f 550 551 552 f 553 554 555 f 556 557 558 f 559 560 561 f 562 563 564 f 565 566 567 f 568 569 570 f 571 572 573 f 574 575 576 f 577 578 579 f 580 581 582 f 583 584 585 f 586 587 588 f 589 590 591 f 592 593 594 f 595 596 597 f 598 599 600 f 601 602 603 f 604 605 606 f 607 608 609 f 610 611 612 f 613 614 615 f 616 617 618 f 619 620 621 f 622 623 624 f 625 626 627 f 628 629 630 f 631 632 633 f 634 635 636 f 637 638 639 f 640 641 642 f 643 644 645 f 646 647 648 f 649 650 651 f 652 653 654 f 655 656 657 f 658 659 660 f 661 662 663 f 664 665 666 f 667 668 669 f 670 671 672 f 673 674 675 f 676 677 678 f 679 680 681 f 682 683 684 f 685 686 687 f 688 689 690 f 691 692 693 f 694 695 696 f 697 698 699 f 700 701 702 f 703 704 705 f 706 707 708 f 709 710 711 f 712 713 714 f 715 716 717 f 718 719 720 f 721 722 723 f 724 725 726 f 727 728 729 f 730 731 732 f 733 734 735 f 736 737 738 f 739 740 741 f 742 743 744 f 745 746 747 f 748 749 750 f 751 752 753 f 754 755 756 f 757 758 759 f 760 761 762 f 763 764 765 f 766 767 768 f 769 770 771 f 772 773 774 f 775 776 777 f 778 779 780 f 781 782 783 f 784 785 786 f 787 788 789 f 790 791 792 f 793 794 795 f 796 797 798 f 799 800 801 f 802 803 804 f 805 806 807 f 808 809 810 f 811 812 813 f 814 815 816 f 817 818 819 f 820 821 822 f 823 824 825 f 826 827 828 f 829 830 831 f 832 833 834 f 835 836 837 f 838 839 840 f 841 842 843 f 844 845 846 f 847 848 849 f 850 851 852 f 853 854 855 f 856 857 858 f 859 860 861 f 862 863 864 f 865 866 867 f 868 869 870 f 871 872 873 f 874 875 876 f 877 878 879 f 880 881 882 f 883 884 885 f 886 887 888 f 889 890 891 f 892 893 894 f 895 896 897 f 898 899 900 f 901 902 903 f 904 905 906 f 907 908 909 f 910 911 912 f 913 914 915 f 916 917 918 f 919 920 921 f 922 923 924 f 925 926 927 f 928 929 930 f 931 932 933 f 934 935 936 f 937 938 939 f 940 941 942 f 943 944 945 f 946 947 948 f 949 950 951 f 952 953 954 f 955 956 957 f 958 959 960 f 961 962 963 f 964 965 966 f 967 968 969 f 970 971 972 f 973 974 975 f 976 977 978 f 979 980 981 f 982 983 984 f 985 986 987 f 988 989 990 f 991 992 993 f 994 995 996 f 997 998 999 f 1000 1001 1002 f 1003 1004 1005 f 1006 1007 1008 f 1009 1010 1011 f 1012 1013 1014 f 1015 1016 1017 f 1018 1019 1020 f 1021 1022 1023 f 1024 1025 1026 f 1027 1028 1029 f 1030 1031 1032 f 1033 1034 1035 f 1036 1037 1038 f 1039 1040 1041 f 1042 1043 1044 f 1045 1046 1047 f 1048 1049 1050 f 1051 1052 1053 f 1054 1055 1056 f 1057 1058 1059 f 1060 1061 1062 f 1063 1064 1065 f 1066 1067 1068 f 1069 1070 1071 f 1072 1073 1074 f 1075 1076 1077 f 1078 1079 1080 f 1081 1082 1083 f 1084 1085 1086 f 1087 1088 1089 f 1090 1091 1092 f 1093 1094 1095 f 1096 1097 1098 f 1099 1100 1101 f 1102 1103 1104 f 1105 1106 1107 f 1108 1109 1110 f 1111 1112 1113 f 1114 1115 1116 f 1117 1118 1119 f 1120 1121 1122 f 1123 1124 1125 f 1126 1127 1128 f 1129 1130 1131 f 1132 1133 1134 f 1135 1136 1137 f 1138 1139 1140 f 1141 1142 1143 f 1144 1145 1146 f 1147 1148 1149 f 1150 1151 1152 f 1153 1154 1155 f 1156 1157 1158 f 1159 1160 1161 f 1162 1163 1164 f 1165 1166 1167 f 1168 1169 1170 f 1171 1172 1173 f 1174 1175 1176 f 1177 1178 1179 f 1180 1181 1182 f 1183 1184 1185 f 1186 1187 1188 f 1189 1190 1191 f 1192 1193 1194 f 1195 1196 1197 f 1198 1199 1200 f 1201 1202 1203 f 1204 1205 1206 f 1207 1208 1209 f 1210 1211 1212 f 1213 1214 1215 f 1216 1217 1218 f 1219 1220 1221 f 1222 1223 1224 f 1225 1226 1227 f 1228 1229 1230 f 1231 1232 1233 f 1234 1235 1236 f 1237 1238 1239 f 1240 1241 1242 f 1243 1244 1245 f 1246 1247 1248 f 1249 1250 1251 f 1252 1253 1254 f 1255 1256 1257 f 1258 1259 1260 f 1261 1262 1263 f 1264 1265 1266 f 1267 1268 1269 f 1270 1271 1272 f 1273 1274 1275 f 1276 1277 1278 f 1279 1280 1281 f 1282 1283 1284 f 1285 1286 1287 f 1288 1289 1290 f 1291 1292 1293 f 1294 1295 1296 f 1297 1298 1299 f 1300 1301 1302 f 1303 1304 1305 f 1306 1307 1308 f 1309 1310 1311 f 1312 1313 1314 f 1315 1316 1317 f 1318 1319 1320 f 1321 1322 1323 f 1324 1325 1326 f 1327 1328 1329 f 1330 1331 1332 f 1333 1334 1335 f 1336 1337 1338 f 1339 1340 1341 f 1342 1343 1344 f 1345 1346 1347 f 1348 1349 1350 f 1351 1352 1353 f 1354 1355 1356 f 1357 1358 1359 f 1360 1361 1362 f 1363 1364 1365 f 1366 1367 1368 f 1369 1370 1371 f 1372 1373 1374 f 1375 1376 1377 f 1378 1379 1380 f 1381 1382 1383 f 1384 1385 1386 f 1387 1388 1389 f 1390 1391 1392 f 1393 1394 1395 f 1396 1397 1398 f 1399 1400 1401 f 1402 1403 1404 f 1405 1406 1407 f 1408 1409 1410 f 1411 1412 1413 f 1414 1415 1416 f 1417 1418 1419 f 1420 1421 1422 f 1423 1424 1425 f 1426 1427 1428 f 1429 1430 1431 f 1432 1433 1434 f 1435 1436 1437 f 1438 1439 1440 f 1441 1442 1443 f 1444 1445 1446 f 1447 1448 1449 f 1450 1451 1452 f 1453 1454 1455 f 1456 1457 1458 f 1459 1460 1461 f 1462 1463 1464 f 1465 1466 1467 f 1468 1469 1470 f 1471 1472 1473 f 1474 1475 1476 f 1477 1478 1479 f 1480 1481 1482 f 1483 1484 1485 f 1486 1487 1488 f 1489 1490 1491 f 1492 1493 1494 f 1495 1496 1497 f 1498 1499 1500 f 1501 1502 1503 f 1504 1505 1506 f 1507 1508 1509 f 1510 1511 1512 f 1513 1514 1515 f 1516 1517 1518 f 1519 1520 1521 f 1522 1523 1524 f 1525 1526 1527 f 1528 1529 1530 f 1531 1532 1533 f 1534 1535 1536 f 1537 1538 1539 f 1540 1541 1542 f 1543 1544 1545 f 1546 1547 1548 f 1549 1550 1551 f 1552 1553 1554 f 1555 1556 1557 f 1558 1559 1560 f 1561 1562 1563 f 1564 1565 1566 f 1567 1568 1569 f 1570 1571 1572 f 1573 1574 1575 f 1576 1577 1578 f 1579 1580 1581 f 1582 1583 1584 f 1585 1586 1587 f 1588 1589 1590 f 1591 1592 1593 f 1594 1595 1596 f 1597 1598 1599 f 1600 1601 1602 f 1603 1604 1605 f 1606 1607 1608 f 1609 1610 1611 f 1612 1613 1614 f 1615 1616 1617 f 1618 1619 1620 f 1621 1622 1623 f 1624 1625 1626 f 1627 1628 1629 f 1630 1631 1632 f 1633 1634 1635 f 1636 1637 1638 f 1639 1640 1641 f 1642 1643 1644 f 1645 1646 1647 f 1648 1649 1650 f 1651 1652 1653 f 1654 1655 1656 f 1657 1658 1659 f 1660 1661 1662 f 1663 1664 1665 f 1666 1667 1668 f 1669 1670 1671 f 1672 1673 1674 f 1675 1676 1677 f 1678 1679 1680 f 1681 1682 1683 f 1684 1685 1686 f 1687 1688 1689 f 1690 1691 1692 f 1693 1694 1695 f 1696 1697 1698 f 1699 1700 1701 f 1702 1703 1704 f 1705 1706 1707 f 1708 1709 1710 f 1711 1712 1713 f 1714 1715 1716 f 1717 1718 1719 f 1720 1721 1722 f 1723 1724 1725 f 1726 1727 1728 f 1729 1730 1731 f 1732 1733 1734 f 1735 1736 1737 f 1738 1739 1740 f 1741 1742 1743 f 1744 1745 1746 f 1747 1748 1749 f 1750 1751 1752 f 1753 1754 1755 f 1756 1757 1758 f 1759 1760 1761 f 1762 1763 1764 f 1765 1766 1767 f 1768 1769 1770 f 1771 1772 1773 f 1774 1775 1776 f 1777 1778 1779 f 1780 1781 1782 f 1783 1784 1785 f 1786 1787 1788 f 1789 1790 1791 f 1792 1793 1794 f 1795 1796 1797 f 1798 1799 1800 f 1801 1802 1803 f 1804 1805 1806 f 1807 1808 1809 f 1810 1811 1812 f 1813 1814 1815 f 1816 1817 1818 f 1819 1820 1821 f 1822 1823 1824 f 1825 1826 1827 f 1828 1829 1830 f 1831 1832 1833 f 1834 1835 1836 f 1837 1838 1839 f 1840 1841 1842 f 1843 1844 1845 f 1846 1847 1848 f 1849 1850 1851 f 1852 1853 1854 f 1855 1856 1857 f 1858 1859 1860 f 1861 1862 1863 f 1864 1865 1866 f 1867 1868 1869 f 1870 1871 1872 f 1873 1874 1875 f 1876 1877 1878 f 1879 1880 1881 f 1882 1883 1884 f 1885 1886 1887 f 1888 1889 1890 f 1891 1892 1893 f 1894 1895 1896 f 1897 1898 1899 f 1900 1901 1902 f 1903 1904 1905 f 1906 1907 1908 f 1909 1910 1911 f 1912 1913 1914 f 1915 1916 1917 f 1918 1919 1920 f 1921 1922 1923 f 1924 1925 1926 f 1927 1928 1929 f 1930 1931 1932 f 1933 1934 1935 f 1936 1937 1938 f 1939 1940 1941 f 1942 1943 1944 f 1945 1946 1947 f 1948 1949 1950 f 1951 1952 1953 f 1954 1955 1956 f 1957 1958 1959 f 1960 1961 1962 f 1963 1964 1965 f 1966 1967 1968 f 1969 1970 1971 f 1972 1973 1974 f 1975 1976 1977 f 1978 1979 1980 f 1981 1982 1983 f 1984 1985 1986 f 1987 1988 1989 f 1990 1991 1992 f 1993 1994 1995 f 1996 1997 1998 f 1999 2000 2001 f 2002 2003 2004 f 2005 2006 2007 f 2008 2009 2010 f 2011 2012 2013 f 2014 2015 2016 f 2017 2018 2019 f 2020 2021 2022 f 2023 2024 2025 f 2026 2027 2028 f 2029 2030 2031 f 2032 2033 2034 f 2035 2036 2037 f 2038 2039 2040 f 2041 2042 2043 f 2044 2045 2046 f 2047 2048 2049 f 2050 2051 2052 f 2053 2054 2055 f 2056 2057 2058 f 2059 2060 2061 f 2062 2063 2064 f 2065 2066 2067 f 2068 2069 2070 f 2071 2072 2073 f 2074 2075 2076 f 2077 2078 2079 f 2080 2081 2082 f 2083 2084 2085 f 2086 2087 2088 f 2089 2090 2091 f 2092 2093 2094 f 2095 2096 2097 f 2098 2099 2100 f 2101 2102 2103 f 2104 2105 2106 f 2107 2108 2109 f 2110 2111 2112 f 2113 2114 2115 f 2116 2117 2118 f 2119 2120 2121 f 2122 2123 2124 f 2125 2126 2127 f 2128 2129 2130 f 2131 2132 2133 f 2134 2135 2136 f 2137 2138 2139 f 2140 2141 2142 f 2143 2144 2145 f 2146 2147 2148 f 2149 2150 2151 f 2152 2153 2154 f 2155 2156 2157 f 2158 2159 2160 f 2161 2162 2163 f 2164 2165 2166 f 2167 2168 2169 f 2170 2171 2172 f 2173 2174 2175 f 2176 2177 2178 f 2179 2180 2181 f 2182 2183 2184 f 2185 2186 2187 f 2188 2189 2190 f 2191 2192 2193 f 2194 2195 2196 f 2197 2198 2199 f 2200 2201 2202 f 2203 2204 2205 f 2206 2207 2208 f 2209 2210 2211 f 2212 2213 2214 f 2215 2216 2217 f 2218 2219 2220 f 2221 2222 2223 f 2224 2225 2226 f 2227 2228 2229 f 2230 2231 2232 f 2233 2234 2235 f 2236 2237 2238 f 2239 2240 2241 f 2242 2243 2244 f 2245 2246 2247 f 2248 2249 2250 f 2251 2252 2253 f 2254 2255 2256 f 2257 2258 2259 f 2260 2261 2262 f 2263 2264 2265 f 2266 2267 2268 f 2269 2270 2271 f 2272 2273 2274 f 2275 2276 2277 f 2278 2279 2280 f 2281 2282 2283 f 2284 2285 2286 f 2287 2288 2289 f 2290 2291 2292 f 2293 2294 2295 f 2296 2297 2298 f 2299 2300 2301 f 2302 2303 2304 f 2305 2306 2307 f 2308 2309 2310 f 2311 2312 2313 f 2314 2315 2316 f 2317 2318 2319 f 2320 2321 2322 f 2323 2324 2325 f 2326 2327 2328 f 2329 2330 2331 f 2332 2333 2334 f 2335 2336 2337 f 2338 2339 2340 f 2341 2342 2343 f 2344 2345 2346 f 2347 2348 2349 f 2350 2351 2352 f 2353 2354 2355 f 2356 2357 2358 f 2359 2360 2361 f 2362 2363 2364 f 2365 2366 2367 f 2368 2369 2370 f 2371 2372 2373 f 2374 2375 2376 f 2377 2378 2379 f 2380 2381 2382 f 2383 2384 2385 f 2386 2387 2388 f 2389 2390 2391 f 2392 2393 2394 f 2395 2396 2397 f 2398 2399 2400 f 2401 2402 2403 f 2404 2405 2406 f 2407 2408 2409 f 2410 2411 2412 f 2413 2414 2415 f 2416 2417 2418 f 2419 2420 2421 f 2422 2423 2424 f 2425 2426 2427 f 2428 2429 2430 f 2431 2432 2433 f 2434 2435 2436 f 2437 2438 2439 f 2440 2441 2442 f 2443 2444 2445 f 2446 2447 2448 f 2449 2450 2451 f 2452 2453 2454 f 2455 2456 2457 f 2458 2459 2460 f 2461 2462 2463 f 2464 2465 2466 f 2467 2468 2469 f 2470 2471 2472 f 2473 2474 2475 f 2476 2477 2478 f 2479 2480 2481 f 2482 2483 2484 f 2485 2486 2487 f 2488 2489 2490 f 2491 2492 2493 f 2494 2495 2496 f 2497 2498 2499 f 2500 2501 2502 f 2503 2504 2505 f 2506 2507 2508 f 2509 2510 2511 f 2512 2513 2514 f 2515 2516 2517 f 2518 2519 2520 f 2521 2522 2523 f 2524 2525 2526 f 2527 2528 2529 f 2530 2531 2532 f 2533 2534 2535 f 2536 2537 2538 f 2539 2540 2541 f 2542 2543 2544 f 2545 2546 2547 f 2548 2549 2550 f 2551 2552 2553 f 2554 2555 2556 f 2557 2558 2559 f 2560 2561 2562 f 2563 2564 2565 f 2566 2567 2568 f 2569 2570 2571 f 2572 2573 2574 f 2575 2576 2577 f 2578 2579 2580 f 2581 2582 2583 f 2584 2585 2586 f 2587 2588 2589 f 2590 2591 2592 f 2593 2594 2595 f 2596 2597 2598 f 2599 2600 2601 f 2602 2603 2604 f 2605 2606 2607 f 2608 2609 2610 f 2611 2612 2613 f 2614 2615 2616 f 2617 2618 2619 f 2620 2621 2622 f 2623 2624 2625 f 2626 2627 2628 f 2629 2630 2631 f 2632 2633 2634 f 2635 2636 2637 f 2638 2639 2640 f 2641 2642 2643 f 2644 2645 2646 f 2647 2648 2649 f 2650 2651 2652 f 2653 2654 2655 f 2656 2657 2658 f 2659 2660 2661 f 2662 2663 2664 f 2665 2666 2667 f 2668 2669 2670 f 2671 2672 2673 f 2674 2675 2676 f 2677 2678 2679 f 2680 2681 2682 f 2683 2684 2685 f 2686 2687 2688 f 2689 2690 2691 f 2692 2693 2694 f 2695 2696 2697 f 2698 2699 2700 f 2701 2702 2703 f 2704 2705 2706 f 2707 2708 2709 f 2710 2711 2712 f 2713 2714 2715 f 2716 2717 2718 f 2719 2720 2721 f 2722 2723 2724 f 2725 2726 2727 f 2728 2729 2730 f 2731 2732 2733 f 2734 2735 2736 f 2737 2738 2739 f 2740 2741 2742 f 2743 2744 2745 f 2746 2747 2748 f 2749 2750 2751 f 2752 2753 2754 f 2755 2756 2757 f 2758 2759 2760 f 2761 2762 2763 f 2764 2765 2766 f 2767 2768 2769 f 2770 2771 2772 f 2773 2774 2775 f 2776 2777 2778 f 2779 2780 2781 f 2782 2783 2784 f 2785 2786 2787 f 2788 2789 2790 f 2791 2792 2793 f 2794 2795 2796 f 2797 2798 2799 f 2800 2801 2802 f 2803 2804 2805 f 2806 2807 2808 f 2809 2810 2811 f 2812 2813 2814 f 2815 2816 2817 f 2818 2819 2820 f 2821 2822 2823 f 2824 2825 2826 f 2827 2828 2829 f 2830 2831 2832 f 2833 2834 2835 f 2836 2837 2838 f 2839 2840 2841 f 2842 2843 2844 f 2845 2846 2847 f 2848 2849 2850 f 2851 2852 2853 f 2854 2855 2856 f 2857 2858 2859 f 2860 2861 2862 f 2863 2864 2865 f 2866 2867 2868 f 2869 2870 2871 f 2872 2873 2874 f 2875 2876 2877 f 2878 2879 2880 f 2881 2882 2883 f 2884 2885 2886 f 2887 2888 2889 f 2890 2891 2892 f 2893 2894 2895 f 2896 2897 2898 f 2899 2900 2901 f 2902 2903 2904 f 2905 2906 2907 f 2908 2909 2910 f 2911 2912 2913 f 2914 2915 2916 f 2917 2918 2919 f 2920 2921 2922 f 2923 2924 2925 f 2926 2927 2928 f 2929 2930 2931 f 2932 2933 2934 f 2935 2936 2937 f 2938 2939 2940 f 2941 2942 2943 f 2944 2945 2946 f 2947 2948 2949 f 2950 2951 2952 f 2953 2954 2955 f 2956 2957 2958 f 2959 2960 2961 f 2962 2963 2964 f 2965 2966 2967 f 2968 2969 2970 f 2971 2972 2973 f 2974 2975 2976 f 2977 2978 2979 f 2980 2981 2982 f 2983 2984 2985 f 2986 2987 2988 f 2989 2990 2991 f 2992 2993 2994 f 2995 2996 2997 f 2998 2999 3000 f 3001 3002 3003 f 3004 3005 3006 f 3007 3008 3009 f 3010 3011 3012 f 3013 3014 3015 f 3016 3017 3018 f 3019 3020 3021 f 3022 3023 3024 f 3025 3026 3027 f 3028 3029 3030 f 3031 3032 3033 f 3034 3035 3036 f 3037 3038 3039 f 3040 3041 3042 f 3043 3044 3045 f 3046 3047 3048 f 3049 3050 3051 f 3052 3053 3054 f 3055 3056 3057 f 3058 3059 3060 f 3061 3062 3063 f 3064 3065 3066 f 3067 3068 3069 f 3070 3071 3072 f 3073 3074 3075 f 3076 3077 3078 f 3079 3080 3081 f 3082 3083 3084 f 3085 3086 3087 f 3088 3089 3090 f 3091 3092 3093 f 3094 3095 3096 f 3097 3098 3099 f 3100 3101 3102 f 3103 3104 3105 f 3106 3107 3108 f 3109 3110 3111 f 3112 3113 3114 f 3115 3116 3117 f 3118 3119 3120 f 3121 3122 3123 f 3124 3125 3126 f 3127 3128 3129 f 3130 3131 3132 f 3133 3134 3135 f 3136 3137 3138 f 3139 3140 3141 f 3142 3143 3144 f 3145 3146 3147 f 3148 3149 3150 f 3151 3152 3153 f 3154 3155 3156 f 3157 3158 3159 f 3160 3161 3162 f 3163 3164 3165 f 3166 3167 3168 f 3169 3170 3171 f 3172 3173 3174 f 3175 3176 3177 f 3178 3179 3180 f 3181 3182 3183 f 3184 3185 3186 f 3187 3188 3189 f 3190 3191 3192 f 3193 3194 3195 f 3196 3197 3198 f 3199 3200 3201 f 3202 3203 3204 f 3205 3206 3207 f 3208 3209 3210 f 3211 3212 3213 f 3214 3215 3216 f 3217 3218 3219 f 3220 3221 3222 f 3223 3224 3225 f 3226 3227 3228 f 3229 3230 3231 f 3232 3233 3234 f 3235 3236 3237 f 3238 3239 3240 f 3241 3242 3243 f 3244 3245 3246 f 3247 3248 3249 f 3250 3251 3252 f 3253 3254 3255 f 3256 3257 3258 f 3259 3260 3261 f 3262 3263 3264 f 3265 3266 3267 f 3268 3269 3270 f 3271 3272 3273 f 3274 3275 3276 f 3277 3278 3279 f 3280 3281 3282 f 3283 3284 3285 f 3286 3287 3288 f 3289 3290 3291 f 3292 3293 3294 f 3295 3296 3297 f 3298 3299 3300 f 3301 3302 3303 f 3304 3305 3306 f 3307 3308 3309 f 3310 3311 3312 f 3313 3314 3315 f 3316 3317 3318 f 3319 3320 3321 f 3322 3323 3324 f 3325 3326 3327 f 3328 3329 3330 f 3331 3332 3333 f 3334 3335 3336 f 3337 3338 3339 f 3340 3341 3342 f 3343 3344 3345 f 3346 3347 3348 f 3349 3350 3351 f 3352 3353 3354 f 3355 3356 3357 f 3358 3359 3360 f 3361 3362 3363 f 3364 3365 3366 f 3367 3368 3369 f 3370 3371 3372 f 3373 3374 3375 f 3376 3377 3378 f 3379 3380 3381 f 3382 3383 3384 f 3385 3386 3387 f 3388 3389 3390 f 3391 3392 3393 f 3394 3395 3396 f 3397 3398 3399 f 3400 3401 3402 f 3403 3404 3405 f 3406 3407 3408 f 3409 3410 3411 f 3412 3413 3414 f 3415 3416 3417 f 3418 3419 3420 f 3421 3422 3423 f 3424 3425 3426 f 3427 3428 3429 f 3430 3431 3432 f 3433 3434 3435 f 3436 3437 3438 f 3439 3440 3441 f 3442 3443 3444 f 3445 3446 3447 f 3448 3449 3450 f 3451 3452 3453 f 3454 3455 3456 f 3457 3458 3459 f 3460 3461 3462 f 3463 3464 3465 f 3466 3467 3468 f 3469 3470 3471 f 3472 3473 3474 f 3475 3476 3477 f 3478 3479 3480 f 3481 3482 3483 f 3484 3485 3486 f 3487 3488 3489 f 3490 3491 3492 f 3493 3494 3495 f 3496 3497 3498 f 3499 3500 3501 f 3502 3503 3504 f 3505 3506 3507 f 3508 3509 3510 f 3511 3512 3513 f 3514 3515 3516 f 3517 3518 3519 f 3520 3521 3522 f 3523 3524 3525 f 3526 3527 3528 f 3529 3530 3531 f 3532 3533 3534 f 3535 3536 3537 f 3538 3539 3540 f 3541 3542 3543 f 3544 3545 3546 f 3547 3548 3549 f 3550 3551 3552 f 3553 3554 3555 f 3556 3557 3558 f 3559 3560 3561 f 3562 3563 3564 f 3565 3566 3567 f 3568 3569 3570 f 3571 3572 3573 f 3574 3575 3576 f 3577 3578 3579 f 3580 3581 3582 f 3583 3584 3585 f 3586 3587 3588 f 3589 3590 3591 f 3592 3593 3594 f 3595 3596 3597 f 3598 3599 3600 f 3601 3602 3603 f 3604 3605 3606 f 3607 3608 3609 f 3610 3611 3612 f 3613 3614 3615 f 3616 3617 3618 f 3619 3620 3621 f 3622 3623 3624 f 3625 3626 3627 f 3628 3629 3630 f 3631 3632 3633 f 3634 3635 3636 f 3637 3638 3639 f 3640 3641 3642 f 3643 3644 3645 f 3646 3647 3648 f 3649 3650 3651 f 3652 3653 3654 f 3655 3656 3657 f 3658 3659 3660 f 3661 3662 3663 f 3664 3665 3666 f 3667 3668 3669 f 3670 3671 3672 f 3673 3674 3675 f 3676 3677 3678 f 3679 3680 3681 f 3682 3683 3684 f 3685 3686 3687 f 3688 3689 3690 f 3691 3692 3693 f 3694 3695 3696 f 3697 3698 3699 f 3700 3701 3702 f 3703 3704 3705 f 3706 3707 3708 f 3709 3710 3711 f 3712 3713 3714 f 3715 3716 3717 f 3718 3719 3720 f 3721 3722 3723 f 3724 3725 3726 f 3727 3728 3729 f 3730 3731 3732 f 3733 3734 3735 f 3736 3737 3738 f 3739 3740 3741 f 3742 3743 3744 f 3745 3746 3747 f 3748 3749 3750 f 3751 3752 3753 f 3754 3755 3756 f 3757 3758 3759 f 3760 3761 3762 f 3763 3764 3765 f 3766 3767 3768 f 3769 3770 3771 f 3772 3773 3774 f 3775 3776 3777 f 3778 3779 3780 f 3781 3782 3783 f 3784 3785 3786 f 3787 3788 3789 f 3790 3791 3792 f 3793 3794 3795 f 3796 3797 3798 f 3799 3800 3801 f 3802 3803 3804 f 3805 3806 3807 f 3808 3809 3810 f 3811 3812 3813 f 3814 3815 3816 f 3817 3818 3819 f 3820 3821 3822 f 3823 3824 3825 f 3826 3827 3828 f 3829 3830 3831 f 3832 3833 3834 f 3835 3836 3837 f 3838 3839 3840 f 3841 3842 3843 f 3844 3845 3846 f 3847 3848 3849 f 3850 3851 3852 f 3853 3854 3855 f 3856 3857 3858 f 3859 3860 3861 f 3862 3863 3864 f 3865 3866 3867 f 3868 3869 3870 f 3871 3872 3873 f 3874 3875 3876 f 3877 3878 3879 f 3880 3881 3882 f 3883 3884 3885 f 3886 3887 3888 f 3889 3890 3891 f 3892 3893 3894 f 3895 3896 3897 f 3898 3899 3900 f 3901 3902 3903 f 3904 3905 3906 f 3907 3908 3909 f 3910 3911 3912 f 3913 3914 3915 f 3916 3917 3918 f 3919 3920 3921 f 3922 3923 3924 f 3925 3926 3927 f 3928 3929 3930 f 3931 3932 3933 f 3934 3935 3936 f 3937 3938 3939 f 3940 3941 3942 f 3943 3944 3945 f 3946 3947 3948 f 3949 3950 3951 f 3952 3953 3954 f 3955 3956 3957 f 3958 3959 3960 f 3961 3962 3963 f 3964 3965 3966 f 3967 3968 3969 f 3970 3971 3972 f 3973 3974 3975 f 3976 3977 3978 f 3979 3980 3981 f 3982 3983 3984 f 3985 3986 3987 f 3988 3989 3990 f 3991 3992 3993 f 3994 3995 3996 f 3997 3998 3999 f 4000 4001 4002 f 4003 4004 4005 f 4006 4007 4008 f 4009 4010 4011 f 4012 4013 4014 f 4015 4016 4017 f 4018 4019 4020 f 4021 4022 4023 f 4024 4025 4026 f 4027 4028 4029 f 4030 4031 4032 f 4033 4034 4035 f 4036 4037 4038 f 4039 4040 4041 f 4042 4043 4044 f 4045 4046 4047 f 4048 4049 4050 f 4051 4052 4053 f 4054 4055 4056 f 4057 4058 4059 f 4060 4061 4062 f 4063 4064 4065 f 4066 4067 4068 f 4069 4070 4071 f 4072 4073 4074 f 4075 4076 4077 f 4078 4079 4080 f 4081 4082 4083 f 4084 4085 4086 f 4087 4088 4089 f 4090 4091 4092 f 4093 4094 4095 f 4096 4097 4098 f 4099 4100 4101 f 4102 4103 4104 f 4105 4106 4107 f 4108 4109 4110 f 4111 4112 4113 f 4114 4115 4116 f 4117 4118 4119 f 4120 4121 4122 f 4123 4124 4125 f 4126 4127 4128 f 4129 4130 4131 f 4132 4133 4134 f 4135 4136 4137 f 4138 4139 4140 f 4141 4142 4143 f 4144 4145 4146 f 4147 4148 4149 f 4150 4151 4152 f 4153 4154 4155 f 4156 4157 4158 f 4159 4160 4161 f 4162 4163 4164 f 4165 4166 4167 f 4168 4169 4170 f 4171 4172 4173 f 4174 4175 4176 f 4177 4178 4179 f 4180 4181 4182 f 4183 4184 4185 f 4186 4187 4188 f 4189 4190 4191 f 4192 4193 4194 f 4195 4196 4197 f 4198 4199 4200 f 4201 4202 4203 f 4204 4205 4206 f 4207 4208 4209 f 4210 4211 4212 f 4213 4214 4215 f 4216 4217 4218 f 4219 4220 4221 f 4222 4223 4224 f 4225 4226 4227 f 4228 4229 4230 f 4231 4232 4233 f 4234 4235 4236 f 4237 4238 4239 f 4240 4241 4242 f 4243 4244 4245 f 4246 4247 4248 f 4249 4250 4251 f 4252 4253 4254 f 4255 4256 4257 f 4258 4259 4260 f 4261 4262 4263 f 4264 4265 4266 f 4267 4268 4269 f 4270 4271 4272 f 4273 4274 4275 f 4276 4277 4278 f 4279 4280 4281 f 4282 4283 4284 f 4285 4286 4287 f 4288 4289 4290 f 4291 4292 4293 f 4294 4295 4296 f 4297 4298 4299 f 4300 4301 4302 f 4303 4304 4305 f 4306 4307 4308 f 4309 4310 4311 f 4312 4313 4314 f 4315 4316 4317 f 4318 4319 4320 f 4321 4322 4323 f 4324 4325 4326 f 4327 4328 4329 f 4330 4331 4332 f 4333 4334 4335 f 4336 4337 4338 f 4339 4340 4341 f 4342 4343 4344 f 4345 4346 4347 f 4348 4349 4350 f 4351 4352 4353 f 4354 4355 4356 f 4357 4358 4359 f 4360 4361 4362 f 4363 4364 4365 f 4366 4367 4368 f 4369 4370 4371 f 4372 4373 4374 f 4375 4376 4377 f 4378 4379 4380 f 4381 4382 4383 f 4384 4385 4386 f 4387 4388 4389 f 4390 4391 4392 f 4393 4394 4395 f 4396 4397 4398 f 4399 4400 4401 f 4402 4403 4404 f 4405 4406 4407 f 4408 4409 4410 f 4411 4412 4413 f 4414 4415 4416 f 4417 4418 4419 f 4420 4421 4422 f 4423 4424 4425 f 4426 4427 4428 f 4429 4430 4431 f 4432 4433 4434 f 4435 4436 4437 f 4438 4439 4440 f 4441 4442 4443 f 4444 4445 4446 f 4447 4448 4449 f 4450 4451 4452 f 4453 4454 4455 f 4456 4457 4458 f 4459 4460 4461 f 4462 4463 4464 f 4465 4466 4467 f 4468 4469 4470 f 4471 4472 4473 f 4474 4475 4476 f 4477 4478 4479 f 4480 4481 4482 f 4483 4484 4485 f 4486 4487 4488 f 4489 4490 4491 f 4492 4493 4494 f 4495 4496 4497 f 4498 4499 4500 f 4501 4502 4503 f 4504 4505 4506 f 4507 4508 4509 f 4510 4511 4512 f 4513 4514 4515 f 4516 4517 4518 f 4519 4520 4521 f 4522 4523 4524 f 4525 4526 4527 f 4528 4529 4530 f 4531 4532 4533 f 4534 4535 4536 f 4537 4538 4539 f 4540 4541 4542 f 4543 4544 4545 f 4546 4547 4548 f 4549 4550 4551 f 4552 4553 4554 f 4555 4556 4557 f 4558 4559 4560 f 4561 4562 4563 f 4564 4565 4566 f 4567 4568 4569 f 4570 4571 4572 f 4573 4574 4575 f 4576 4577 4578 f 4579 4580 4581 f 4582 4583 4584 f 4585 4586 4587 f 4588 4589 4590 f 4591 4592 4593 f 4594 4595 4596 f 4597 4598 4599 f 4600 4601 4602 f 4603 4604 4605 f 4606 4607 4608 f 4609 4610 4611 f 4612 4613 4614 f 4615 4616 4617 f 4618 4619 4620 f 4621 4622 4623 f 4624 4625 4626 f 4627 4628 4629 f 4630 4631 4632 f 4633 4634 4635 f 4636 4637 4638 f 4639 4640 4641 f 4642 4643 4644 f 4645 4646 4647 f 4648 4649 4650 f 4651 4652 4653 f 4654 4655 4656 f 4657 4658 4659 f 4660 4661 4662 f 4663 4664 4665 f 4666 4667 4668 f 4669 4670 4671 f 4672 4673 4674 f 4675 4676 4677 f 4678 4679 4680 f 4681 4682 4683 f 4684 4685 4686 f 4687 4688 4689 f 4690 4691 4692 f 4693 4694 4695 f 4696 4697 4698 f 4699 4700 4701 f 4702 4703 4704 f 4705 4706 4707 f 4708 4709 4710 f 4711 4712 4713 f 4714 4715 4716 f 4717 4718 4719 f 4720 4721 4722 f 4723 4724 4725 f 4726 4727 4728 f 4729 4730 4731 f 4732 4733 4734 f 4735 4736 4737 f 4738 4739 4740 f 4741 4742 4743 f 4744 4745 4746 f 4747 4748 4749 f 4750 4751 4752 f 4753 4754 4755 f 4756 4757 4758 f 4759 4760 4761 f 4762 4763 4764 f 4765 4766 4767 f 4768 4769 4770 f 4771 4772 4773 f 4774 4775 4776 f 4777 4778 4779 f 4780 4781 4782 f 4783 4784 4785 f 4786 4787 4788 f 4789 4790 4791 f 4792 4793 4794 f 4795 4796 4797 f 4798 4799 4800 f 4801 4802 4803 f 4804 4805 4806 f 4807 4808 4809 f 4810 4811 4812 f 4813 4814 4815 f 4816 4817 4818 f 4819 4820 4821 f 4822 4823 4824 f 4825 4826 4827 f 4828 4829 4830 f 4831 4832 4833 f 4834 4835 4836 f 4837 4838 4839 f 4840 4841 4842 f 4843 4844 4845 f 4846 4847 4848 f 4849 4850 4851 f 4852 4853 4854 f 4855 4856 4857 f 4858 4859 4860 f 4861 4862 4863 f 4864 4865 4866 f 4867 4868 4869 f 4870 4871 4872 f 4873 4874 4875 f 4876 4877 4878 f 4879 4880 4881 f 4882 4883 4884 f 4885 4886 4887 f 4888 4889 4890 f 4891 4892 4893 f 4894 4895 4896 f 4897 4898 4899 f 4900 4901 4902 f 4903 4904 4905 f 4906 4907 4908 f 4909 4910 4911 f 4912 4913 4914 f 4915 4916 4917 f 4918 4919 4920 f 4921 4922 4923 f 4924 4925 4926 f 4927 4928 4929 f 4930 4931 4932 f 4933 4934 4935 f 4936 4937 4938 f 4939 4940 4941 f 4942 4943 4944 f 4945 4946 4947 f 4948 4949 4950 f 4951 4952 4953 f 4954 4955 4956 f 4957 4958 4959 f 4960 4961 4962 f 4963 4964 4965 f 4966 4967 4968 f 4969 4970 4971 f 4972 4973 4974 f 4975 4976 4977 f 4978 4979 4980 f 4981 4982 4983 f 4984 4985 4986 f 4987 4988 4989 f 4990 4991 4992 f 4993 4994 4995 f 4996 4997 4998 f 4999 5000 5001 f 5002 5003 5004 f 5005 5006 5007 f 5008 5009 5010 f 5011 5012 5013 f 5014 5015 5016 f 5017 5018 5019 f 5020 5021 5022 f 5023 5024 5025 f 5026 5027 5028 f 5029 5030 5031 f 5032 5033 5034 f 5035 5036 5037 f 5038 5039 5040 f 5041 5042 5043 f 5044 5045 5046 f 5047 5048 5049 f 5050 5051 5052 f 5053 5054 5055 f 5056 5057 5058 f 5059 5060 5061 f 5062 5063 5064 f 5065 5066 5067 f 5068 5069 5070 f 5071 5072 5073 f 5074 5075 5076 f 5077 5078 5079 f 5080 5081 5082 f 5083 5084 5085 f 5086 5087 5088 f 5089 5090 5091 f 5092 5093 5094 f 5095 5096 5097 f 5098 5099 5100 f 5101 5102 5103 f 5104 5105 5106 f 5107 5108 5109 f 5110 5111 5112 f 5113 5114 5115 f 5116 5117 5118 f 5119 5120 5121 f 5122 5123 5124 f 5125 5126 5127 f 5128 5129 5130 f 5131 5132 5133 f 5134 5135 5136 f 5137 5138 5139 f 5140 5141 5142 f 5143 5144 5145 f 5146 5147 5148 f 5149 5150 5151 f 5152 5153 5154 f 5155 5156 5157 f 5158 5159 5160 f 5161 5162 5163 f 5164 5165 5166 f 5167 5168 5169 f 5170 5171 5172 f 5173 5174 5175 f 5176 5177 5178 f 5179 5180 5181 f 5182 5183 5184 f 5185 5186 5187 f 5188 5189 5190 f 5191 5192 5193 f 5194 5195 5196 f 5197 5198 5199 f 5200 5201 5202 f 5203 5204 5205 f 5206 5207 5208 f 5209 5210 5211 f 5212 5213 5214 f 5215 5216 5217 f 5218 5219 5220 f 5221 5222 5223 f 5224 5225 5226 f 5227 5228 5229 f 5230 5231 5232 f 5233 5234 5235 f 5236 5237 5238 f 5239 5240 5241 f 5242 5243 5244 f 5245 5246 5247 f 5248 5249 5250 f 5251 5252 5253 f 5254 5255 5256 f 5257 5258 5259 f 5260 5261 5262 f 5263 5264 5265 f 5266 5267 5268 f 5269 5270 5271 f 5272 5273 5274 f 5275 5276 5277 f 5278 5279 5280 f 5281 5282 5283 f 5284 5285 5286 f 5287 5288 5289 f 5290 5291 5292 f 5293 5294 5295 f 5296 5297 5298 f 5299 5300 5301 f 5302 5303 5304 f 5305 5306 5307 f 5308 5309 5310 f 5311 5312 5313 f 5314 5315 5316 f 5317 5318 5319 f 5320 5321 5322 f 5323 5324 5325 f 5326 5327 5328 f 5329 5330 5331 f 5332 5333 5334 f 5335 5336 5337 f 5338 5339 5340 f 5341 5342 5343 f 5344 5345 5346 f 5347 5348 5349 f 5350 5351 5352 f 5353 5354 5355 f 5356 5357 5358 f 5359 5360 5361 f 5362 5363 5364 f 5365 5366 5367 f 5368 5369 5370 f 5371 5372 5373 f 5374 5375 5376 f 5377 5378 5379 f 5380 5381 5382 f 5383 5384 5385 f 5386 5387 5388 f 5389 5390 5391 f 5392 5393 5394 f 5395 5396 5397 f 5398 5399 5400 f 5401 5402 5403 f 5404 5405 5406 f 5407 5408 5409 f 5410 5411 5412 f 5413 5414 5415 f 5416 5417 5418 f 5419 5420 5421 f 5422 5423 5424 f 5425 5426 5427 f 5428 5429 5430 f 5431 5432 5433 f 5434 5435 5436 f 5437 5438 5439 f 5440 5441 5442 f 5443 5444 5445 f 5446 5447 5448 f 5449 5450 5451 f 5452 5453 5454 f 5455 5456 5457 f 5458 5459 5460 f 5461 5462 5463 f 5464 5465 5466 f 5467 5468 5469 f 5470 5471 5472 f 5473 5474 5475 f 5476 5477 5478 f 5479 5480 5481 f 5482 5483 5484 f 5485 5486 5487 f 5488 5489 5490 f 5491 5492 5493 f 5494 5495 5496 f 5497 5498 5499 f 5500 5501 5502 f 5503 5504 5505 f 5506 5507 5508 f 5509 5510 5511 f 5512 5513 5514 f 5515 5516 5517 f 5518 5519 5520 f 5521 5522 5523 f 5524 5525 5526 f 5527 5528 5529 f 5530 5531 5532 f 5533 5534 5535 f 5536 5537 5538 f 5539 5540 5541 f 5542 5543 5544 f 5545 5546 5547 f 5548 5549 5550 f 5551 5552 5553 f 5554 5555 5556 f 5557 5558 5559 f 5560 5561 5562 f 5563 5564 5565 f 5566 5567 5568 f 5569 5570 5571 f 5572 5573 5574 f 5575 5576 5577 f 5578 5579 5580 f 5581 5582 5583 f 5584 5585 5586 f 5587 5588 5589 f 5590 5591 5592 f 5593 5594 5595 f 5596 5597 5598 f 5599 5600 5601 f 5602 5603 5604 f 5605 5606 5607 f 5608 5609 5610 f 5611 5612 5613 f 5614 5615 5616 f 5617 5618 5619 f 5620 5621 5622 f 5623 5624 5625 f 5626 5627 5628 f 5629 5630 5631 f 5632 5633 5634 f 5635 5636 5637 f 5638 5639 5640 f 5641 5642 5643 f 5644 5645 5646 f 5647 5648 5649 f 5650 5651 5652 f 5653 5654 5655 f 5656 5657 5658 f 5659 5660 5661 f 5662 5663 5664 f 5665 5666 5667 f 5668 5669 5670 f 5671 5672 5673 f 5674 5675 5676 f 5677 5678 5679 f 5680 5681 5682 f 5683 5684 5685 f 5686 5687 5688 f 5689 5690 5691 f 5692 5693 5694 f 5695 5696 5697 f 5698 5699 5700 f 5701 5702 5703 f 5704 5705 5706 f 5707 5708 5709 f 5710 5711 5712 f 5713 5714 5715 f 5716 5717 5718 f 5719 5720 5721 f 5722 5723 5724 f 5725 5726 5727 f 5728 5729 5730 f 5731 5732 5733 f 5734 5735 5736 f 5737 5738 5739 f 5740 5741 5742 f 5743 5744 5745 f 5746 5747 5748 f 5749 5750 5751 f 5752 5753 5754 f 5755 5756 5757 f 5758 5759 5760 f 5761 5762 5763 f 5764 5765 5766 f 5767 5768 5769 f 5770 5771 5772 f 5773 5774 5775 f 5776 5777 5778 f 5779 5780 5781 f 5782 5783 5784 f 5785 5786 5787 f 5788 5789 5790 f 5791 5792 5793 f 5794 5795 5796 f 5797 5798 5799 f 5800 5801 5802 f 5803 5804 5805 f 5806 5807 5808 f 5809 5810 5811 f 5812 5813 5814 f 5815 5816 5817 f 5818 5819 5820 f 5821 5822 5823 f 5824 5825 5826 f 5827 5828 5829 f 5830 5831 5832 f 5833 5834 5835 f 5836 5837 5838 f 5839 5840 5841 f 5842 5843 5844 f 5845 5846 5847 f 5848 5849 5850 f 5851 5852 5853 f 5854 5855 5856 f 5857 5858 5859 f 5860 5861 5862 f 5863 5864 5865 f 5866 5867 5868 f 5869 5870 5871 f 5872 5873 5874 f 5875 5876 5877 f 5878 5879 5880 f 5881 5882 5883 f 5884 5885 5886 f 5887 5888 5889 f 5890 5891 5892 f 5893 5894 5895 f 5896 5897 5898 f 5899 5900 5901 f 5902 5903 5904 f 5905 5906 5907 f 5908 5909 5910 f 5911 5912 5913 f 5914 5915 5916 f 5917 5918 5919 f 5920 5921 5922 f 5923 5924 5925 f 5926 5927 5928 f 5929 5930 5931 f 5932 5933 5934 f 5935 5936 5937 f 5938 5939 5940 f 5941 5942 5943 f 5944 5945 5946 f 5947 5948 5949 f 5950 5951 5952 f 5953 5954 5955 f 5956 5957 5958 f 5959 5960 5961 f 5962 5963 5964 f 5965 5966 5967 f 5968 5969 5970 f 5971 5972 5973 f 5974 5975 5976 f 5977 5978 5979 f 5980 5981 5982 f 5983 5984 5985 f 5986 5987 5988 f 5989 5990 5991 f 5992 5993 5994 f 5995 5996 5997 f 5998 5999 6000 f 6001 6002 6003 f 6004 6005 6006 f 6007 6008 6009 f 6010 6011 6012 f 6013 6014 6015 f 6016 6017 6018 f 6019 6020 6021 f 6022 6023 6024 f 6025 6026 6027 f 6028 6029 6030 f 6031 6032 6033 f 6034 6035 6036 f 6037 6038 6039 f 6040 6041 6042 f 6043 6044 6045 f 6046 6047 6048 f 6049 6050 6051 f 6052 6053 6054 f 6055 6056 6057 f 6058 6059 6060 f 6061 6062 6063 f 6064 6065 6066 f 6067 6068 6069 f 6070 6071 6072 f 6073 6074 6075 f 6076 6077 6078 f 6079 6080 6081 f 6082 6083 6084 f 6085 6086 6087 f 6088 6089 6090 f 6091 6092 6093 f 6094 6095 6096 f 6097 6098 6099 f 6100 6101 6102 f 6103 6104 6105 f 6106 6107 6108 f 6109 6110 6111 f 6112 6113 6114 f 6115 6116 6117 f 6118 6119 6120 f 6121 6122 6123 f 6124 6125 6126 f 6127 6128 6129 f 6130 6131 6132 f 6133 6134 6135 f 6136 6137 6138 f 6139 6140 6141 f 6142 6143 6144 f 6145 6146 6147 f 6148 6149 6150 f 6151 6152 6153 f 6154 6155 6156 f 6157 6158 6159 f 6160 6161 6162 f 6163 6164 6165 f 6166 6167 6168 f 6169 6170 6171 f 6172 6173 6174 f 6175 6176 6177 f 6178 6179 6180 f 6181 6182 6183 f 6184 6185 6186 f 6187 6188 6189 f 6190 6191 6192 f 6193 6194 6195 f 6196 6197 6198 f 6199 6200 6201 f 6202 6203 6204 f 6205 6206 6207 f 6208 6209 6210 f 6211 6212 6213 f 6214 6215 6216 f 6217 6218 6219 f 6220 6221 6222 f 6223 6224 6225 f 6226 6227 6228 f 6229 6230 6231 f 6232 6233 6234 f 6235 6236 6237 f 6238 6239 6240 f 6241 6242 6243 f 6244 6245 6246 f 6247 6248 6249 f 6250 6251 6252 f 6253 6254 6255 f 6256 6257 6258 f 6259 6260 6261 f 6262 6263 6264 f 6265 6266 6267 f 6268 6269 6270 f 6271 6272 6273 f 6274 6275 6276 f 6277 6278 6279 f 6280 6281 6282 f 6283 6284 6285 f 6286 6287 6288 f 6289 6290 6291 f 6292 6293 6294 f 6295 6296 6297 f 6298 6299 6300 f 6301 6302 6303 f 6304 6305 6306 f 6307 6308 6309 f 6310 6311 6312 f 6313 6314 6315 f 6316 6317 6318 f 6319 6320 6321 f 6322 6323 6324 f 6325 6326 6327 f 6328 6329 6330 f 6331 6332 6333 f 6334 6335 6336 f 6337 6338 6339 f 6340 6341 6342 f 6343 6344 6345 f 6346 6347 6348 f 6349 6350 6351 f 6352 6353 6354 f 6355 6356 6357 f 6358 6359 6360 f 6361 6362 6363 f 6364 6365 6366 f 6367 6368 6369 f 6370 6371 6372 f 6373 6374 6375 f 6376 6377 6378 f 6379 6380 6381 f 6382 6383 6384 f 6385 6386 6387 f 6388 6389 6390 f 6391 6392 6393 f 6394 6395 6396 f 6397 6398 6399 f 6400 6401 6402 f 6403 6404 6405 f 6406 6407 6408 f 6409 6410 6411 f 6412 6413 6414 f 6415 6416 6417 f 6418 6419 6420 f 6421 6422 6423 f 6424 6425 6426 f 6427 6428 6429 f 6430 6431 6432 f 6433 6434 6435 f 6436 6437 6438 f 6439 6440 6441 f 6442 6443 6444 f 6445 6446 6447 f 6448 6449 6450 f 6451 6452 6453 f 6454 6455 6456 f 6457 6458 6459 f 6460 6461 6462 f 6463 6464 6465 f 6466 6467 6468 f 6469 6470 6471 f 6472 6473 6474 f 6475 6476 6477 f 6478 6479 6480 f 6481 6482 6483 f 6484 6485 6486 f 6487 6488 6489 f 6490 6491 6492 f 6493 6494 6495 f 6496 6497 6498 f 6499 6500 6501 f 6502 6503 6504 f 6505 6506 6507 f 6508 6509 6510 f 6511 6512 6513 f 6514 6515 6516 f 6517 6518 6519 f 6520 6521 6522 f 6523 6524 6525 f 6526 6527 6528 f 6529 6530 6531 f 6532 6533 6534 f 6535 6536 6537 f 6538 6539 6540 ================================================ FILE: test/fcl_resources/illformated_mesh.dae ================================================ Blender User Blender 2.79.0 2018-07-11T13:15:16 2018-07-11T13:15:16 Z_UP 49.13434 1.777778 0.1 100 0 0 0 49.13434 1.777778 0.1 100 0 0 0 0 0 0 1 0.5 0.5 0.5 1 0.2227451 0.2227451 0.2227451 1 0.125 0.125 0.125 1 0 1 0 0 0 1 0.5 0.5 0.5 1 0.3921569 0.5427451 0.64 1 0.125 0.125 0.125 1 0 1 0 0 0 1 0.5 0.5 0.5 1 0.4643137 0.4643137 0.4643137 1 0.125 0.125 0.125 1 0 1 0.04899346 -0.05519777 0.03526097 0.05259793 -0.0517736 0.03647089 0.05426996 -0.04994899 0.03943789 0.05913853 -0.0441541 0.03912585 0.06061244 -0.04202508 0.04216986 0.0619927 -0.04004728 0.04056584 0.06587535 -0.0332728 0.04294598 0.06681215 -0.0313391 0.04382598 0.06877338 -0.0267601 0.04541283 0.07002717 -0.02329576 0.04640489 0.07340037 -0.007734 0.05180186 0.07268106 -0.01262289 0.05221194 0.07370746 -0.003014862 0.05546885 0.0737484 -0.002906322 0.05349385 0.004809379 -0.07364749 0.02881383 0.07377976 0.00209254 0.05523401 0.07374715 0.002056241 0.05715799 0.009712278 -0.07316434 0.02898287 0.07343137 0.007327139 0.05708187 0.07277637 0.01203835 0.06063199 0.07279801 0.0121122 0.05874788 0.01419377 -0.07238531 0.03155899 0.07043635 0.02196544 0.06399899 0.07031029 0.0224229 0.06234598 0.06879997 0.02669513 0.06387096 0.06651097 0.03192973 0.067451 0.06848126 0.02742373 0.06580501 0.01921397 -0.0712589 0.02965396 0.06635677 0.03229695 0.06579202 0.06107842 0.04142022 0.06897497 0.05606198 0.04794776 0.07298201 0.04671609 0.0571298 0.07445901 0.04249948 0.06033229 0.075576 0.04130715 0.06112027 0.07752299 0.03804975 0.06323367 0.07657396 0.02873796 0.06797313 0.078224 0.02385938 0.06983506 0.078875 0.01890808 0.07130903 0.08103501 0.01903039 0.07130217 0.079405 0.02371299 -0.06984013 0.032588 0.02853858 -0.06807219 0.03075093 0.01496624 0.07231813 0.07956802 0.01428627 0.07237261 0.08144801 0.03308665 -0.06598901 0.03147298 0.009533762 0.07315123 0.08166903 0.004784941 0.07361352 0.08186602 0.03361165 -0.06565493 0.03403598 0.03691089 -0.0639097 0.03221899 0.03779447 -0.06333982 0.03481996 0.04489254 -0.05857962 0.03407198 0.04092627 -0.06142002 0.03308784 0.04227727 -0.06044089 0.03577697 -0.064763 0.03743427 -0.08769649 0.04362976 0.06100404 -0.08783614 0.04213374 0.06180536 -0.08769899 -0.06254565 0.04138916 -0.08783769 0.03906577 0.0640226 -0.08778935 0.0324524 0.06739449 -0.08769905 0.03267705 0.06750619 -0.08776545 0.02933287 0.06902599 -0.08789998 -0.07004916 0.02679729 -0.08778899 0.02732145 0.06963425 -0.08769768 -0.07181841 0.02161192 -0.08778846 0.02421599 0.07098346 -0.08778929 -0.07320445 0.01631164 -0.08778989 0.01114743 0.07396537 -0.08769917 -0.07420051 0.01092118 -0.08779168 0.01361817 0.07375347 -0.08779126 -0.0749998 2.06904e-6 -0.08781319 0 0.07479995 -0.08769994 0.005632281 0.07479315 -0.08776599 0.07480078 0 -0.08769905 -0.01114839 0.07396447 -0.08769994 -0.005589783 0.0745908 -0.08769994 0.07499939 -2.36794e-6 -0.08779156 0.07459628 0.0055902 -0.08769476 -0.01361626 0.07375389 -0.08778715 -0.02204757 0.07147675 -0.08769994 0.07419997 0.01092356 -0.08779299 -0.01895827 0.07256489 -0.08778947 0.07320475 0.01631027 -0.0877912 0.07292151 0.01667219 -0.08769667 -0.02732563 0.06963098 -0.08769905 0.07147586 0.02205568 -0.08769828 -0.02420938 0.07098549 -0.08778619 0.07004916 0.02679717 -0.08778756 0.06790649 0.03183865 -0.0877884 -0.03428387 0.06670588 -0.08785647 -0.03251469 0.06736916 -0.08769607 0.06477659 0.03740596 -0.08769857 -0.03905987 0.06402635 -0.08783859 -0.04211235 0.06182157 -0.08769798 0.06738257 0.03248137 -0.0876972 -0.04663449 0.05848413 -0.08769899 -0.04363507 0.06099987 -0.08778905 0.06178385 0.04216992 -0.08769667 -0.05085766 0.0548532 -0.08769786 -0.05482953 0.05088102 -0.08769887 -0.05205208 0.05399638 -0.08778846 -0.05845487 0.04667717 -0.08769577 0.0508517 0.05485916 -0.08769768 0.04660779 0.05850934 -0.08769637 -0.004791796 -0.07485681 0.02791082 0.0742467 -0.01067352 0.05009984 -0.04798173 -0.05765604 -0.07899999 0.01897245 -0.07257097 -0.07899999 0.02421927 -0.07099241 -0.07899999 0.0748099 -0.005475342 -0.07899999 0.07321548 -0.01630938 -0.07899999 0.01733309 -0.07297992 0.02855998 0.07182955 -0.02161026 -0.07899999 -0.02364969 -0.07118421 0.02917993 -0.01434254 -0.07362604 0.02833586 0.06255817 -0.04138809 -0.07899999 0.05937016 -0.04584395 -0.07899999 -0.05937016 -0.04584395 -0.07899999 0.01362442 -0.07376229 -0.07899999 0.01219063 -0.07401269 0.02820187 0.008203923 -0.07455998 -0.07899999 -0.0326038 -0.06755363 0.03043586 -0.02818137 -0.06951481 0.02975696 0.05206245 -0.05399996 -0.07899999 0.002739489 -0.07495999 -0.07899999 -0.02933675 -0.06903517 -0.07899999 -0.0654124 -0.03671103 -0.07899999 0.0436452 -0.06100475 -0.07899999 -0.002739489 -0.07495999 -0.07899999 -0.0342977 -0.06670957 -0.07899999 -0.07264655 -0.01868081 0.04733186 0.02933675 -0.06903517 -0.07899999 -0.03907567 -0.06402796 -0.07899999 -0.07421058 -0.01092135 -0.07899999 0.005582332 -0.07449108 -0.08569997 -0.07449108 -0.005582332 -0.07899999 -0.07469999 0 -0.08569997 0.005582332 -0.07449108 -0.07899999 0 -0.07469999 -0.08569997 -0.07386571 -0.01113349 -0.07899999 -0.06953626 -0.02729094 -0.08569997 0.02729094 -0.06953626 -0.07899999 -0.0673024 -0.03241097 -0.08569997 -0.06469208 -0.03734987 -0.08569997 0.03241097 -0.0673024 -0.07899999 0.03734987 -0.06469208 -0.07899999 0.03241097 -0.0673024 -0.08569997 -0.06171995 -0.04207998 -0.07899999 -0.06469208 -0.03734987 -0.07899999 -0.06171995 -0.04207998 -0.08569997 -0.05840265 -0.04657465 -0.08569997 0.04657465 -0.05840265 -0.07899999 -0.05475884 -0.05080878 -0.07899999 0.05080878 -0.05475884 -0.07899999 -0.05475884 -0.05080878 -0.08569997 0.05475884 -0.05080878 -0.07899999 0.05475884 -0.05080878 -0.08569997 -0.04207998 -0.06171995 -0.08569997 -0.04657465 -0.05840265 -0.07899999 -0.05080878 -0.05475884 -0.07899999 -0.04207998 -0.06172001 -0.07899999 0.06171995 -0.04207998 -0.08569997 -0.03241097 -0.0673024 -0.08569997 0.06469208 -0.03734987 -0.07899999 0.06469208 -0.03734987 -0.08569997 0.06953626 -0.02729094 -0.07899999 0.06953626 -0.02729094 -0.08569997 -0.03241097 -0.0673024 -0.07899999 -0.02729094 -0.06953626 -0.07899999 -0.02201819 -0.07138127 -0.08569997 0.07138127 -0.02201819 -0.07899999 0.07138127 -0.02201819 -0.08569997 -0.0166223 -0.0728271 -0.07899999 -0.02201819 -0.07138127 -0.07899999 -0.0166223 -0.0728271 -0.08569997 0.07386571 -0.01113349 -0.08569997 0.001560032 0.08310246 0.07464897 6.43154e-4 0.08402442 0.07463103 -3.61762e-4 0.08547061 0.075984 -0.002296805 0.08443844 0.07596302 0.001315057 0.08371883 0.07399702 -0.001008391 0.08384543 0.07464402 -0.008202791 -0.07455009 -0.08569997 -0.01362276 -0.0737524 -0.08569997 0.07479989 -0.005474627 -0.08744996 0.07479989 -0.005474627 -0.08569997 0.0742008 -0.01091998 -0.08569997 0.07499998 0 -0.08569997 -0.002739131 -0.07494997 -0.08744996 -0.07499998 0 -0.08569997 -0.07499998 0 -0.08744996 -0.07479989 -0.005474627 -0.08569997 -0.07181996 -0.02160727 -0.08569997 -0.0732057 -0.01630729 -0.08744996 -0.07005119 -0.02679216 -0.08569997 0.02421599 -0.07098299 -0.08569997 0.01896995 -0.07256126 -0.08569997 -0.06790858 -0.0318343 -0.08569997 0.02421599 -0.07098299 -0.08744996 0.02933287 -0.06902599 -0.08744996 -0.06540369 -0.03670614 -0.08744996 -0.06540369 -0.03670614 -0.08569997 0.04363936 -0.06099671 -0.08569997 0.04363936 -0.06099671 -0.08744996 -0.05936217 -0.04583787 -0.08569997 -0.06254976 -0.04138255 -0.08744996 -0.06254976 -0.04138255 -0.08569997 0.05205547 -0.05399268 -0.08569997 -0.05585777 -0.05004876 -0.08744996 -0.05205547 -0.05399268 -0.08744996 -0.04797548 -0.05764842 -0.08744996 -0.05585777 -0.05004876 -0.08569997 0.05585777 -0.05004876 -0.08569997 0.05205547 -0.05399268 -0.08744996 0.05585777 -0.05004876 -0.08744996 -0.05205547 -0.05399268 -0.08569997 0.05936217 -0.04583787 -0.08744996 -0.04363936 -0.06099671 -0.08744996 0.06254976 -0.04138255 -0.08569997 0.06254976 -0.04138255 -0.08744996 -0.03429317 -0.06670069 -0.08569997 0.06790858 -0.0318343 -0.08744996 -0.02421599 -0.07098299 -0.08569997 0.07181996 -0.02160727 -0.08744996 0.07005119 -0.02679216 -0.08744996 -0.01896995 -0.07256126 -0.08569997 -0.01362276 -0.0737524 -0.08744996 0.0732057 -0.01630729 -0.08744996 0.0742008 -0.01091998 -0.08744996 0.008202791 -0.07455009 -0.08744996 0.01896995 -0.07256126 -0.08744996 0.01362276 -0.0737524 -0.08744996 -0.0742008 -0.01091998 -0.08744996 -0.07479989 -0.005474627 -0.08744996 -0.07459175 -0.005599856 -0.0876978 0.02752023 -0.06957721 -0.0876888 -0.07181996 -0.02160727 -0.08744996 -0.07005119 -0.02679216 -0.08744996 0.03429317 -0.06670069 -0.08744996 -0.069628 -0.02733469 -0.0876984 0.03907036 -0.0640195 -0.08744996 -0.06790858 -0.0318343 -0.08744996 0.04797548 -0.05764842 -0.08744996 -0.06477659 -0.03740596 -0.08769857 -0.05936217 -0.04583787 -0.08744996 -0.06180036 -0.04214179 -0.08769869 0.06540369 -0.03670614 -0.08744996 -0.04663449 -0.05848413 -0.08769899 -0.04213374 -0.06180536 -0.08769899 -0.03907036 -0.0640195 -0.08744996 -0.0324524 -0.06739449 -0.08769911 -0.03429317 -0.06670069 -0.08744996 -0.02933287 -0.06902599 -0.08744996 -0.02421599 -0.07098299 -0.08744996 -0.01896995 -0.07256126 -0.08744996 -0.005589306 -0.07459157 -0.08769917 -0.008202791 -0.07455009 -0.08744996 0.002739131 -0.07494997 -0.08744996 0 -0.07480001 -0.0877 -0.0081923 -0.07455176 -0.08779162 0.07479977 -0.005475163 -0.0878458 0.0742008 -0.01091998 -0.08880001 0.07479989 -0.005474627 -0.08880001 0.07419908 -0.01092815 -0.08778446 -0.002727866 -0.07495087 -0.0877918 -0.008202791 -0.07455009 -0.08880001 0.002739131 -0.07494997 -0.08789998 -0.002739131 -0.07494997 -0.08880001 0.002739131 -0.07494997 -0.08880001 -0.07420015 -0.01092261 -0.08778309 0.01361209 -0.0737549 -0.08779132 -0.0732057 -0.01630729 -0.08880001 0.01896995 -0.07256126 -0.08880001 0.01896595 -0.07256257 -0.08779168 -0.07320469 -0.01631158 -0.08782488 0.02421599 -0.07098299 -0.08880001 0.02420938 -0.07098549 -0.08778619 -0.07004928 -0.02679747 -0.08782249 -0.06790715 -0.03183716 -0.08778846 0.02933287 -0.06902599 -0.08880001 -0.07005119 -0.02679216 -0.08880001 0.03429317 -0.06670069 -0.08880001 -0.06540369 -0.03670614 -0.08880001 0.03429388 -0.06670039 -0.08788561 -0.06254976 -0.04138255 -0.08880001 -0.06254547 -0.04138928 -0.08778798 0.04796802 -0.05765485 -0.08784806 0.05585277 -0.05005484 -0.08778899 -0.04363936 -0.06099671 -0.08880001 -0.03906553 -0.06402266 -0.08783632 0.06540191 -0.03670936 -0.0877881 0.06790858 -0.0318343 -0.08880001 0.07005119 -0.02679216 -0.08880001 -0.02421069 -0.07098507 -0.0877906 -0.02421599 -0.07098299 -0.08880001 0.07005006 -0.02679514 -0.08782088 0.07181996 -0.02160727 -0.08880001 -0.01895827 -0.07256489 -0.08778947 0.0732057 -0.01630729 -0.08880001 0 -0.07480001 -0.08899998 0.008202791 -0.07455009 -0.08880001 0.01362276 -0.0737524 -0.08880001 -0.07479989 -0.005474627 -0.08880001 -0.0745908 -0.005589783 -0.08899998 -0.0742008 -0.01091998 -0.08880001 0.02732735 -0.06962937 -0.08899998 -0.07181996 -0.02160727 -0.08880001 -0.07147675 -0.02204757 -0.08899998 -0.06962937 -0.02732735 -0.08899998 0.04213625 -0.06180268 -0.08899998 -0.06739246 -0.03245449 -0.08899998 0.03907036 -0.0640195 -0.08880001 -0.06790858 -0.0318343 -0.08880001 0.04363936 -0.06099671 -0.08880001 0.04663687 -0.05848085 -0.08899998 0.05087673 -0.05483222 -0.08899998 0.04797548 -0.05764842 -0.08880001 -0.06180268 -0.04213625 -0.08899998 -0.05936217 -0.04583787 -0.08880001 0.05205547 -0.05399268 -0.08880001 -0.05848085 -0.04663687 -0.08899998 0.05848085 -0.04663687 -0.08899998 0.05936217 -0.04583787 -0.08880001 0.05585777 -0.05004876 -0.08880001 -0.05585777 -0.05004876 -0.08880001 -0.05205547 -0.05399268 -0.08880001 0.06254976 -0.04138255 -0.08880001 -0.05483222 -0.05087673 -0.08899998 0.06477868 -0.03739988 -0.08899998 -0.04797548 -0.05764842 -0.08880001 -0.05087673 -0.05483222 -0.08899998 0.06540369 -0.03670614 -0.08880001 -0.04213625 -0.06180268 -0.08899998 0.06962937 -0.02732735 -0.08899998 -0.03739988 -0.06477868 -0.08899998 -0.03907036 -0.0640195 -0.08880001 -0.03245449 -0.06739246 -0.08899998 -0.03429317 -0.06670069 -0.08880001 0.07292455 -0.01664447 -0.08899998 -0.02933287 -0.06902599 -0.08880001 0.07396447 -0.01114839 -0.08899998 0.0745908 -0.005589783 -0.08899998 0.07499998 0 -0.08880001 -0.01664447 -0.07292461 -0.08899998 -0.01896995 -0.07256126 -0.08880001 -0.01362276 -0.0737524 -0.08880001 -0.005589783 -0.0745908 -0.08899998 -0.07480001 0 -0.08899998 -0.07292455 -0.01664447 -0.08899998 -0.07396447 -0.01114839 -0.08899998 -0.06477868 -0.03739988 -0.08899998 -0.04663687 -0.05848085 -0.08899998 -0.02732735 -0.06962937 -0.08899998 -0.02204757 -0.07147681 -0.08899998 -0.01114839 -0.07396447 -0.08899998 0.005589783 -0.0745908 -0.08899998 0.01114839 -0.07396447 -0.08899998 0.02204757 -0.07147681 -0.08899998 0.01664447 -0.07292461 -0.08899998 0.03245449 -0.06739246 -0.08899998 0.03739988 -0.06477868 -0.08899998 0.05483222 -0.05087673 -0.08899998 0.06180268 -0.04213625 -0.08899998 0.06739246 -0.03245449 -0.08899998 0.07147675 -0.02204757 -0.08899998 0.07480001 0 -0.08899998 0.0745908 0.005589783 -0.08899998 0.06739246 0.03245449 -0.08899998 0.06180268 0.04213625 -0.08899998 0.05483222 0.05087673 -0.08899998 0.04663687 0.05848085 -0.08899998 0.04213625 0.06180268 -0.08899998 0.03739988 0.06477868 -0.08899998 0.03245449 0.06739246 -0.08899998 0.02732735 0.06962937 -0.08899998 0.02204757 0.07147675 -0.08899998 0.01664447 0.07292455 -0.08899998 -0.005589783 0.0745908 -0.08899998 -0.02732735 0.06962937 -0.08899998 -0.04213625 0.06180268 -0.08899998 -0.04663687 0.05848085 -0.08899998 -0.05087673 0.05483222 -0.08899998 -0.06962937 0.02732735 -0.08899998 -0.06739246 0.03245449 -0.08899998 -0.01114839 0.07396447 -0.08899998 -0.008202791 0.07455009 -0.08879995 -0.01362276 0.07375234 -0.08879995 -0.01664447 0.07292455 -0.08899998 -0.01896995 0.07256126 -0.08879995 0.07396447 0.01114839 -0.08899998 0.07479989 0.005474627 -0.08880001 0.07292455 0.01664447 -0.08899998 -0.02204757 0.07147675 -0.08899998 -0.02933287 0.06902599 -0.08879995 0.07181996 0.02160727 -0.08880001 -0.03245449 0.06739246 -0.08899998 0.07005119 0.02679216 -0.08879995 0.07147675 0.02204757 -0.08899998 -0.03429317 0.06670069 -0.08879995 0.06962937 0.02732735 -0.08899998 -0.03907036 0.0640195 -0.08879995 -0.03739988 0.06477868 -0.08899998 0.06790858 0.03183424 -0.08879995 0.06540369 0.03670614 -0.08879995 0.06477868 0.03739982 -0.08899998 -0.05205547 0.05399262 -0.08879995 -0.05483222 0.05087673 -0.08899998 0.05848085 0.04663687 -0.08899998 -0.06180268 0.04213625 -0.08899998 -0.05936217 0.04583787 -0.08879995 -0.05848085 0.04663687 -0.08899998 0.05585777 0.05004876 -0.08879995 0.05087673 0.05483222 -0.08899998 -0.06477868 0.03739982 -0.08899998 0.04797548 0.05764842 -0.08879995 -0.07181996 0.02160727 -0.08880001 0.03907036 0.0640195 -0.08879995 -0.07147675 0.02204757 -0.08899998 -0.07292455 0.01664447 -0.08899998 0.02933287 0.06902599 -0.08879995 -0.07396447 0.01114839 -0.08899998 -0.0742008 0.01091992 -0.08880001 -0.0745908 0.005589783 -0.08899998 -0.07479989 0.005474627 -0.08880001 0.008202791 0.07455009 -0.08879995 0.01114839 0.07396447 -0.08899998 0.005589783 0.0745908 -0.08899998 -0.002739131 0.07494997 -0.08879995 0 0.07479995 -0.08899998 -0.07499998 0 -0.08880001 -0.07479947 0.005474686 -0.08779495 0.002735376 0.07495015 -0.08789247 0.002739131 0.07494997 -0.08879995 0.0742008 0.01091992 -0.08880001 -0.008190035 0.07455199 -0.08778768 -0.002737164 0.07495015 -0.08778709 0.07479977 0.005475342 -0.08784329 0.0732057 0.01630729 -0.08880001 0.07181918 0.02161008 -0.08782351 -0.02421599 0.07098299 -0.08879995 -0.02932214 0.069031 -0.08779025 0.06254976 0.04138255 -0.08879995 0.06540125 0.03671067 -0.08782267 0.06254696 0.04138684 -0.08778786 -0.04363936 0.06099665 -0.08879995 0.05585438 0.05005306 -0.08782798 0.05935728 0.04584485 -0.08784049 0.05936217 0.04583787 -0.08879995 -0.04797089 0.05765259 -0.08783346 -0.04797548 0.05764842 -0.08879995 -0.05585116 0.05005687 -0.08782809 -0.05585777 0.05004876 -0.08879995 0.05205547 0.05399262 -0.08879995 0.04796802 0.05765485 -0.08784806 0.05204868 0.0539999 -0.08778846 -0.0593574 0.04584467 -0.08784049 0.04363936 0.06099665 -0.08879995 -0.06254976 0.04138255 -0.08879995 0.03429317 0.06670069 -0.08879995 -0.06540369 0.03670614 -0.08879995 0.03429317 0.06670069 -0.08789998 -0.06540119 0.03671056 -0.0877881 -0.06790858 0.03183424 -0.08879995 -0.06790709 0.03183758 -0.08778697 -0.07005119 0.02679216 -0.08879995 0.01896995 0.07256126 -0.08879995 0.02421599 0.07098299 -0.08879995 0.01896446 0.07256299 -0.08778947 0.01362276 0.07375234 -0.08879995 -0.0732057 0.01630729 -0.08880001 0.008202791 0.07455009 -0.08789998 -0.05110865 -0.05265671 0.03874886 -0.07316577 0.003668189 0.05827289 -0.06544995 0.03276795 0.06835997 -0.0431112 0.05908006 0.07748097 -0.03073376 0.06634855 0.08000099 0.06572449 0.03221625 0.06816899 0.07292038 0.006935477 0.05940598 -1.31025e-5 -1.56951e-4 0.05694699 0.04362678 -0.0590223 0.03654187 -0.04797136 -0.05765193 -0.0877887 -0.04361855 -0.06101155 -0.08776301 0.06477659 -0.03740596 -0.08769857 0.06254547 -0.04138928 -0.08778798 -0.03739768 -0.06478106 -0.08769911 0.06739217 -0.0324558 -0.08769971 -0.03428345 -0.06670618 -0.08778989 0.06790548 -0.0318408 -0.08778697 -0.02732563 -0.06963098 -0.08769911 0.069628 -0.02733469 -0.0876984 -0.02932214 -0.069031 -0.08779031 0.07147866 -0.02206355 -0.08769446 0.0718187 -0.02161175 -0.08783376 0.07292419 -0.01665318 -0.0876981 -0.02204602 -0.07147806 -0.08769917 0.07396477 -0.01115781 -0.08769798 0.07320517 -0.01630944 -0.08782595 -0.0136168 -0.07375377 -0.08778977 -0.01664423 -0.07292479 -0.08769977 -0.01114827 -0.07396471 -0.08769977 0.07459175 -0.005599856 -0.0876978 0.005589306 -0.07459157 -0.08769917 0.0056324 -0.07479321 -0.08776599 -0.07480078 0 -0.08769905 0.008202791 -0.07455009 -0.08789998 0.01114743 -0.07396537 -0.08769917 -0.07479977 -0.005471885 -0.08779507 0.01664078 -0.07292789 -0.0876978 -0.07396459 -0.01115018 -0.08769959 0.02204757 -0.07147681 -0.0877 -0.07292169 -0.01667118 -0.08769667 -0.07181876 -0.02161169 -0.08782356 -0.07147198 -0.02207326 -0.0876969 0.02933984 -0.06902331 -0.08787059 0.03244757 -0.06739848 -0.08769768 0.03735637 -0.06480866 -0.08769637 -0.06738376 -0.03247869 -0.08769726 0.03906595 -0.06402242 -0.0878542 0.04209613 -0.06183511 -0.08769631 -0.06540125 -0.03671067 -0.08782279 -0.05935758 -0.04584425 -0.08778798 0.04363137 -0.06100291 -0.08785182 0.04660046 -0.05851554 -0.08769619 0.0508517 -0.05485916 -0.08769768 -0.05847847 -0.04664176 -0.08769881 -0.05585259 -0.05005508 -0.0877887 0.05204772 -0.05400079 -0.08783078 -0.05480736 -0.05090993 -0.08769607 0.05480736 -0.05090993 -0.08769607 -0.0520516 -0.05399686 -0.08783072 -0.05087429 -0.05483609 -0.08769887 0.06178724 -0.04216337 -0.0876975 0.05848056 -0.04663789 -0.08769971 0.0593574 -0.04584467 -0.0878399 -0.07126659 -0.01865613 0.05041188 -0.01139187 -0.07246804 0.03188097 -0.04271256 0.05996513 0.07758301 -0.07137447 -0.01670348 0.05121088 -0.0473544 0.05640316 0.07630401 -0.0508607 0.05257606 0.075226 -0.05110085 0.05297887 0.07515698 -0.05093657 0.05336022 0.07485401 -0.05460757 0.04945302 0.073879 -0.05432677 0.04918801 0.07403498 -0.06646746 -0.03098195 0.04626184 -0.05735576 0.04542863 0.07274901 -0.05766898 0.04531997 0.07267397 0.009665548 0.07294213 0.08209502 -0.05797153 0.04539173 0.07251995 -0.06092166 0.04107815 0.07113802 -0.06155246 -0.04043382 0.04286599 -0.06168788 0.03927725 0.070616 0.008323252 0.07263284 0.082179 -0.06266599 -0.03802555 0.04381996 -0.05478698 -0.04880636 0.04008299 -0.06381976 0.03699451 0.06924396 -0.0635671 0.03715163 0.06965601 0.003601372 0.07301825 0.082313 2.46389e-5 0.07376784 0.08192902 -0.06599026 0.03264242 0.06814098 -0.05825787 -0.0450986 0.04125601 0.002292454 0.07345062 0.08237498 -0.004268825 0.07293504 0.08228403 -0.05491375 -0.04918068 0.03978788 -0.06831836 0.02781862 0.06608998 -0.004786193 0.07361525 0.08183801 -0.006040692 0.07330012 0.08223402 -0.06941801 0.02327054 0.065068 -0.06785976 0.02798599 0.06664901 -0.06992775 0.02301442 0.064785 -0.016267 0.07127785 0.08170902 -0.05165046 -0.05262732 0.03857797 -0.01118475 0.07267856 0.08204603 -0.01480847 0.07208436 0.08179199 -0.07095199 0.01811033 0.06327897 -0.04717528 -0.05621796 0.03751397 -0.0714004 0.01820641 0.06310397 -0.07256245 0.01270115 0.06122297 -0.07200527 0.01336991 0.06163597 -0.01981556 0.07083714 0.08142101 -0.07330876 0.007125139 0.05932086 -0.07274687 0.008533656 0.05995982 -0.02087217 0.07007032 0.08129101 -0.03991085 -0.06203037 0.035227 -0.02591145 0.06882184 0.08070302 -0.03858637 -0.06244319 0.03535598 -0.02586865 0.06838774 0.08070701 -0.02924305 0.06754964 0.08019399 -0.03400003 -0.06506139 0.03444898 -0.07363981 0.001817584 0.05748283 -0.03369909 0.06543505 0.07947897 -0.03530573 -0.064673 0.03450793 -0.07325947 -0.001189768 0.05658888 -0.07368236 -0.003463268 0.05532199 -0.0382725 0.06306356 0.07821798 -0.03501725 0.06419634 0.07925403 -0.02923095 -0.06734383 0.03365695 -0.03936946 0.06214565 0.078336 -0.02713227 -0.06852692 0.03314894 -0.07322877 -0.008860468 0.05346488 -0.07302719 -0.006060779 0.054901 -0.07315576 -0.008563399 0.05392396 -0.02183848 -0.07004749 0.03271991 -0.0147016 -0.07226473 0.03181886 -0.07239401 -0.0114237 0.05304199 -0.01847809 -0.07140481 0.03207695 -0.04693508 0.05691236 0.076074 0.06825071 0.02749913 0.06639301 0.04808336 -0.05576241 0.03762501 0.06792056 0.02730894 0.066468 0.05050104 -0.0537576 0.03809183 0.05464547 -0.04896491 0.04002785 0.06605195 0.03239047 0.06811296 0.06318426 0.03693014 0.06980299 0.05095589 -0.05280518 0.038697 0.0644074 0.03596574 0.068861 0.0577597 -0.04523515 0.04132086 0.06189227 0.04013717 0.07030099 0.05741268 -0.04630291 0.04067283 0.06062614 0.04169982 0.07126998 0.06061947 -0.04130828 0.04268199 0.06030786 0.04144263 0.07136702 0.05893135 0.04413944 0.07209199 0.05910331 0.04414314 0.07167297 0.0634737 -0.03675401 0.04426085 0.05712825 0.04571235 0.072847 0.05244314 0.05188083 0.07433801 -0.004772365 -0.07360649 0.03127795 0.06574815 -0.032489 0.04573899 0.05570769 0.04811847 0.07349902 0.05023896 0.05316907 0.075432 0.05399185 0.04936605 0.07411402 0.06794756 -0.02756303 0.04744684 -0.001944959 -0.07340151 0.03155785 2.59641e-7 -0.07376283 0.03111499 0.04891139 0.05522197 0.07553297 0.06670337 -0.0311805 0.04607701 0.05224853 0.05186015 0.07477498 0.04643976 0.05703336 0.07662898 0.004751563 -0.07361048 0.03113794 0.06962156 -0.02298003 0.04903584 0.0699926 -0.02326363 0.04864984 0.04625403 0.05666077 0.07664197 0.04520249 0.05829691 0.07659298 0.04202729 0.05985397 0.07774901 0.00396198 -0.07356768 0.03142297 0.06992197 -0.02303946 0.04892283 0.04116588 0.06100857 0.07795602 0.009166002 -0.07310092 0.03161984 0.008058965 -0.07298278 0.03170299 0.0716198 -0.01770359 0.05056285 0.009480357 -0.0731517 0.03129595 0.01355957 -0.07242769 0.03181087 0.03760766 0.06271767 0.07874196 0.07203036 -0.0135563 0.052302 0.03767877 0.06322222 0.078709 0.03722095 0.06369084 0.07841098 0.01324927 -0.07221961 0.03196698 0.0188362 -0.07131803 0.03192687 0.03342849 0.06503582 0.07954502 0.03361874 0.06543236 0.079472 0.03253525 0.06620603 0.07931101 0.02912127 0.06707036 0.08025097 0.02770328 0.06837004 0.08003497 0.02855545 0.06779223 0.08032602 0.02018237 -0.07055222 0.03254497 0.02400869 0.06951975 0.08092904 0.07341438 0.00718379 0.05895495 0.02421712 0.06898832 0.08091598 0.02318638 0.07002943 0.08065199 0.03132694 -0.06639218 0.03398686 0.02879369 -0.06791192 0.03309983 0.01936525 0.07098186 0.08141601 0.0732333 0.002044439 0.05770999 0.03556525 -0.06421601 0.03474187 0.01432657 0.07210773 0.08183199 0.01599645 0.07125234 0.08170098 0.06070405 -0.04172933 0.04244995 0.06351149 -0.0375002 0.043751 0.06583166 -0.0332669 0.04516386 0.0680868 -0.02836102 0.04687196 0.0728228 -0.008195161 0.05416101 0.07331568 -0.008056581 0.05393087 0.07261365 0.01220405 0.06107884 0.07312047 -0.008066773 0.05412983 0.07321751 -0.002831995 0.05601996 0.07344669 0.002061188 0.05767798 0.07358837 -0.002767205 0.05589085 0.07109397 0.01754933 0.06308501 0.07329946 0.007447719 0.05937701 0.07219767 0.01230067 0.06126499 0.07178497 0.01699346 0.06244885 0.07028388 0.02189731 0.06448996 0.06960016 0.02272301 0.06487798 0.06929218 0.02473956 0.06540298 0.06649267 0.03175336 0.06777 0.0642094 0.03593194 0.06935399 0.07144141 0.01768904 0.06298202 0.06877046 0.02668482 0.06567001 0.063735 0.03678447 0.06958699 0.06129896 0.04073387 0.07100898 0.04651635 -0.05724817 0.03684484 0.04739505 -0.05603229 0.03757786 0.04381495 -0.05920988 0.03637588 -0.002739131 0.07494997 -0.08744996 0.07499998 0 -0.08744996 -0.01362276 0.07375234 -0.08744996 -0.01664423 0.07292479 -0.08769977 -0.02421599 0.07098299 -0.08744996 0.07396477 0.01115775 -0.08769798 -0.03739768 0.06478106 -0.08769905 -0.03429317 0.06670069 -0.08744996 0.06790858 0.03183424 -0.08744996 0.06962919 0.02732872 -0.08769965 -0.04363936 0.06099665 -0.08744996 0.06540369 0.03670614 -0.08744996 -0.05205547 0.05399262 -0.08744996 0.05936217 0.04583787 -0.08744996 0.05845838 0.04667246 -0.08769589 -0.05585777 0.05004876 -0.08744996 0.05482953 0.05088102 -0.08769887 0.05585777 0.05004876 -0.08744996 -0.06178009 0.04217797 -0.08769547 -0.05936217 0.04583787 -0.08744996 0.04797548 0.05764842 -0.08744996 -0.06540369 0.03670614 -0.08744996 -0.06790858 0.03183424 -0.08744996 -0.069628 0.02733469 -0.0876984 -0.06739217 0.0324558 -0.08769965 0.03907036 0.0640195 -0.08744996 -0.07147657 0.02204906 -0.08769959 0.03739768 0.06478106 -0.08769905 -0.07292455 0.01664626 -0.08769959 0.03429317 0.06670069 -0.08744996 -0.07396459 0.01115018 -0.08769959 -0.07459175 0.005599856 -0.0876978 0.02204757 0.07147675 -0.08769994 0.01664423 0.07292479 -0.08769977 0.005589306 0.07459157 -0.08769917 0.008202791 0.07455009 -0.08744996 0.002739131 0.07494997 -0.08744996 -0.001315236 0.08108115 0.07399702 -0.001323044 0.08352053 0.07398599 -0.06793218 -0.0287221 0.04677796 -0.0658766 -0.03317409 0.04522097 -0.06439375 -0.03605848 0.04193699 -0.06351709 -0.03749603 0.04374688 -0.06128495 -0.04108279 0.04226487 -0.05838137 -0.04511332 0.04086095 -0.05810147 -0.04550606 0.038706 -0.05517464 -0.04902923 0.03739798 -0.0549733 -0.04918462 0.03954899 -0.04793298 -0.0561285 0.03490483 -0.00481832 0.07364076 0.08021998 -0.04793858 -0.05606532 0.03734683 -0.04431927 -0.05895853 0.0363729 -0.009651839 0.07313966 0.08165997 -0.009777307 0.07314735 0.08004903 -0.01474797 0.07228094 0.08136701 -0.01921439 0.07125282 0.07938802 -0.02490437 0.06943732 0.08043301 -0.01986217 0.07104563 0.08094102 -0.03956335 -0.06232041 0.03275698 -0.02933835 0.06768494 0.07978397 -0.03366589 0.06563973 0.07907998 -0.03268265 0.06616652 0.07761299 -0.04097884 0.06137543 0.07591497 -0.03690958 0.06390506 0.07682299 -0.03582227 -0.06448578 0.03423082 -0.04270607 0.0601511 0.07718801 -0.03157186 -0.06667011 0.03348499 -0.04899907 0.05518382 0.07378 -0.05260998 0.05175405 0.072582 -0.05465549 0.04954504 0.07353401 -0.02995139 -0.06745469 0.03099083 -0.02716284 -0.06858623 0.03276985 -0.05809718 0.0454598 0.072133 -0.05914247 0.04414105 0.06992501 -0.06124275 0.04112035 0.07067602 -0.0619961 0.04003393 0.06841599 -0.02510964 -0.06940299 0.030312 -0.06682187 0.03132057 0.06550097 -0.06611508 0.03271991 0.06774401 -0.02263265 -0.07021021 0.03226488 -0.01993286 -0.07106924 0.02971196 -0.07029008 0.02241843 0.06419402 -0.0715543 0.01806426 0.06087487 -0.07172709 0.01724022 0.06227284 -0.01728814 -0.07175123 0.02947396 -0.07271945 0.01239472 0.06059288 -0.07377511 0.002380967 0.05532497 -0.01430135 -0.07240533 0.02925294 -0.07374686 0.002295911 0.05720597 1.11842e-5 -0.07380694 0.02861595 -0.07376217 -0.00234735 0.05370301 -0.07371789 -0.003448784 0.05341684 -0.07343417 -0.007322311 0.05196595 -0.07326316 -0.008861482 0.05153983 -0.0724771 -0.01372343 0.05175685 -0.07279735 -0.01211726 0.05029284 -0.07179701 -0.01708769 0.04855483 -0.07062506 -0.02141755 0.04704588 -0.07140707 -0.01850438 0.05011296 -0.06986159 -0.02364611 0.04852396 -0.0668143 -0.03134691 0.04358798 -0.07479989 0.005474627 -0.08569997 -0.0742008 0.01091992 -0.08569997 0.07479989 0.005474627 -0.08744996 -0.008202791 0.07455009 -0.08569997 -0.008202791 0.07455009 -0.08744996 -0.01896995 0.07256126 -0.08744996 0.0742008 0.01091992 -0.08569997 0.0742008 0.01091992 -0.08744996 0.07479989 0.005474627 -0.08569997 0.0732057 0.01630729 -0.08744996 -0.01896995 0.07256126 -0.08569997 0.07181996 0.02160727 -0.08744996 0.0732057 0.01630729 -0.08569997 -0.02933287 0.06902599 -0.08744996 0.07005119 0.02679216 -0.08744996 -0.03907036 0.0640195 -0.08744996 0.06254976 0.04138255 -0.08569997 0.06254976 0.04138255 -0.08744996 -0.04363936 0.06099665 -0.08569997 -0.04797548 0.05764842 -0.08569997 -0.04797548 0.05764842 -0.08744996 0.05205547 0.05399262 -0.08744996 -0.05205547 0.05399262 -0.08569997 -0.05585777 0.05004876 -0.08569997 -0.05936217 0.04583787 -0.08569997 0.04363936 0.06099665 -0.08744996 0.04797548 0.05764842 -0.08569997 -0.06540369 0.03670614 -0.08569997 -0.06254976 0.04138255 -0.08744996 0.02933287 0.06902599 -0.08744996 0.03429317 0.06670069 -0.08569997 -0.06790858 0.03183424 -0.08569997 -0.07005119 0.02679216 -0.08744996 -0.07181996 0.02160722 -0.08569997 -0.07005119 0.02679216 -0.08569997 0.02421599 0.07098299 -0.08744996 0.01896995 0.07256126 -0.08744996 0.02421599 0.07098299 -0.08569997 -0.07181996 0.02160727 -0.08744996 -0.0732057 0.01630729 -0.08744996 -0.0742008 0.01091992 -0.08744996 0.01362276 0.07375234 -0.08569997 0.01362276 0.07375234 -0.08744996 -0.07479989 0.005474627 -0.08744996 -6.42344e-4 0.08077502 0.07463103 0.001322925 0.08127915 0.07398599 -0.05601143 0.04805183 0.07128798 -0.05588299 0.04848545 0.07099801 -0.06701999 -0.03139293 0.04318284 -0.06504046 -0.03567677 0.04163485 -0.06358325 -0.03770631 0.04105287 -0.06156969 -0.0407266 0.04026496 -0.05751478 0.04712343 0.07033699 -0.05909574 0.04448044 0.06962901 -0.06201624 0.04041051 0.06814801 -0.0648126 0.03613305 0.06656199 0.008699893 0.07337415 0.07985901 -0.05474847 -0.04965806 0.03690385 -0.05775976 -0.046126 0.03812783 -0.0645774 0.03572624 0.066859 -0.06699711 0.03116935 0.06504601 0.004867315 0.07390385 0.07975298 0.005323171 0.07455521 0.07984602 0.00927633 0.07369422 0.079557 0.004769802 0.07364404 0.08021998 -0.05167406 -0.05269438 0.03612995 -6.5e-6 0.07379812 0.08026599 -0.06835287 0.02916157 0.06410998 -0.05134958 -0.0531525 0.03569895 -0.06878298 0.02674067 0.06385302 -0.001097798 0.07404822 0.07981801 -0.005442798 0.07388114 0.079737 -0.01082116 0.07323575 0.07954996 -0.04777789 -0.05640751 0.03453898 -0.01047158 0.07396006 0.07963901 -0.04392105 -0.05933398 0.03365999 -0.03938609 -0.06253713 0.03242099 -0.07046669 0.02225732 0.06194597 -0.07193249 0.01729446 0.06012886 -0.0346626 -0.06515961 0.03172898 -0.01464599 0.07233482 0.07973301 -0.03755486 -0.06390601 0.03184884 -0.01571989 0.07233041 0.07928502 -0.07272636 0.01257312 0.05889385 -0.02094423 0.07095706 0.07884603 -0.07342517 0.007433772 0.05710887 -0.072959 0.01187419 0.05832386 -0.07366716 0.006502807 0.05641198 -0.02381199 0.06985121 0.07887703 -0.02996206 -0.06759911 0.03065598 -0.02830845 0.06815284 0.07830601 -0.028979 -0.06821382 0.03035485 -0.02647405 0.06908875 0.07818597 -0.03615659 0.06542634 0.07668101 -0.07396125 -0.0023579 0.05328989 -0.03090202 0.06722027 0.07753998 -0.0203101 -0.07110422 0.02944386 -0.03528678 0.06506025 0.07674401 -0.07419717 -0.003745377 0.05270296 -0.07349169 -0.008201062 0.05129688 -0.03546679 0.06545025 0.076689 -0.03919237 0.06272506 0.07599902 -0.01433593 -0.07248491 0.02895897 -0.04471963 0.05990642 0.07476699 -0.07259386 -0.01403999 0.04927498 -0.04489517 0.05857163 0.07496201 -0.04274708 0.06074285 0.07510197 -0.04395937 0.05947113 0.07487797 -0.05188375 0.05275577 0.07248401 -0.04851675 0.05588763 0.07355999 -0.07025825 -0.02400928 0.04567897 -0.0521481 0.0530864 0.07240301 -0.06889545 -0.02646082 0.04528784 -0.06994366 -0.02395367 0.04582995 0.05599135 -0.04808533 0.03769797 0.05116426 -0.05334067 0.035622 0.05870199 -0.04494458 0.03853297 0.06396847 0.03699827 0.06706202 0.06631726 0.03262799 0.06552201 0.0631572 -0.03888291 0.04052299 0.06387257 0.03696823 0.06742501 0.06180953 -0.0405876 0.04003185 0.0612396 0.04152035 0.06855797 0.06456059 -0.03575891 0.04204183 0.05792719 0.04572427 0.07045996 -0.009568274 -0.07314032 0.03129982 -0.009696245 -0.07320868 0.02875298 0.05865776 0.04543316 0.06980097 0.06680679 -0.03135818 0.04357784 -0.01203542 -0.07311993 0.02865499 -0.004717469 -0.07367169 0.028584 0.05560046 0.04881602 0.07111698 0.05406045 0.05106115 0.071702 0.05211722 0.05246573 0.07243996 0.06509637 -0.0350821 0.04195994 0.05444109 0.04982393 0.07190901 -0.002261996 -0.07405912 0.02832895 0.05070734 0.05361837 0.073233 0.0686447 -0.0273503 0.04468697 0.04838389 0.05599814 0.073596 0.0690307 -0.02731871 0.044532 0.0687772 -0.02676159 0.04518193 0.07042789 -0.02205032 0.04693597 0.07051956 -0.0220341 0.04652684 0.007839083 -0.07446342 0.028189 0.005110144 -0.07377809 0.02846497 0.04460644 0.05901533 0.07469099 0.04118186 0.06154787 0.075459 0.07271957 -0.01261961 0.05010396 0.07157099 -0.01960891 0.04720383 0.0394513 0.06349635 0.07601201 0.01019942 -0.07319712 0.02869486 0.07186627 -0.01697468 0.048388 0.03711014 0.06430035 0.07633602 0.03348958 0.06576186 0.07747101 0.07301986 -0.01208978 0.04989695 0.01439172 -0.07239502 0.02916985 0.07355338 -0.007443368 0.05159199 0.03200995 0.06668841 0.077376 0.01770126 -0.07195222 0.02905982 0.02773147 0.06866765 0.07795298 0.0198037 -0.07121199 0.02940696 0.02384793 -0.06984752 0.03007584 0.07388788 -0.001917362 0.05352801 0.0204612 0.07189166 0.07892203 0.02299517 0.07037502 0.078574 0.07387059 0.003478944 0.05536186 0.01429235 0.07274156 0.07930803 0.03293508 -0.06620573 0.03113687 0.01929366 0.07149577 0.078924 0.07424795 8.36816e-4 0.05429196 0.07296144 0.01238214 0.05843091 0.04132628 -0.06132268 0.03283995 0.07250088 0.01622533 0.05962598 0.04316169 -0.06031799 0.03309285 0.07161468 0.01849573 0.06057995 0.07178658 0.01712822 0.06049388 0.07020699 0.02315264 0.06224596 0.07047486 0.02312409 0.06205499 0.06827187 0.02833867 0.06404799 0.05585777 0.05004876 -0.08569997 0.05205547 0.05399262 -0.08569997 0.05080878 0.05475884 -0.08569997 0.05475884 0.05080872 -0.08569997 0.04657465 0.05840265 -0.08569997 0.05840265 0.04657465 -0.08569997 0.04363936 0.06099665 -0.08569997 0.05936217 0.04583787 -0.08569997 0.06171995 0.04207998 -0.08569997 0.03907036 0.0640195 -0.08569997 0.06540369 0.03670614 -0.08569997 0.06469208 0.03734987 -0.08569997 0.06790858 0.03183424 -0.08569997 0.03241097 0.06730234 -0.08569997 0.0673024 0.03241097 -0.08569997 0.07005119 0.02679216 -0.08569997 0.02729094 0.06953626 -0.08569997 0.02933287 0.06902599 -0.08569997 0.07138127 0.02201819 -0.08569997 0.01896995 0.07256126 -0.08569997 0.07181996 0.02160722 -0.08569997 0.0166223 0.0728271 -0.08569997 0.0728271 0.01662224 -0.08569997 0.008202791 0.07455009 -0.08569997 0.005582332 0.07449108 -0.08569997 0.002739131 0.07494997 -0.08569997 0.07469999 0 -0.08569997 0.07449108 -0.005582332 -0.08569997 -0.002739131 0.07494997 -0.08569997 -0.01362276 0.07375234 -0.08569997 -0.01113349 0.07386565 -0.08569997 0.0732057 -0.01630729 -0.08569997 -0.0166223 0.0728271 -0.08569997 0.07181996 -0.02160727 -0.08569997 0.0728271 -0.0166223 -0.08569997 -0.02421599 0.07098299 -0.08569997 0.06790858 -0.0318343 -0.08569997 0.07005119 -0.02679216 -0.08569997 0.0673024 -0.03241097 -0.08569997 -0.03241097 0.06730234 -0.08569997 -0.02933287 0.06902599 -0.08569997 0.06540369 -0.03670614 -0.08569997 -0.03429317 0.06670069 -0.08569997 -0.03907036 0.0640195 -0.08569997 0.05936217 -0.04583787 -0.08569997 0.05840265 -0.04657465 -0.08569997 -0.05080878 0.05475884 -0.08569997 0.05080878 -0.05475884 -0.08569997 0.04657465 -0.05840265 -0.08569997 0.04797548 -0.05764842 -0.08569997 0.04207998 -0.06171995 -0.08569997 -0.06171995 0.04207998 -0.08569997 -0.06254976 0.04138255 -0.08569997 0.03907036 -0.0640195 -0.08569997 0.03734987 -0.06469208 -0.08569997 0.03429317 -0.06670069 -0.08569997 -0.0673024 0.03241097 -0.08569997 0.02729094 -0.06953626 -0.08569997 0.02933287 -0.06902599 -0.08569997 0.02201819 -0.07138127 -0.08569997 0.0166223 -0.0728271 -0.08569997 -0.07138127 0.02201819 -0.08569997 -0.0732057 0.01630729 -0.08569997 -0.0728271 0.01662224 -0.08569997 0.008202791 -0.07455009 -0.08569997 0.01362276 -0.0737524 -0.08569997 0.01113349 -0.07386571 -0.08569997 0.002739131 -0.07494997 -0.08569997 -0.07449108 -0.005582332 -0.08569997 -0.002739131 -0.07494997 -0.08569997 -0.005582332 -0.07449108 -0.08569997 -0.0742008 -0.01091998 -0.08569997 -0.01113349 -0.07386571 -0.08569997 -0.0732057 -0.01630729 -0.08569997 -0.0728271 -0.0166223 -0.08569997 -0.07386571 -0.01113349 -0.08569997 -0.07138127 -0.02201819 -0.08569997 -0.02933287 -0.06902599 -0.08569997 -0.02729094 -0.06953626 -0.08569997 -0.03734987 -0.06469208 -0.08569997 -0.03907036 -0.0640195 -0.08569997 -0.04657465 -0.05840265 -0.08569997 -0.04797548 -0.05764842 -0.08569997 -0.04363936 -0.06099671 -0.08569997 -0.05080878 -0.05475884 -0.08569997 -0.003052473 0.08245146 0.07593399 -0.001742005 0.08219593 0.07461303 5.05777e-4 0.08070462 0.074705 0.001620352 0.08176082 0.074602 -0.05406165 0.08599996 0.05308598 -0.03777176 0.08599996 0.06592202 -0.01448702 0.08599996 0.07464599 0.01204228 0.08599996 0.07507199 0.02622306 0.08599996 0.071379 0.0675624 0.08599996 0.03361588 0.07458001 0.08599996 0.008934974 -0.07322859 0.08599996 -0.01522696 -0.003625214 0.08599996 -0.07391744 0.01686877 0.08599996 -0.07215064 0.03515416 0.08599996 -0.06537777 0.04677897 0.057837 0.07405102 0.04813218 0.05720371 0.07383102 0.03663837 -0.06443369 0.03166699 -0.07012927 0.02586466 0.062967 0.03211975 0.06716036 0.07728201 0.02999317 0.06846863 0.077735 0.06770426 0.03066647 0.06463199 0.02553457 0.07025152 0.07835298 0.02531147 -0.069696 0.02984184 -0.0710054 0.02204173 0.06164199 0.02744597 -0.06965899 0.02985495 -0.07282006 0.01466214 0.05908387 0.07147449 0.02202272 0.06163489 0.0152989 0.07316333 0.07936298 0.01058197 0.07399201 0.07964998 0.07289546 0.01675027 0.05980789 0.07377678 0.008951961 0.057105 0.07452046 0.006506264 0.05625689 -0.0741533 0.004126906 0.05543196 -0.07237839 -0.01644825 0.04829984 -0.01695936 0.07264411 0.07918304 0.07366031 -0.009243905 0.05079787 -0.02124339 0.07136005 0.07873803 -0.0204612 -0.07202273 0.02903598 -0.06650501 -0.03421533 0.04214096 -0.02776396 -0.06949108 0.02991282 -0.04075103 0.06267094 0.07572603 -0.06092298 -0.0423699 0.03931397 -0.05643439 -0.04810827 0.03732496 -0.04966676 -0.05504649 0.03491985 -0.04850977 0.0568841 0.07371997 0.05858397 -0.04553872 0.03821599 -0.06472575 0.03744524 0.06698203 0.04938286 -0.05528938 0.03483599 0.07386571 0.01113349 -0.08569997 0.07449108 0.005582332 -0.08569997 -0.005582332 0.07449108 -0.08569997 -0.01113349 0.07386565 -0.07899999 0.0728271 0.01662224 -0.07899999 -0.02729094 0.06953626 -0.08569997 0.07138127 0.02201819 -0.07899999 -0.02729094 0.06953626 -0.07899999 -0.02201819 0.07138127 -0.07899999 -0.02201819 0.07138127 -0.08569997 -0.03241097 0.06730234 -0.07899999 0.06953626 0.02729094 -0.08569997 -0.03734987 0.06469208 -0.07899999 -0.03734987 0.06469208 -0.08569997 0.06469208 0.03734982 -0.07899999 0.0673024 0.03241091 -0.07899999 -0.04207998 0.06171995 -0.07899999 -0.04207998 0.06171995 -0.08569997 -0.04657465 0.05840265 -0.08569997 0.05840265 0.04657465 -0.07899999 -0.05080878 0.05475884 -0.07899999 -0.04657465 0.05840265 -0.07899999 -0.05475884 0.05080872 -0.07899999 -0.05475884 0.05080872 -0.08569997 -0.05840265 0.04657465 -0.08569997 0.04207998 0.06171995 -0.07899999 0.04207998 0.06171995 -0.08569997 0.03734987 0.06469208 -0.08569997 -0.06469208 0.03734987 -0.08569997 0.03241097 0.06730234 -0.07899999 0.03734987 0.06469208 -0.07899999 0.02201819 0.07138127 -0.08569997 -0.06953626 0.02729094 -0.07899999 -0.06953626 0.02729094 -0.08569997 0.02201819 0.07138127 -0.07899999 -0.07386571 0.01113349 -0.08569997 0.01113349 0.07386565 -0.08569997 -0.07449108 0.005582332 -0.08569997 -0.07469999 0 -0.07899999 0 0.07469999 -0.08569997 0.01113349 0.07386565 -0.07899999 -0.07057136 0.02653557 -0.03446269 -0.02671325 0.07149147 -0.06923878 0.07302141 0.03084266 -0.01685494 0.07157236 0.03029775 -0.02422297 0.07389569 0.03149378 -0.01181495 0.0741167 0.03371053 -0.01029282 -0.06840676 0.03297525 -0.03633058 -0.06670916 0.03859823 -0.03661155 0.071873 0.02800518 -0.02440196 0.06796181 0.03250455 -0.03976953 -0.06388509 0.04705697 -0.03868055 -0.06119728 0.05138427 -0.04228144 0.07023859 0.03223377 -0.02883738 -0.06598776 0.04038298 -0.03738445 0.07157236 0.02812707 -0.02589792 0.07289206 0.02785366 -0.01885396 0.07023859 0.02773648 -0.03443104 0.07023859 0.02882915 -0.03255838 -0.030945 0.06832939 -0.06930875 -0.03200656 0.06844168 -0.06790179 -5.2029e-5 0.08580279 -0.07400029 0.07458001 0.0313391 -0.006796956 0.07315069 0.02535134 -0.01878386 0.0741167 0.02901285 -0.01079994 -0.06840676 0.03197163 -0.03805708 -0.06885486 0.03143465 -0.03646987 0.07315069 0.02863878 -0.0169909 0.07289206 0.02473086 -0.02096682 0.07157236 0.02678424 -0.02725344 0.07389569 0.02754938 -0.01272284 -0.02757799 0.07025039 -0.06970936 0.0741167 0.02461028 -0.01214498 0.07458001 0.02800828 -0.007012963 0.07238256 0.02419465 -0.02473396 0.07468557 0.03062742 -0.00574696 0.07497709 0.02545893 -9.07993e-4 0.07468557 0.02724987 -0.005967974 0.039837 0.08599996 -0.06267237 0.03489208 0.08278739 -0.06541347 0.0748592 0.02911615 -0.003518939 0.07463276 0.02390193 -0.007310986 0.071873 0.02311521 -0.03047508 0.0749914 0.02681607 -1.26987e-4 -0.06670916 0.03752017 -0.03769981 0.006955385 0.07857096 -0.07367497 0.07389569 0.02389526 -0.01429986 0.07157236 0.02521896 -0.02927386 0.0699957 0.02753371 -0.03689408 0.071873 0.02430558 -0.02844297 0.07315069 0.02149808 -0.02210998 0.0749315 0.02819085 -0.002153933 0.07389569 0.02054291 -0.01657485 0.07289206 0.02100282 -0.02495884 0.07315069 0.02297145 -0.02062284 -0.01845556 0.07377827 -0.07189959 0.07458001 0.01988655 -0.009688973 0.0741167 0.02107423 -0.01407897 0.07164758 0.02275776 -0.03374594 0.0749315 0.02295744 -0.002737939 -0.07421529 0.08599996 -0.009834945 -0.07200348 0.08599996 -0.02001798 -0.0722987 0.08586114 -0.0189749 0.07477235 0.01908314 -0.007131934 -0.06150662 0.04969751 -0.04230934 -0.07500016 0.08599996 9.99987e-4 -0.07068288 0.08517587 -0.02410894 -0.07498377 0.05257886 -5.48987e-4 -0.07500016 0.05202037 9.99987e-4 -0.06598776 0.0375458 -0.04048925 -0.07045978 0.08599996 -0.02473795 -0.06388509 0.04435312 -0.04012018 -0.07495528 0.05294615 -0.001566946 0.0749973 0.01410055 -0.001321971 -0.0748021 0.08599996 -0.004450917 0.07315069 0.01977872 -0.02432584 0.07389569 0.01733833 -0.01985198 0.07308608 0.01836276 -0.02762216 0.07463276 0.01584047 -0.01164084 -0.07489866 0.05259734 -0.002860963 -0.06634849 0.03572309 -0.0420956 -0.06885486 0.08572053 -0.02877694 0.07389569 0.01553303 -0.02249795 -0.07476019 0.05257368 -0.00493592 -0.07476019 0.08571416 -0.004997968 -0.06840687 0.08599996 -0.02979916 0.0741167 0.01772004 -0.01686286 -0.01377195 0.0753712 -0.07269185 -0.01854658 0.07302784 -0.07252347 0.04283457 0.08580309 -0.06068277 -0.05815666 0.05627214 -0.04590463 -0.06388509 0.04254812 -0.04164886 -0.06598776 0.08599996 -0.03470593 0.074952 0.01662993 -0.004285991 -0.02674818 0.07007879 -0.07082194 -0.07351958 0.08563596 -0.01384693 0.07498431 0.01052325 -0.006526947 -0.0634976 0.04618108 -0.03992569 -0.01366335 0.07454276 -0.07307624 0.0741167 0.01507437 -0.02004683 -0.0634976 0.08599996 -0.03898406 -0.008801937 0.07630985 -0.07344686 -0.05511665 0.06023883 -0.04932594 -0.07322859 0.05643665 -0.01505982 0.07477235 0.01052898 -0.01428186 0.07458001 0.01372992 -0.01455187 -0.04194325 0.07451838 -0.0609411 -0.06150662 0.08583217 -0.04199254 0.07489538 0.01282495 -0.008616983 -0.06150662 0.04770755 -0.04336369 0.07400619 0.01318264 -0.02572184 -0.02205497 0.07169425 -0.07222169 0.007066965 0.08599996 -0.07368415 -0.06670916 0.03429836 -0.04370665 -0.06863075 0.03070443 -0.03954875 -0.06131345 0.04607754 -0.04515022 -0.06073325 0.08388978 -0.04304349 0.07494908 0.01121544 2.85006e-4 0.001989901 0.08599996 -0.07397514 -0.07499337 0.01808917 9.99987e-4 -0.06073325 0.08599996 -0.04308617 -0.003703415 0.07651066 -0.07391774 -0.07476019 0.02663326 -0.005118966 -0.07490861 0.006244361 -0.001574993 -0.0581699 0.05440229 -0.04621118 0.04433608 0.08599996 -0.05961453 0.07494908 0.007128 -0.002501964 0.04678148 0.08576035 -0.05773288 -0.05465555 0.05918157 -0.04989796 -0.07068288 0.05980128 -0.02386498 -0.0747931 0.005699992 9.99987e-4 -0.07498317 0.01195067 -0.001010954 -0.03947412 0.07511508 -0.06255269 -0.07492697 0.02128911 -0.003120899 -0.008813083 0.07546287 -0.07366347 0.07463276 0.009281396 -0.02033299 -0.05769985 0.08599996 -0.04700446 -0.06073325 0.04957807 -0.04382884 -0.05406165 0.08599996 -0.05108582 -0.0704599 0.06003665 -0.02448081 -0.07421529 0.03327256 -0.009646952 0.07495665 0.006668925 -0.01240181 -0.05013787 0.06360977 -0.05426412 -0.05143529 0.06278866 -0.05306208 0.07458001 0.00812292 -0.02626496 -0.07499498 0.009500145 -0.005971968 -0.07476019 0.02026015 -0.006909966 -0.01293843 0.07388567 -0.07431328 -0.06885486 0.06212192 -0.02849525 -0.07492697 0.01415443 -0.006579995 -0.06389421 0.04096674 -0.04358375 -0.05406165 0.06100535 -0.05044925 0.0486201 0.08599996 -0.05622071 -0.07476019 0.02335846 -0.005784988 -0.07434558 0.02940315 -0.008934974 0.07495665 0.003625214 -0.01875698 -0.05013793 0.08599996 -0.05488955 0.07499426 0.0053882 -0.007514953 -0.06073325 0.04771167 -0.04506987 -0.05813789 0.05240505 -0.04706192 -0.07421529 0.03006833 -0.009850978 0.07497346 0.002338767 -0.009087979 -0.05465912 0.05753338 -0.05016875 0.0748592 0.004754424 -0.02459985 0.07497239 0.002374887 -0.02197498 0.07499998 0.001226544 -0.01638698 -0.07434558 0.02529603 -0.00988698 -0.07476019 0.0173527 -0.008393943 -0.01363605 0.07402139 -0.07365769 -0.07499068 0.005531132 -0.006574988 -0.07351958 0.03292125 -0.01378095 -0.008885324 0.07482337 -0.07417464 0.02596884 0.07735645 -0.06923127 0.003111362 0.07684767 -0.07394307 0.03270477 0.07398486 -0.06624275 0.0283637 0.07505965 -0.06822544 0.03703325 0.07273083 -0.06392329 -0.05463117 0.05576592 -0.05090427 0.01337397 0.08150887 -0.07278335 0.0119068 0.08071476 -0.07303464 -0.05092442 0.06188416 -0.05360245 0.01572078 0.079674 -0.07229578 -0.06840676 0.06249856 -0.02950596 -0.003995776 0.07581317 -0.07410818 0.02169203 0.07796525 -0.07070004 0.04174464 0.08123314 -0.06128376 -0.06110048 0.04405975 -0.04873657 -0.06388509 0.04011714 -0.04503357 3.9103e-4 0.0764454 -0.07402086 0.01204228 0.08599996 -0.07307165 -0.06670916 0.08573168 -0.03333008 0.04282236 0.07491135 -0.06035286 -0.008584499 0.07451719 -0.07496404 -0.0581454 0.05021613 -0.04860228 0.03997588 0.07318186 -0.06217867 0.04075336 0.07294797 -0.06167584 -0.07421529 0.02699625 -0.0104689 -0.07476019 0.01462864 -0.01036983 -0.004002988 0.075239 -0.07459443 -0.07499569 0.05061817 1.91018e-4 -0.07494497 0.04504746 -0.001839995 -0.0748021 0.05236673 -0.004393935 -0.05406165 0.08350098 -0.05102235 -0.07498967 0.02459615 -3.82992e-4 -0.07499969 0.03702998 0.001248896 -0.07499498 0.004926383 -0.01103186 -0.07471621 0.04127383 -0.005433976 -0.0749551 0.03316915 -0.001559913 -0.07491987 0.02744019 -0.002409875 -0.07484436 0.03450924 -0.00375694 0.05227404 0.08599996 -0.05288773 -0.008747339 0.08240115 -0.07349675 0.007999837 0.07959079 -0.07355809 0.01355326 0.07983624 -0.0727337 0.01610195 0.08149856 -0.07224339 0.01999723 0.08093476 -0.07126128 -0.04773706 0.08588385 -0.05695915 0.01704448 0.07859098 -0.07197529 0.0204553 0.07734847 -0.07105296 -0.07421529 0.02408987 -0.01151883 -0.07351958 0.02908802 -0.01466 -0.07322859 0.03105503 -0.01569396 0.02384912 0.07599747 -0.06995046 -0.07434558 0.02149087 -0.01153486 0.03270477 0.07962399 -0.06641924 0.03671485 0.07972109 -0.06432509 -0.0459395 0.06602984 -0.05780476 0.03997588 0.07698184 -0.06229907 0.03639626 0.07843106 -0.0644629 0.0373345 0.08194708 -0.06404536 0.04039657 0.07892274 -0.06209385 0.003752887 0.07595437 -0.07403665 0.0461753 0.08070665 -0.05806148 0.04505276 0.08270668 -0.05897504 0.003211915 0.07525837 -0.07465153 0.04364877 0.07856684 -0.05988419 2.63749e-4 0.07547187 -0.07450217 0.04203706 0.07662135 -0.06094366 0.0461753 0.07581847 -0.05791527 0.04440778 0.07176375 -0.05912667 0.04196333 0.07105726 -0.06081902 -0.06115388 0.04485267 -0.04712265 0.04607176 0.07206225 -0.05787622 -0.0722987 0.03656858 -0.01880395 0.04440778 0.06847536 -0.05902618 0.04035705 0.07017076 -0.06185895 -0.07322859 0.03390133 -0.01513087 -0.06150662 0.04293495 -0.04969716 0.04536217 0.06803447 -0.05830436 0.04646426 0.06926947 -0.05749416 -0.07484436 0.04593986 -0.003773987 -0.07434558 0.05404573 -0.008795976 -0.0742343 0.04891097 -0.009573936 -0.07421529 0.05439949 -0.009722948 -0.074997 0.01906037 -5.28989e-4 -0.07499659 0.02506858 0.001639008 -0.05813574 0.05113697 -0.0478698 -0.0742343 0.03832834 -0.009535968 -0.07434558 0.03264802 -0.008726954 0.003956437 0.08085978 -0.07389366 -0.07499068 0.001702547 -0.01214993 0.0351541 0.07587695 -0.06505614 0.01239734 0.0768339 -0.07290798 -0.008726775 0.07915157 -0.07347887 0.007942914 0.08080518 -0.07357126 0.007997155 0.08183145 -0.07356399 6.83465e-5 0.08010476 -0.07400137 0.005681753 0.08013725 -0.07377916 0.03359144 0.08155244 -0.06604045 0.01238775 0.07807409 -0.07292407 0.01097208 0.07764935 -0.07315099 0.01660567 0.07680886 -0.07204699 0.03360408 0.07754969 -0.0659101 -0.004310727 0.07488596 -0.07539129 -0.07200348 0.04006856 -0.01966494 0.04900687 0.08147364 -0.05575907 0.05125135 0.08287447 -0.05377691 0.05195868 0.08042907 -0.05304384 0.04952037 0.07781797 -0.05520856 0.04809653 0.08062046 -0.05650717 -0.04774194 0.06503278 -0.05634659 0.05281746 0.08240169 -0.05225312 -0.030945 0.07682883 -0.06715965 0.05605298 0.08360624 -0.04886889 0.04809653 0.07540625 -0.05635595 0.05455034 0.07272249 -0.05023753 0.05378115 0.07401406 -0.0510692 -0.04769837 0.06224745 -0.05670183 -0.06427258 0.08574694 -0.03771609 0.04866194 0.07019078 -0.05573076 0.04900175 0.06657946 -0.05533647 -0.04701197 0.06433874 -0.05699896 -0.04950326 0.08239537 -0.05534631 0.07498389 0.01658833 0.001060903 -0.05088037 0.05889946 -0.05448186 -0.05088657 0.06046962 -0.05386781 -0.07326626 0.05142194 -0.01486295 -0.07351958 0.05591583 -0.01369583 -0.07351958 0.03594946 -0.01359397 -0.07434558 0.01799917 -0.01390594 -0.07322859 0.03686916 -0.01494985 -0.07421529 0.01793724 -0.01547199 -0.009070456 0.08163559 -0.07345986 -0.07476019 0.009994029 -0.01542484 -4.11065e-5 0.07501 -0.07556194 0.05605298 0.08599996 -0.04892647 -0.009187698 0.07993066 -0.07343786 -1.61846e-4 0.08155977 -0.07399898 -0.002625167 0.08018118 -0.07395219 -0.004413545 0.08085536 -0.07386767 0.02995663 0.08213585 -0.06776165 0.007822394 0.07606059 -0.07359737 0.03100669 0.07883715 -0.06719326 0.03028589 0.07709705 -0.06746286 0.05813646 0.08248609 -0.04638928 0.05239635 0.07739257 -0.05253952 0.05512917 0.07694548 -0.04972559 0.05195868 0.07449114 -0.05288434 0.05378115 0.08032917 -0.05123138 0.05738788 0.08011025 -0.04724305 0.05455034 0.08136534 -0.05045485 0.05738788 0.07296776 -0.04707854 -0.0459395 0.08599996 -0.05840367 -0.05814141 0.04856383 -0.05052906 -0.0722987 0.05785447 -0.01877194 -0.07200348 0.05889004 -0.01980996 -0.07211625 0.05579257 -0.01939886 0.01698309 0.07507467 -0.07195085 -0.07166975 0.05121654 -0.02084797 -0.07211625 0.04207205 -0.01929599 -0.07326626 0.04161334 -0.01480782 -0.0725314 0.04500967 -0.01782995 -0.07200348 0.03731071 -0.01982599 -0.01505589 0.08149337 -0.07246077 -0.01178437 0.08212226 -0.07306098 -0.01114976 0.08073484 -0.07315397 -0.07492697 0.007449984 -0.01311182 -0.07322859 0.02666437 -0.01748597 -0.01700878 0.07943677 -0.07200205 0.01258367 0.07541775 -0.07291406 -0.04364454 0.06721544 -0.05953729 -0.01444727 0.07974708 -0.07256245 0.004993498 0.07808756 -0.07382398 6.80024e-4 0.0787307 -0.07399719 0.003240764 0.07934385 -0.07393795 -0.07351958 0.02553707 -0.01618885 0.003810882 0.07491308 -0.07542628 -0.05462509 0.05318808 -0.05292785 -0.07421529 0.02052164 -0.01347082 -0.07351958 0.02227991 -0.01839596 -0.05858129 0.04684787 -0.05250817 0.0269593 0.08106344 -0.0689609 0.02596884 0.08258605 -0.06937694 0.006414234 0.07747185 -0.07371026 -7.52021e-4 0.07820278 -0.07399594 6.3485e-4 0.07763296 -0.07399505 0.06035125 0.07594525 -0.04340302 0.05911087 0.07241207 -0.04495394 0.05782645 0.07645577 -0.04663479 0.05911087 0.07999408 -0.04511797 0.06123507 0.08323258 -0.04232245 0.06406706 0.08111327 -0.03797703 0.06198292 0.07977944 -0.04118448 0.02165329 0.0748381 -0.07063508 0.05227416 0.07080584 -0.05248284 0.0494349 0.06987994 -0.05505865 0.05490267 0.06683635 -0.04971754 -0.0546357 0.05435574 -0.05181282 0.0634433 0.07080715 -0.03880089 0.06198292 0.07138639 -0.04102396 0.06406706 0.06903606 -0.03777027 -0.07421529 0.01633626 -0.01708799 -0.07434558 0.01549458 -0.01626884 0.05402809 0.06728029 -0.05064374 0.05490267 0.06215304 -0.04960095 0.05191767 0.06564849 -0.05268585 0.03888237 0.07525098 -0.06291908 -0.07211625 0.04664564 -0.01932996 -0.0733605 0.04462808 -0.01439386 -0.07402175 0.04439467 -0.01092195 -0.0634976 0.06545746 -0.03862059 0.05952787 0.08599996 -0.04470777 -0.0713337 0.04480499 -0.02183896 -0.02674818 0.08260101 -0.06908518 -0.07068288 0.04280894 -0.02370196 0.0169028 0.07428038 -0.07216775 -0.04194319 0.08599996 -0.06130069 -0.01703643 0.08138674 -0.07202416 -0.02264177 0.08140701 -0.07048577 -0.02205044 0.08251827 -0.07069766 -0.02408957 0.08157819 -0.07001268 -0.02522778 0.08014124 -0.06957894 -0.02122259 0.07928526 -0.07087099 -0.01845943 0.0804857 -0.07166224 -0.004759252 0.07866406 -0.07384699 -0.03777176 0.08599996 -0.06392168 -0.04194325 0.06803786 -0.06073826 -0.07322859 0.02923387 -0.01628297 -0.006341934 0.07805478 -0.07371634 -0.004796445 0.07757598 -0.07384049 0.01936 0.0804463 -0.07142508 -0.04282057 0.06668567 -0.06016474 0.02362585 0.08011144 -0.07013744 0.02144086 0.07921165 -0.07079958 0.02692866 0.07899349 -0.06891316 0.0634433 0.07965826 -0.03895789 0.007723987 0.07521539 -0.07403057 -0.05009692 0.05746984 -0.05661594 -0.003066182 0.08158499 -0.07393598 0.05773568 0.06572139 -0.04649984 0.058411 0.06533646 -0.04567372 -0.04593539 0.06377774 -0.05800879 0.06604617 0.06964999 -0.03435039 0.06589317 0.06648719 -0.03458309 -0.02674818 0.07749527 -0.06894004 0.05952775 0.05666583 -0.04408335 0.07500016 0.02605116 0.001001 0.0260939 0.07351058 -0.06907939 -0.0722987 0.03387933 -0.01932895 -0.0694279 0.05536967 -0.02706956 -0.0703659 0.05664068 -0.02469998 -0.04104727 0.08279895 -0.06178385 -0.0703659 0.04827195 -0.02461594 -0.06866389 0.04848635 -0.02876317 -0.07489866 0.005078852 -0.01982986 0.06269538 0.08599996 -0.04023563 -0.02840054 0.08142215 -0.06840544 -0.03547596 0.08570259 -0.06519669 -0.02347844 0.07888275 -0.07016074 -0.05087935 0.05758595 -0.05533236 0.02164268 0.07353836 -0.07073539 0.02605158 0.07242196 -0.06920045 -0.01043015 0.07837289 -0.07323879 -0.04724818 0.08234846 -0.05725616 0.01241916 0.0743736 -0.07361495 -0.01178658 0.07767438 -0.07302129 -0.06073325 0.06702774 -0.04270195 -0.01376128 0.07860845 -0.07268697 -0.01003354 0.07720458 -0.07329595 0.0692467 0.07905244 -0.02777439 0.06637859 0.07443529 -0.03380054 0.0643993 0.07498818 -0.03732323 0.06604617 0.07941615 -0.03449773 0.0672962 0.06902188 -0.03193044 0.0672962 0.07928478 -0.03207165 -0.04277575 0.06561785 -0.06034874 0.06975281 0.08599996 -0.02660381 -0.07200348 0.03466546 -0.02034085 -0.07057136 0.04045265 -0.02415198 0.005470097 0.08153855 -0.07379549 0.07067728 0.08083516 -0.02408283 0.0700863 0.08086717 -0.02568894 0.008117079 0.07456964 -0.07486575 0.01278334 0.07391268 -0.07434004 0.06062448 0.06553936 -0.04281991 0.06816488 0.07386785 -0.03017807 0.06765949 0.06709223 -0.03116124 -0.03947412 0.06915146 -0.06236314 0.07079118 0.07285284 -0.02368396 -0.07434558 0.01360023 -0.01870596 0.0692467 0.06791126 -0.02764666 0.0700863 0.06543946 -0.02552795 0.07019209 0.06729608 -0.02526998 -0.02205044 0.07789409 -0.07058525 -0.05466496 0.05226784 -0.05404555 -0.05143529 0.05459779 -0.05827724 0.016999 0.07359009 -0.07263755 -0.06738138 0.06018543 -0.03163743 -0.06866389 0.05624145 -0.02885746 -0.06670916 0.06330227 -0.03300714 -0.06804001 0.05365896 -0.03018689 -0.06598776 0.06382489 -0.03436946 -0.06625139 0.05260562 -0.03371512 -0.06598776 0.04991263 -0.03415846 -0.07489877 0.004083812 -0.02401185 -0.03043717 0.08123767 -0.06752294 -0.03532356 0.08130449 -0.06514847 0.03061234 0.07121336 -0.06725108 0.0635271 0.08564466 -0.03893065 0.06526666 0.08599996 -0.03601282 -0.05087316 0.05589926 -0.05719047 -0.03876948 0.07908678 -0.06310945 -0.03213697 0.07966858 -0.06669318 -0.03043717 0.07835614 -0.06743508 -0.03547596 0.07598209 -0.06488746 -0.03753638 0.07706856 -0.06377196 -0.04513067 0.06316149 -0.05901265 -0.03498584 0.07775765 -0.06520497 -0.01830065 0.07842379 -0.07168626 -0.01376557 0.07646745 -0.07264989 0.06062448 0.05983108 -0.04270392 0.05773568 0.06056088 -0.04638242 0.05585885 0.06411635 -0.04861497 0.05836343 0.06104975 -0.04563665 0.071877 0.08432048 -0.02043586 -0.04693198 0.06091707 -0.05828493 0.07192957 0.07864463 -0.02021282 0.07019209 0.07892364 -0.02538985 0.06557989 0.08344936 -0.03540748 0.06211286 0.06244605 -0.04066407 0.07278478 0.08069586 -0.01708495 0.03062665 0.07213437 -0.06716346 -0.07476019 0.006903648 -0.02218997 -0.03873515 0.06865096 -0.06285309 -0.03777176 0.06986725 -0.06340718 0.06304877 0.05554234 -0.03913909 -0.07428038 0.01113992 -0.02535295 0.07192957 0.06596255 -0.02011394 0.07067728 0.06497126 -0.02392995 0.0675624 0.08599996 -0.03161615 0.07258957 0.06535136 -0.01774781 0.07278478 0.06293398 -0.01697093 0.07237017 0.06339699 -0.01855283 -0.05858129 0.06886547 -0.04554188 -0.05858129 0.08375245 -0.04587048 -0.0704599 0.04322487 -0.02431398 -0.06427258 0.06543415 -0.03737205 -0.06885486 0.04356002 -0.02842807 -0.07057136 0.03799241 -0.02462196 -0.06399697 0.06331205 -0.03778576 -0.06150662 0.06660985 -0.04161661 -0.0722987 0.02973175 -0.02101397 -0.0649181 0.05597823 -0.03613466 -0.06625139 0.05963385 -0.03381997 -0.06427258 0.05194246 -0.03714352 -0.04337269 0.080823 -0.06015038 -0.04123824 0.08090454 -0.06161051 -0.03714549 0.08104556 -0.06412476 -0.07351958 0.01916736 -0.02157998 -0.0459395 0.07344245 -0.05802696 -0.04337269 0.07637429 -0.06001275 -0.0722987 0.02671778 -0.02305096 -0.07322859 0.02347242 -0.01964694 -0.04123824 0.07676404 -0.06148034 0.02158576 0.07250058 -0.07137393 -0.02395462 0.07431066 -0.06987029 -0.0188176 0.07576805 -0.07147264 -0.01779747 0.0748986 -0.07173568 -0.01554268 0.07661896 -0.07228493 -0.07200348 0.03297299 -0.02088385 0.06304877 0.06177026 -0.03925186 0.07204425 0.07224267 -0.01977282 0.07237017 0.08072745 -0.01867681 0.07258957 0.07851684 -0.01783794 0.0648573 0.06171631 -0.03633016 0.02170807 0.07180005 -0.07235306 0.0735253 0.08296203 -0.01380497 -0.06885486 0.04593276 -0.02830195 0.0740171 0.08057677 -0.01109695 -0.03298693 0.08599996 -0.06648159 -0.07337409 0.01701617 -0.02683997 0.02604866 0.07118439 -0.06984704 0.07364779 0.07826536 -0.01315999 0.03492397 0.07062816 -0.06501936 0.03058189 0.07048577 -0.06747019 0.07422357 0.07808208 -0.009749948 0.0652666 0.05079215 -0.03545212 0.02165573 0.07203948 -0.07191276 -0.07057136 0.03641861 -0.02512294 0.0735253 0.05964535 -0.01368695 0.07425147 0.06074887 -0.00949496 0.07389569 0.05660605 -0.01171398 0.07364779 0.06414914 -0.01309198 0.06756246 0.08444255 -0.03159517 0.0744003 0.06042838 -0.008397936 -0.07495737 0.01386678 0.001397967 -0.05143529 0.08337348 -0.05362135 -0.05511665 0.08355575 -0.04990243 -0.04282099 0.06397575 -0.06097745 -0.03547596 0.07076925 -0.06472164 -0.05023467 0.08145147 -0.05467545 0.0305922 0.06964838 -0.06795608 -0.04485625 0.07842469 -0.05899876 -0.04754626 0.08064574 -0.05696296 -0.05769985 0.08369976 -0.04695194 -0.05511665 0.07033938 -0.04957556 0.03069758 0.06880885 -0.06873106 -0.07421529 0.01446658 -0.01949298 -0.06097209 0.06442558 -0.04232186 -0.06319266 0.06027919 -0.03900355 0.0683611 0.08564305 -0.02989631 -0.06220138 0.05694293 -0.04042917 -0.06097209 0.05864644 -0.04220604 -0.05769985 0.05822485 -0.04637175 -0.05858129 0.05748838 -0.04529076 -0.04712414 0.05982917 -0.05892574 -0.05769985 0.06853324 -0.0466066 0.03927636 0.06862145 -0.06250119 0.05605298 0.06961739 -0.04853236 -0.04754626 0.07552707 -0.05681324 0.03489583 0.06819379 -0.06563705 0.0744003 0.08052444 -0.008459985 0.0730803 0.07162344 -0.01579898 0.07389867 0.07099443 -0.01175898 0.07422357 0.06327337 -0.00969696 0.02610784 0.07055926 -0.07056796 -0.07322859 0.02118474 -0.02180486 0.0683611 0.04668444 -0.02940845 0.0638113 0.05828106 -0.03799706 0.06577587 0.06021267 -0.03469938 0.06711846 0.05373662 -0.03207343 0.06577587 0.05331349 -0.03459316 0.03487128 0.06939017 -0.06517916 0.06711846 0.06101095 -0.03217494 0.06975281 0.06074434 -0.02632999 0.0747624 0.07782465 -0.004955947 0.07473129 0.08307999 -0.00534296 0.07492768 0.08040195 -0.002289891 0.074952 0.08587497 -0.001625955 0.0749914 0.08594536 -1.46982e-4 0.0747624 0.06204307 -0.004925966 0.0748592 0.05366224 -0.003552973 0.07449209 0.07035988 -0.007679939 0.07483738 0.06177955 -0.003904879 0.07487291 0.07773894 -0.003357887 0.0392434 0.06719505 -0.06271857 0.07490807 0.01217216 0.002195954 0.04357206 0.06518876 -0.05978924 -0.07057136 0.03419888 -0.02615094 0.034904 0.06727439 -0.06635499 0.03047758 0.06853908 -0.0694884 -0.06840676 0.04428815 -0.02943009 0.03463006 0.06653755 -0.06779229 0.05952775 0.06765013 -0.04431724 -0.07322859 0.01945644 -0.02403187 0.04354834 0.06665498 -0.05961889 -0.03298693 0.07406663 -0.06610715 -0.028373 0.07847458 -0.06832677 0.04685556 0.06481599 -0.05706793 -0.03877604 0.06743657 -0.06299996 -0.02395462 0.08599996 -0.07017618 0.07023859 0.08547574 -0.0253328 -0.030945 0.07234978 -0.06702208 -0.03546309 0.06942409 -0.06478929 -0.06885486 0.04128307 -0.02885526 -0.06885486 0.03982633 -0.02931386 0.07157236 0.08599996 -0.02144694 -0.07200348 0.03058534 -0.02199596 -0.07200348 0.02837008 -0.02338397 -0.0192666 0.08599996 -0.07156747 -0.04695367 0.05908954 -0.06016784 0.03489118 0.06672316 -0.06707936 -0.07051557 0.03157645 -0.02815157 -0.06840676 0.04205405 -0.02984708 0.04274809 0.06371134 -0.06119483 -0.06840676 0.04661601 -0.02930784 -0.05801796 0.06889718 -0.04627454 0.04664742 0.06296598 -0.05753004 0.06269538 0.06636959 -0.03987336 -0.02853149 0.08599996 -0.06847935 0.04861259 0.06378036 -0.05560314 0.02159857 0.08599996 -0.07091856 -0.0722987 0.02222174 -0.02837425 -0.0722987 0.02383995 -0.02599495 0.07289218 0.08599996 -0.01668095 0.05159986 0.06179451 -0.05292707 0.03935837 0.06558269 -0.06341695 0.04356884 0.06418818 -0.06012785 0.0485115 0.06183695 -0.05598998 -0.02205044 0.08577084 -0.07077676 0.03933668 0.06429135 -0.06481486 0.0635271 0.06588637 -0.03858155 0.0652666 0.06480026 -0.03567522 0.04361188 0.06244421 -0.06121957 0.07389569 0.08599996 -0.01184082 0.07315069 0.08537745 -0.01557195 -0.06598776 0.04780101 -0.03426176 -0.06670916 0.04898995 -0.03280115 0.05160027 0.06020116 -0.05315244 0.04773485 0.06055659 -0.05751925 -0.05406165 0.07075005 -0.05069732 0.07458001 0.08599996 -0.006934881 -0.04774194 0.05785489 -0.06078827 0.05430907 0.05984526 -0.05019325 0.05605018 0.05851233 -0.04829543 -0.03873693 0.06614178 -0.06364047 0.05156606 0.05906593 -0.05353534 0.06619596 0.06367665 -0.03398329 0.04677295 0.06006866 -0.05917137 0.04283457 0.06157666 -0.06373089 -0.02674818 0.07359325 -0.06882917 0.05519884 0.05738306 -0.04952412 0.05771166 0.05717653 -0.04635947 -0.07200348 0.02687186 -0.02461397 -0.06670916 0.04681771 -0.03290969 -0.06840676 0.04062485 -0.03029572 0.0741167 0.08581274 -0.01049095 0.05428963 0.05683517 -0.05086195 0.04969584 0.05782532 -0.05692017 0.05155116 0.05733937 -0.05448186 -0.02853149 0.07308977 -0.06809788 -0.0320515 0.07101547 -0.06653589 -0.01448702 0.08599996 -0.07264649 -0.05511665 0.06967395 -0.04955929 -0.0428496 0.06278729 -0.06188184 0.04357105 0.06154912 -0.06241303 0.0476582 0.05852466 -0.05969256 0.0683611 0.06199634 -0.02960026 -0.04594105 0.05934453 -0.06176894 0.05594718 0.05490088 -0.04929298 0.05425429 0.05530846 -0.05170994 0.05762207 0.05487728 -0.04684525 0.07489496 0.08599996 -0.00297296 0.04857069 0.05720531 -0.06008517 0.05078995 0.05519849 -0.05873638 0.04678148 0.05863434 -0.06139785 -0.07322859 0.01635771 -0.03149127 0.05425947 0.05380219 -0.05291116 -0.06670916 0.04473263 -0.03329157 -0.03543984 0.06797945 -0.0653125 0.05161643 0.05523324 -0.05658215 0.06269538 0.05363965 -0.0396384 0.06019008 0.05438184 -0.04330629 0.06080198 0.05550289 -0.04237556 -0.02752995 0.07221704 -0.06857174 0.05605298 0.05284976 -0.05045723 0.05771106 0.05325967 -0.0473271 -0.01771599 0.0858289 -0.07195097 0.07023859 0.06026417 -0.02507597 0.06029665 0.05227768 -0.04351669 0.0635271 0.05276298 -0.03834974 -0.02395462 0.07764458 -0.06995749 0.07157236 0.06004261 -0.02122998 0.05952775 0.05187886 -0.04492938 -0.03947407 0.06378304 -0.06551527 -0.04364454 0.06100529 -0.06327366 -0.04274934 0.06208586 -0.06294339 0.05429548 0.05175393 -0.0561285 -0.06427258 0.04996412 -0.03723555 0.05771106 0.05197691 -0.04805529 0.06029635 0.04996049 -0.04447484 0.05771106 0.05072069 -0.04901278 0.05605298 0.05172014 -0.05151915 0.06311106 0.05133557 -0.03911572 0.0652666 0.04873842 -0.03555059 0.0631113 0.04948484 -0.03940016 -0.07200348 0.02549713 -0.02600085 -0.03872019 0.06525135 -0.06437546 0.06080198 0.04762828 -0.04498249 0.07188087 0.06178843 -0.02024394 0.07289206 0.05833673 -0.01650798 0.06619596 0.04752212 -0.03387814 0.03075248 0.08599996 -0.06752789 0.06619596 0.04965072 -0.0337736 0.05431205 0.05261826 -0.05430394 0.0631113 0.04776388 -0.03998625 -0.07200348 0.02389442 -0.02806907 0.05952775 0.04958647 -0.04619556 0.05771106 0.04791563 -0.05328679 0.0631113 0.04589939 -0.04094594 -0.03140276 0.06959968 -0.06742739 0.0652666 0.04676645 -0.03590637 0.02165329 0.08251208 -0.07081824 0.05952775 0.04786014 -0.04776239 -0.0634976 0.05087244 -0.03848373 0.06756246 0.04783564 -0.03110289 -0.05143529 0.07170563 -0.05330437 0.05605298 0.05087614 -0.05262356 0.05771106 0.04905235 -0.0509631 0.06619596 0.04547858 -0.03425055 0.06756246 0.04558783 -0.03121805 0.06028789 0.04601007 -0.04809844 0.0652666 0.04490226 -0.03654754 0.06269538 0.04571872 -0.04203838 0.0633192 0.04337525 -0.04245269 -0.06670916 0.04276138 -0.03397423 -0.06427258 0.04806447 -0.03757464 -0.06598776 0.04577398 -0.03463023 0.0652666 0.04261714 -0.03776854 0.06756246 0.04274028 -0.03183269 0.06080198 0.04392737 -0.05040407 0.06619596 0.04354643 -0.03491806 0.05952775 0.04619854 -0.05024462 -0.06863075 0.03819108 -0.03074145 0.0683611 0.04436105 -0.02953016 0.06975281 0.04447382 -0.02615398 -0.07057136 0.02798408 -0.03178936 0.06080198 0.04455304 -0.04883438 -0.02103883 0.07401144 -0.07084816 -0.0704599 0.02975124 -0.03014034 0.0741167 0.05612415 -0.01037794 0.07023859 0.04362636 -0.02490592 -0.03205305 0.07008868 -0.06674468 -0.03880435 0.06459474 -0.06512147 0.07458001 0.05485332 -0.006855964 -0.07200348 0.02269548 -0.03011578 0.06975281 0.04200536 -0.02628898 0.06315839 0.04206514 -0.04458576 0.0652666 0.03876525 -0.04159885 0.06619596 0.04117786 -0.03618633 -0.06885486 0.03522384 -0.0319736 0.07157236 0.04102343 -0.02107197 0.07468557 0.05447202 -0.005798876 0.07023859 0.04110229 -0.02504599 0.0652666 0.04096674 -0.03903478 0.06756246 0.04010426 -0.03294104 -0.0634976 0.04902625 -0.03881067 0.0683611 0.04213124 -0.02994602 0.06975281 0.03963679 -0.02673786 0.0699957 0.03840076 -0.02636194 0.07157236 0.03832823 -0.02122694 0.071873 0.04036498 -0.02010095 -0.06670916 0.04034447 -0.03527015 -0.07215106 0.02134776 -0.03171926 0.0749315 0.05316632 -0.002177894 0.0631113 0.04129576 -0.04604333 0.06619596 0.03718286 -0.04015904 0.06619596 0.03946685 -0.03750038 0.06756246 0.03769063 -0.03456085 0.07497239 0.05274903 -0.001020908 0.06975281 0.03598445 -0.0282045 0.0683611 0.03869336 -0.03131914 0.07289206 0.03783965 -0.01637995 -0.0634976 0.05279475 -0.03839647 -0.06115955 0.05341345 -0.04197955 0.0749914 0.05242985 -1.34986e-4 -0.02390176 0.07315695 -0.06993979 0.07315069 0.05794113 -0.01541286 0.07315069 0.03710156 -0.01529186 -0.06885486 0.03340077 -0.03369158 -0.03544718 0.06675803 -0.06632435 0.0683611 0.03441369 -0.03467702 0.06756246 0.03596484 -0.03618669 0.07023859 0.03713095 -0.02599895 -0.06598776 0.04264926 -0.03586655 0.06352698 0.03988504 -0.04754817 0.071873 0.03762656 -0.02026093 0.07289206 0.03493553 -0.01655495 -0.06598776 0.04447728 -0.03503215 0.07172268 0.03537148 -0.02124881 -0.06840676 0.03610974 -0.03290265 0.0683611 0.03619813 -0.0329957 0.06975281 0.03400236 -0.02944076 0.07023859 0.03494572 -0.0270099 0.07172268 0.03291285 -0.02212196 0.07315069 0.03414905 -0.01547181 0.06619596 0.03527861 -0.04437178 0.0683611 0.03306996 -0.03641659 -0.06112009 0.05519825 -0.04192829 0.07389569 0.03460997 -0.01161897 -0.02314299 0.07224786 -0.07073354 -0.009125649 0.08599996 -0.07347095 0.06756246 0.03466635 -0.03786915 0.06619596 0.03626924 -0.04173755 0.071873 0.03094851 -0.02241295 0.0699957 0.03147763 -0.03060305 0.06975281 0.03000468 -0.03362762 0.06885486 0.04128307 0.030855 0.06885486 0.03982645 0.03131401 -0.02170807 0.07793653 0.07269203 -0.05048727 0.05716824 0.05830287 0.06269538 0.04131305 0.05003899 0.06670916 0.03859823 0.03861188 0.07068288 0.04023063 0.02584683 0.07157236 0.04102218 0.023072 0.07157236 0.03832775 0.02322697 0.06427258 0.04035955 0.04505085 0.07068288 0.03775805 0.02631998 0.06885486 0.03522384 0.03397399 0.06756246 0.03888905 0.03566998 0.06975281 0.03598445 0.03020387 0.003073275 0.08274024 0.07593202 0.06670916 0.03626656 0.04132586 0.0652666 0.03994834 0.04206287 0.07289206 0.03783828 0.01837998 0.0722987 0.03937053 0.02063786 0.0722987 0.0365678 0.02080386 0.06427258 0.03867197 0.04870384 0.06885486 0.03777223 0.03226 0.06756246 0.03708142 0.03705996 0.07068288 0.03322583 0.02824801 0.07289206 0.03493475 0.01855498 0.0652666 0.0378853 0.04512184 0.07351958 0.03594791 0.01559388 0.06756246 0.03596484 0.03818684 0.06885486 0.03202784 0.03746783 0.07157236 0.03574293 0.02372694 0.06975281 0.0333324 0.03198999 -0.01278334 0.08159536 0.074898 0.06670916 0.03429836 0.04570686 -0.0424152 0.06579905 0.06260198 0.07068288 0.03541904 0.02714598 0.07157236 0.03175574 0.02534186 0.06975281 0.03143477 0.03377896 0.0672425 0.03439205 0.04197984 0.0722987 0.03387933 0.02132898 0.07157236 0.0288589 0.02729797 -0.0383566 0.06889504 0.06507998 0.0722987 0.02973175 0.02301388 0.0688548 0.02975755 0.04267483 0.07434558 0.06408476 0.01082789 0.07289206 0.03215014 0.01910299 0.07351958 0.03292042 0.01578098 0.07068288 0.03117406 0.02972584 0.07351958 0.03001713 0.01635783 -0.01636606 0.07776302 0.07411903 0.07389569 0.02850556 0.01441186 0.07289206 0.02951556 0.02004599 0.07289206 0.02704411 0.02129501 0.07434558 0.03264641 0.01072686 0.07389569 0.03460842 0.01361888 0.07389569 0.03149294 0.01381486 0.07157236 0.02521896 0.03127396 0.07068288 0.02919083 0.03159499 0.07068288 0.02769577 0.03352689 0.06885486 0.03100377 0.03922802 0.07458001 0.03133744 0.008796989 -0.05047392 0.05621135 0.05955201 0.0722987 0.02671778 0.02505087 0.06975281 0.03000468 0.03562796 0.07434558 0.02940237 0.01093488 0.07476019 0.08571416 0.006998002 0.07458001 0.02800744 0.009012997 0.07434558 0.02629154 0.01156085 -0.07421529 0.08599996 0.01183485 0.07289206 0.02473086 0.02296698 0.07351958 0.02553713 0.01818901 0.07351958 0.02227991 0.02039599 -0.0748592 0.08228003 0.005592942 -0.0748021 0.08599996 0.006450951 0.07495528 0.08587646 0.003592908 -0.04640793 0.06034493 0.06146585 -0.0749315 0.08228498 0.004205942 0.0722987 0.02455842 0.02708798 -0.07468557 0.08567285 0.007866024 -0.07322859 0.08599996 0.01722699 -0.0741167 0.08581274 0.01249086 0.07476019 0.03004711 0.006893992 0.07389569 0.02567863 0.01542997 0.06975281 0.0279771 0.03988587 -0.07497239 0.07838666 0.003036916 -0.002312481 0.08037775 0.07596302 -0.003810882 0.08055555 0.07590103 0.07068288 0.02657842 0.03543984 0.07476019 0.02663242 0.00711894 0.07498377 0.02709287 0.002536952 -0.0464099 0.06124603 0.06071496 0.0749402 0.02222794 0.004648983 -0.05078995 0.05519843 0.060736 -0.04678148 0.05863434 0.063398 0.07434558 0.02149087 0.01353496 0.07458001 0.02481454 0.009660005 0.07289206 0.02249282 0.02507787 0.07351958 0.01818037 0.024872 0.01490366 0.08300697 0.07449102 0.07495528 0.02777791 0.003547966 0.07476019 0.02335858 0.007784962 0.07068288 0.02510821 0.03964591 0.07157236 0.02261382 0.03718799 -0.0749914 0.07061147 0.002142012 -0.0749914 0.05074244 0.002134919 0.07389569 0.02220857 0.01732701 0.07289206 0.02080255 0.02725696 0.07112771 0.02449852 0.03630089 0.0722987 0.02292823 0.02919 0.07389569 0.01969558 0.01927191 0.07458001 0.01988655 0.01168888 -0.07045978 0.08599996 0.02673786 0.07389569 0.01553303 0.02449798 0.07434558 0.0154944 0.018269 0.07434558 0.01888203 0.01517885 -0.07497239 0.05107992 0.003019988 0.07498967 0.02120172 0.002864897 0.07289206 0.01953673 0.02941197 -0.0748592 0.05204516 0.00555098 -0.03833818 0.06681537 0.06561201 0.07482749 0.01747792 0.008919 -0.07200348 0.08599996 0.02201801 -0.0741167 0.06349605 0.01240599 0.07458001 0.01630187 0.01412487 -0.07315069 0.08554494 0.01757287 0.07476019 0.01462864 0.01236999 -0.071873 0.08577126 0.02245998 0.07259535 0.01937931 0.03348398 -0.07468557 0.0529012 0.007794976 0.07389569 0.01733833 0.02185183 -0.0748021 0.05236673 0.006393969 -0.07322859 0.07261323 0.01715087 -0.03831547 0.06786203 0.06524097 0.07458001 0.01372992 0.01655197 -0.03394055 0.07047033 0.06756699 -0.04236346 0.06434518 0.06321698 -0.07023859 0.08547574 0.02733302 -0.04639339 0.05951565 0.06255501 0.07351958 0.01487827 0.03324598 -0.07421529 0.05439949 0.01172298 0.07492506 0.01121312 0.01087898 -0.06840687 0.08599996 0.03179889 -0.07200348 0.07327866 0.02192085 0.07370758 0.01474893 0.02919501 0.07434558 0.01360023 0.02070587 -0.03834795 0.06594914 0.066159 -0.0704599 0.07393515 0.026618 0.07499545 0.009438812 0.005973935 -0.06840676 0.07464045 0.03165799 0.07476019 0.009994029 0.01742601 0.0749796 0.01221925 0.007653951 0.07458001 0.01178413 0.01905399 -0.06619596 0.08565688 0.03631198 0.03075248 0.08599996 0.06952804 -0.07315069 0.06707507 0.01746588 -0.07322859 0.05643665 0.01705998 -0.071873 0.06815993 0.02231997 -0.03399205 0.06929498 0.06779402 -0.0683611 0.07952857 0.03181988 0.07467007 0.008235692 0.02356696 -0.07023859 0.07931464 0.02726984 -0.04248505 0.06305736 0.06408101 -0.06619596 0.07527166 0.03615587 0.07490861 0.006244361 0.003573954 0.07490861 0.003898382 0.005704939 0.07446277 0.009402573 0.02763301 -0.04239279 0.06234842 0.06510996 -0.07498186 0.02444404 0.00286597 -0.04283457 0.06157666 0.06573098 -0.0634976 0.08599996 0.04098397 0.07500016 0.004448354 0.01238197 0.07495528 0.006845593 0.01456201 -0.07200348 0.05919843 0.02181202 0.07491087 0.004766881 0.02175498 -0.02957499 0.07111662 0.06980997 -0.03399616 0.06838417 0.06824803 -0.06598776 0.08599996 0.03670603 0.07476019 0.006116211 0.02765095 0.07495528 0.002863347 0.02468883 -0.027134 0.07266032 0.07067501 0.07501 0 0.05378997 0.07498377 0.001981258 0.023229 -0.07483065 0.0242834 0.006503999 -0.06351238 0.05277782 0.04037398 -0.06479179 0.05799412 0.03838199 -0.06073325 0.05556637 0.04447001 -0.06351238 0.06544846 0.04059797 -0.06218189 0.06019663 0.04252094 -0.05925405 0.06234645 0.04655498 -0.05769985 0.05822426 0.04837197 -0.05603039 0.06441646 0.05043298 -0.05417859 0.06092107 0.05232685 -0.05245018 0.06642895 0.054196 0.03306555 0.07388722 0.06806302 0.03749287 0.07258617 0.065656 0.04176038 0.07113152 0.062958 0.04433608 0.07342863 0.06123 0.04605793 0.06944173 0.059816 -0.06073325 0.08599996 0.0450859 0.04364454 0.06721496 0.06153684 -0.0749315 0.02642077 0.004272997 -0.03837645 0.06489717 0.06725901 -0.07116305 0.05637097 0.02445197 -0.0704599 0.06003665 0.02648097 -0.0704599 0.04322355 0.02631402 -0.0707817 0.04688853 0.02546596 -0.04570907 0.07130604 0.06014001 -0.04527956 0.07145923 0.06046998 -0.04848057 0.06837695 0.057832 -0.0459395 0.06602936 0.05980485 -0.04549413 0.06797152 0.060202 0.026223 0.07731837 0.07113498 0.02175647 0.07451641 0.07260298 -0.07023859 0.06924402 0.02716797 0.0287624 0.07496803 0.07005703 0.02521288 0.07486522 0.07144099 0.05070787 0.07071727 0.05593889 -0.07023859 0.06026417 0.027076 0.04880106 0.07013523 0.05761086 0.04902064 0.06620675 0.05731087 0.05143529 0.06278806 0.05506187 0.0499807 0.06502813 0.05644285 0.07499569 0.02659475 0.001802921 -0.07492709 0.04524552 0.004269957 -8.52335e-4 0.07948422 0.075957 -0.07499969 0.03702944 7.51013e-4 -0.0749551 0.03316801 0.003559947 -0.071873 0.05841684 0.02224385 -0.07224476 0.05639255 0.02095401 -0.07315069 0.05707687 0.01740801 -0.07499575 0.01390624 0.004815995 -0.06961506 0.05885446 0.02865201 -0.07494169 0.01799505 0.006014943 -0.07208406 0.04216367 0.02140688 -0.06838387 0.06845265 0.03162992 -0.07224476 0.04717886 0.020886 -0.06961506 0.04672622 0.02851796 -0.0635271 0.07995396 0.04082989 -0.04213696 0.07023113 0.06267601 -0.04234957 0.06727623 0.06246 0.02149367 0.07603132 0.07271301 0.0253241 0.07658082 0.07144701 0.05185765 0.06134861 0.05470889 0.05260956 0.06562274 0.05401986 -0.03398305 0.06768494 0.068856 -0.07499748 0.02745825 3.80002e-4 -0.07499909 0.0226581 0.001036942 -0.07499426 0.01263916 0.003351986 -0.07483065 0.02939826 0.005937993 -0.0746656 0.03331327 0.007963955 -0.07318967 0.0369839 0.01712083 -0.0736069 0.04514366 0.01520699 -0.06598776 0.07532602 0.03654396 -0.073309 0.04072088 0.01660984 -0.06851357 0.0570504 0.03120297 -0.06935858 0.05312258 0.02921098 -0.07074338 0.05547523 0.02565389 -0.06851357 0.05127096 0.03113085 -0.06731456 0.05551016 0.03370785 -0.03997808 0.07282686 0.064166 -0.03844887 0.07059615 0.06503099 0.01581585 0.07713454 0.07423299 0.01724565 0.07620584 0.07388299 0.04924041 0.06995874 0.05722898 0.05569386 0.06662225 0.05085802 -0.05429548 0.08576756 0.05283486 0.05436146 0.06730413 0.05229902 -0.06838387 0.0619769 0.03154885 0.05260956 0.06273382 0.05394399 -0.07494217 0.00842297 0.003295958 0.05511665 0.06023824 0.05132585 0.05605298 0.05953174 0.05028986 -0.0746656 0.03886646 0.007975995 -0.0749247 0.03574723 0.004314005 -0.0746656 0.04997313 0.008000969 -0.074117 0.05017495 0.01235389 -0.073309 0.05055862 0.01666384 -0.07422637 0.04735624 0.01162201 -0.07421529 0.03327095 0.01164686 -0.0741167 0.03370893 0.01229298 -0.07422637 0.04206866 0.01160287 -0.07421529 0.03006774 0.01185184 -0.06739658 0.05036383 0.03347301 -0.03844887 0.07328724 0.065117 -0.03518939 0.07531458 0.06701701 -0.0371899 0.07246667 0.06582599 -0.03387624 0.07241398 0.06761097 0.01131469 0.07709276 0.07509601 0.05812937 0.06557065 0.04802298 0.05917388 0.06496334 0.04671299 0.05667865 0.06374526 0.0496869 0.05569386 0.06181067 0.05074089 0.05917388 0.05953836 0.04659599 -0.07468557 0.02094757 0.009737968 -0.0741167 0.03053462 0.01249498 -0.02853149 0.07695293 0.070212 -0.02864813 0.07529062 0.07011401 -0.03369557 0.07478773 0.06778001 0.008584499 0.07905524 0.07548803 -0.03038346 0.07212734 0.06927198 0.01000016 0.07779413 0.07529503 0.006133973 0.07750874 0.075733 0.06098425 0.06536805 0.04432398 0.00230211 0.08035975 0.07596802 -0.03022485 0.07638841 0.06946903 -0.02550595 0.07668393 0.07138502 -0.03070574 0.07383513 0.06919902 0.00254184 0.07897317 0.07595503 0.06267845 0.06206262 0.04181885 0.0632146 0.06166863 0.040995 0.06267845 0.05591773 0.04170584 0.06098425 0.05958604 0.04420799 0.06269538 0.05363887 0.04163783 -0.06619596 0.06367665 0.03598284 -0.07492458 0.003578186 0.006674945 -0.07498389 0.004193544 0.009732007 -0.07468557 0.02724903 0.007968008 -0.02520668 0.07499873 0.07144701 -0.01987802 0.07642596 0.073197 0.06503301 0.06236082 0.03804188 -0.03461855 0.06654351 0.06974899 -0.05078995 0.08580476 0.05628997 0.06427258 0.05194157 0.03914386 0.0632146 0.05540221 0.04088199 0.06503301 0.05565482 0.03793299 -0.05769985 0.08599996 0.04900497 -0.02187556 0.07686811 0.07262998 -0.01856184 0.07804614 0.073583 0.0670523 0.05827015 0.03426688 0.06634098 0.05893117 0.0356419 0.0670523 0.05101484 0.03416585 -0.07495737 0.01386815 6.02007e-4 -0.01274186 0.07845824 0.07486701 0.06861287 0.05836397 0.03099501 0.0692752 0.05679148 0.02945202 0.06885486 0.04593163 0.03030186 -0.06073325 0.07650226 0.04489398 -0.07489067 0.009725451 6.04006e-4 0.07049477 0.05767345 0.02636301 0.07049477 0.04925286 0.02627998 0.06767755 0.05433136 0.03295385 0.06975281 0.04447275 0.02815401 0.07058888 0.04392528 0.02596998 0.0692752 0.04882735 0.02936089 -0.05013793 0.08599996 0.05689001 -0.02575707 0.07142996 0.07179999 -0.07483065 0.01563704 0.01009798 -0.07468557 0.02401143 0.008625984 -0.07495665 0.01036363 0.01018798 -0.07416599 0.02724343 0.01278698 -0.07483065 0.01198065 0.01313591 -0.02273374 0.07268393 0.07266998 -0.02273792 0.07359522 0.07231801 -0.07499575 0.004978537 0.01286399 -0.0295251 0.06979733 0.070634 -0.02867138 0.06962633 0.07161802 0.006318271 0.07858926 0.07572001 -0.07318967 0.03402453 0.01730102 -0.0459395 0.08599996 0.060404 0.03515416 0.08599996 0.06737798 -0.07421529 0.02225607 0.01441484 -0.01447296 0.07610315 0.074503 -0.05770558 0.07914298 0.04884201 -0.07468557 0.01718533 0.011801 0.02648556 0.08144927 0.07115101 -0.07414126 0.02448225 0.013987 0.01724565 0.07757312 0.07390803 -0.06598776 0.07133364 0.03648293 -0.0749492 0.005497455 0.01750499 -0.01815366 0.07465577 0.073673 -0.06598776 0.06382483 0.03636986 -0.022731 0.07193243 0.073291 -0.07318967 0.02850085 0.01882499 -0.009125649 0.07675367 0.075405 -0.01799565 0.07360732 0.07417303 -0.04283457 0.08580303 0.06268298 -0.01347118 0.07522243 0.074786 -0.07468557 0.01361513 0.01476699 -0.07483065 0.009093582 0.01660686 -0.01278334 0.07635736 0.07483303 -0.0749914 0.001671016 0.02242797 -0.0741167 0.02107423 0.01607888 -0.07421529 0.01446652 0.02149289 -0.04194319 0.08599996 0.06330102 -0.05013787 0.07879817 0.05668902 0.04433608 0.08599996 0.06161499 -0.008129179 0.07787156 0.07553499 0.03947412 0.08268743 0.06479299 -0.07318967 0.03118622 0.01786297 -0.02596884 0.07037115 0.07307898 -0.02170729 0.07180023 0.07434499 0.01211619 0.07826912 0.07498502 -0.07193815 0.03483265 0.02255588 -0.03888237 0.08587342 0.06525701 -0.07193815 0.03746795 0.022044 -0.07193815 0.04021543 0.02188301 -0.008523643 0.07575744 0.07556498 -0.00315541 0.07646602 0.07594799 -0.07034927 0.04088765 0.02675098 -0.0749315 0.003431618 0.02535986 -0.07318967 0.02520471 0.02062201 -0.07468557 0.007970452 0.02410298 -0.03463006 0.08593016 0.06765103 -0.07416599 0.01743203 0.01856786 -0.07318967 0.02134132 0.02395695 -0.05429548 0.07519036 0.052567 -0.01712232 0.07302963 0.07543599 -0.05406165 0.07845693 0.05289387 -0.07468557 0.01079684 0.01815599 -0.07193815 0.03076702 0.02420485 0.02193737 0.08008146 0.07267302 0.003610253 0.07620692 0.07593798 -0.07193815 0.03314614 0.02309685 -0.003878593 0.07540506 0.07636803 0.0308488 0.07655125 0.06919503 -0.07468557 0.006968021 0.02825099 -0.07034927 0.0361514 0.02802687 -0.03298693 0.08599996 0.06848198 -0.01278334 0.07391262 0.07634001 -0.07023859 0.04362523 0.02690601 0.04433608 0.08050823 0.06144702 0.002448618 0.07709705 0.07595801 -0.07318967 0.01961815 0.026178 -0.008685648 0.07483023 0.076231 -0.07193815 0.02855956 0.02558696 -0.07416599 0.01216673 0.02711397 -0.06073325 0.06702774 0.04470199 -0.05769985 0.06853324 0.04860699 -4.86675e-4 0.07571297 0.07631999 -0.07034927 0.03399264 0.02910995 0.05227404 0.08599996 0.05488801 -0.02395462 0.08599996 0.07217603 -0.07034927 0.03845375 0.027215 -0.01349794 0.07421314 0.07541501 0.0486201 0.08259886 0.05812299 0.039837 0.08599996 0.06467199 -0.0741167 0.01507437 0.02204591 -0.02853149 0.08599996 0.07047897 0.05143529 0.08588832 0.05568999 -0.02170807 0.08573174 0.07287901 0.04774206 0.08166927 0.05883193 0.008022546 0.07623654 0.07556098 -0.02596884 0.08568012 0.07146298 0.0351541 0.07570707 0.06705099 -0.06838387 0.04664915 0.031358 -0.06838387 0.0443241 0.03147995 -0.06598776 0.04991143 0.03615885 -0.0192666 0.08599996 0.07356798 -0.05429548 0.07001322 0.05243599 0.03947412 0.07472223 0.06453996 -0.07193815 0.02410078 0.03025496 -0.06838387 0.04066485 0.03234487 -0.06840676 0.03925824 0.03287297 0.039837 0.07463276 0.06431204 -0.0732091 0.01673287 0.03236585 -0.07193815 0.02706652 0.02681398 -0.05078995 0.07680052 0.05604201 -0.06838387 0.04209274 0.03189599 -0.07193815 0.02569693 0.02819496 0.003694355 0.07520872 0.07665997 -0.06609189 0.04766124 0.03606986 -0.06609189 0.04562634 0.03644084 -0.01717579 0.08576744 0.07407599 -0.07315069 0.01659727 0.03429889 -0.07318967 0.01832622 0.02837598 0.01269167 0.07555824 0.07487899 -0.003795266 0.07491326 0.07726997 -0.04283457 0.07385063 0.06231099 -0.06609189 0.04189842 0.03799897 -0.07034927 0.03197348 0.03056401 -0.05078995 0.07135212 0.05589199 0.01728159 0.07493132 0.07388502 -0.009125649 0.08599996 0.07547098 0.05605298 0.08599996 0.05092597 -0.0704599 0.02827233 0.034051 0.05858129 0.08587002 0.04791688 0.03547596 0.07563865 0.066877 -0.07023859 0.02819627 0.03562384 -0.06838387 0.03678452 0.03443288 -0.06609189 0.04307955 0.0373649 -0.04678148 0.08072042 0.05958288 0.05952787 0.08599996 0.04670786 -0.07197088 0.02144205 0.03586399 0.04364454 0.07362568 0.06173503 5.99554e-5 0.07500976 0.077286 0.05143529 0.07800412 0.05547487 0.007908642 0.07537347 0.07585698 -0.06351238 0.04900825 0.04078799 0.04774206 0.07238936 0.05856084 -0.07023859 0.03029257 0.03266698 -0.06838387 0.03302276 0.03837388 -0.06838387 0.03436762 0.03663301 0.01729357 0.07398563 0.07420504 -0.04213696 0.07327806 0.06277197 0.06269538 0.08599996 0.04223603 0.06150662 0.08583217 0.04399287 0.02664721 0.07324022 0.07087498 -0.07193815 0.02261966 0.03292596 -0.05013787 0.07158327 0.05648702 -0.06609189 0.04432433 0.03684401 -0.06351238 0.0478267 0.04114896 0.004310727 0.07488596 0.07739102 0.01255846 0.07442635 0.07548803 -0.06351238 0.05085504 0.04045999 0.05511665 0.07979238 0.05180901 0.001989901 0.08599996 0.075975 0.02193605 0.07346534 0.07265102 0.01293843 0.07388561 0.07631301 0.06427258 0.08574694 0.039716 -0.04678148 0.07269454 0.05934602 -0.06609189 0.03963881 0.03967601 0.02652645 0.07245212 0.07099503 -0.03888237 0.07486587 0.06490701 -0.0702387 0.02632546 0.04043185 0.05605298 0.07930016 0.05076485 0.06150662 0.07703357 0.04382085 0.03083759 0.07211011 0.06906497 0.05605298 0.06927382 0.05052387 -0.06351238 0.04669713 0.04161685 0.01771599 0.07288783 0.07535398 0.01725953 0.07324784 0.07495903 0.06526666 0.08599996 0.03801298 0.03089207 0.07097905 0.06914001 0.05227416 0.07080584 0.05448287 0.06670916 0.08573162 0.03532987 0.05227416 0.07882702 0.05469602 0.05511665 0.06967395 0.05155897 0.02183067 0.07244062 0.07330298 0.03531557 0.07047915 0.06680399 -0.06076765 0.05211836 0.04478389 -0.06076765 0.05104756 0.04510498 0.02182948 0.07198476 0.07388198 0.02649569 0.07157856 0.07133001 -0.06076765 0.05379211 0.04449594 0.06269538 0.07758456 0.04207998 -0.06619596 0.03841066 0.04056698 -0.07034927 0.02670127 0.03794384 0.05858129 0.07453966 0.04766684 0.02648657 0.07084435 0.07189798 0.03086256 0.07017832 0.06943899 0.0353173 0.06824076 0.06729501 -0.06076765 0.04814112 0.046642 -0.06598776 0.04095482 0.038917 -0.0683611 0.03087472 0.04341387 0.03084737 0.06924897 0.07006698 0.06885486 0.08572053 0.03077685 0.004387557 0.08586353 0.07587903 0.04433608 0.06686723 0.06102889 0.03531754 0.06920617 0.06693398 0.03966104 0.06611377 0.064812 0.07068288 0.08554983 0.02611297 -0.06351238 0.04562586 0.042189 0.06975281 0.08599996 0.02860397 0.03965687 0.06858563 0.06423801 0.02672928 0.07008582 0.072757 0.03085708 0.06864243 0.070818 -0.06351238 0.04293334 0.04433298 0.001762032 0.08492827 0.07597303 -0.06076657 0.04643756 0.04816287 0.007066965 0.08599996 0.07568401 0.03963416 0.06720143 0.06442701 0.0652666 0.07630836 0.03785902 0.06427258 0.06497466 0.03936398 0.04398918 0.06606346 0.06132286 0.05952775 0.06765013 0.04631698 0.04806369 0.06399804 0.05809998 -0.06838387 0.03202003 0.04009902 -0.05770665 0.0551477 0.04867386 -0.06609189 0.03645455 0.0438959 0.0722987 0.08586114 0.02097499 -0.06076765 0.04905337 0.04604101 0.03535789 0.06715685 0.06800401 0.04398858 0.06494057 0.06148386 0.07157236 0.08599996 0.02344685 0.030945 0.06832933 0.07130897 0.03531455 0.06650555 0.06887102 0.03974145 0.06483036 0.06560599 -0.05770736 0.05665242 0.04842299 0.04815191 0.06259036 0.0582109 0.06269538 0.06593602 0.04186499 -0.06598776 0.03876322 0.04091 -0.06609189 0.03736495 0.04232299 0.04399186 0.06369274 0.06194198 -0.03777176 0.07512766 0.065575 -0.02395462 0.07764452 0.071958 0.07289218 0.08599996 0.01868093 0.07351958 0.08563596 0.01584684 0.03966218 0.06406664 0.06666302 0.008632242 0.08573794 0.07552599 0.04812759 0.06156313 0.05854201 0.04398936 0.06258708 0.06264996 0.04397732 0.06132882 0.06404602 0.04820168 0.06022375 0.05914402 -0.06076765 0.05002397 0.04552698 0.06670916 0.06330227 0.03500694 -0.003625214 0.08599996 0.07591801 -0.05419296 0.05952715 0.05236589 0.05189985 0.05967426 0.05492699 -0.06351238 0.04104304 0.04691702 0.07389569 0.08599996 0.01384097 0.05559957 0.05844193 0.05087399 0.06885486 0.06157064 0.0304889 -0.05770444 0.05239397 0.04979389 0.05858129 0.05748754 0.04729098 0.0722987 0.05785447 0.02077198 0.06756246 0.06264853 0.03330188 0.05559676 0.05704665 0.05110287 0.06975281 0.06074434 0.02832984 0.05952775 0.05666506 0.04608297 0.05187612 0.05706703 0.05622202 0.07434558 0.08589667 0.01089799 -0.05078995 0.06320112 0.05566698 0.04817348 0.05931693 0.05989789 0.05558478 0.05613136 0.05134797 0.05188757 0.05802172 0.05560398 0.05903446 0.05544817 0.04681485 0.05905258 0.05365103 0.04710102 0.04815536 0.05815404 0.06129288 -0.06619596 0.03527861 0.04637187 0.07068288 0.05980128 0.02586489 -0.06080198 0.04392737 0.05240386 -0.05419087 0.05794382 0.05262798 -0.05773335 0.05365723 0.04910784 0.04774194 0.05785483 0.062788 -0.05418765 0.05502313 0.054039 -0.06076765 0.04719877 0.04741001 0.06150662 0.05482852 0.04338598 0.07489496 0.08599996 0.004972994 0.05858129 0.05333006 0.04790902 0.0518946 0.05496007 0.05838799 0.07499569 0.08596104 0.001816928 0.01686877 0.08599996 0.07415103 0.05182415 0.05609571 0.05711185 0.05557626 0.05373716 0.05265599 0.06150662 0.05132216 0.04376101 -0.05047798 0.06218487 0.05600798 0.05559355 0.05488014 0.051907 -0.05013787 0.06360918 0.05626398 -0.06077355 0.04533207 0.04955285 0.07498377 0.08592545 0.002564907 0.07500016 0.08599996 9.99987e-4 0.06207865 0.05245774 0.04267001 0.05511665 0.05087876 0.05747497 0.05143529 0.05459773 0.06027698 0.06269538 0.04997962 0.04203587 -0.06076765 0.04459846 0.05087298 0.07157236 0.05878973 0.02322 0.07289206 0.05698645 0.01849997 -0.05418998 0.05619663 0.05330389 0.05560499 0.05136024 0.05506998 0.06427258 0.04996383 0.03923588 0.05903702 0.05151772 0.04795998 0.06150662 0.04817593 0.04505497 -0.05048245 0.06100255 0.05618298 -0.05053126 0.05985647 0.05647403 0.06269538 0.04828453 0.04261189 0.05555355 0.05259639 0.05364584 0.05905085 0.04880505 0.04991489 -0.04639106 0.06479203 0.05948597 0.05902987 0.05015224 0.04883599 0.06150662 0.04969751 0.04430997 0.06670916 0.04898875 0.03480088 0.05558478 0.05059921 0.05650496 0.06427258 0.04806447 0.03957498 -0.0541892 0.05426943 0.05464798 0.06756246 0.04783463 0.03310286 0.0652666 0.04873806 0.03755086 0.0652666 0.04613584 0.03810089 0.06670916 0.04681736 0.03490996 0.06269538 0.04620754 0.043715 0.07351958 0.06215173 0.01572787 0.06150662 0.04675555 0.04606801 -0.05770826 0.04903537 0.05296397 -0.05418807 0.05366474 0.055242 0.05858129 0.04804474 0.052046 0.06427258 0.04513669 0.04072499 0.07389569 0.06424742 0.01374685 0.06269538 0.04470849 0.04486286 0.06756246 0.04558748 0.03321784 0.06150662 0.04538673 0.04735487 0.05952775 0.04743885 0.0503 -0.05044126 0.05851399 0.05728203 0.06670916 0.04473274 0.03529191 -0.04641288 0.06342983 0.05966889 0.05858129 0.04684782 0.05450797 -0.05771106 0.04791563 0.055287 0.06885486 0.04355955 0.03042799 0.0652666 0.04372704 0.03910601 0.05952775 0.04619854 0.05224502 0.06269538 0.04225087 0.04797887 0.06269538 0.04331135 0.04639798 0.06427258 0.04354941 0.04170584 0.06150662 0.04436045 0.04869186 0.02159857 0.08599996 0.07291901 0.06756246 0.04343032 0.03361696 0.0652666 0.04152292 0.04058086 0.06756246 0.04139024 0.03432589 0.06975281 0.04200494 0.02828896 0.06975281 0.03963685 0.02873796 0.06427258 0.04149806 0.04357099 0.06150662 0.04293495 0.05169689 -0.05419135 0.05278754 0.05635988 -0.04639703 0.06205946 0.06022089 0.06670916 0.0415185 0.03656697 0.06670916 0.03977543 0.03764885 0.05895996 -0.0461114 0.03801697 0.06038558 -0.0444979 0.03840595 0.05706185 -0.04868716 0.03695785 0.06253689 0.04136562 0.068255 0.06483608 0.0368523 0.06677603 0.06493437 0.03755015 0.06677103 0.05345839 -0.05261838 0.03559899 0.0541591 -0.05161535 0.03610897 0.06774687 0.03166574 0.064978 0.04974263 -0.05614387 0.03437983 0.06967848 0.02716362 0.06341701 0.04850977 -0.05701524 0.03423798 0.07120406 0.02358973 0.06194496 -0.001294851 0.07478624 0.07992303 0.04075103 -0.06280201 0.03223186 0.04562038 -0.05954205 0.03320497 0.07264655 0.01868075 0.06024789 0.04127722 -0.06263118 0.03213685 0.00479418 0.07485586 0.07981097 0.009579598 0.07439494 0.07965302 0.03708475 -0.06520134 0.03124886 0.01435273 0.07362341 0.07938098 0.07374697 0.01370662 0.05852788 0.07384216 0.01192826 0.05813586 0.03236037 -0.06767064 0.03039497 0.03615659 -0.06555742 0.031277 0.07454597 0.008330047 0.05666995 0.07479166 0.001588523 0.05455183 0.07486766 0.004618585 0.05538588 0.01903247 0.07255512 0.07887303 0.02365589 0.07118177 0.07854402 0.07430016 -0.008781671 0.05095696 0.07473856 -0.003330171 0.05284684 0.02748376 -0.06979364 0.02966094 0.02246308 -0.07156753 0.02904796 0.07482826 -0.005218029 0.05198585 0.03482377 0.06640505 0.07693201 0.0326038 0.06755352 0.07714402 0.01544862 -0.07322359 0.02861994 0.03480345 0.06615531 0.07693397 0.03689783 0.06530714 0.07636702 0.07356607 -0.01365059 0.04926997 0.04390019 0.06050914 0.07497602 0.04104375 0.06278455 0.075495 0.0720852 -0.02074182 0.04661995 0.07336419 -0.0156269 0.04838699 0.07237935 -0.01897913 0.047423 0.006914675 -0.0746904 0.02796787 0.003458619 -0.07493013 0.02788496 0.07046067 -0.02572518 0.04489588 0.07012927 -0.02599573 0.04499089 0 -0.07501 0.02785784 0.04537075 0.05973243 0.07444 0.04916435 0.05665117 0.07337498 0.06704211 -0.03325963 0.042472 0.05212748 0.05359303 0.07257902 0.0558536 0.04970276 0.07122999 0.06842297 -0.0307374 0.04316395 0.06472575 -0.03757637 0.04097586 0.0660662 -0.0355212 0.04150986 0.05930107 0.04553991 0.06978696 0.05616617 0.04967576 0.07111901 0.063385 -0.04011029 0.03992295 0.05627536 0.04959392 0.07093501 0.06213635 -0.04172611 0.03953784 0.05940568 0.04570662 0.06975698 0.06244587 0.04112941 0.068259 -0.05895996 0.04598021 0.06993997 -0.05583596 0.04972255 0.07123702 -0.06493437 -0.03755021 0.04080796 -0.05706185 0.04868716 0.07062101 -0.05248206 0.0532462 0.07245898 -0.0672937 -0.03313702 0.04233396 -0.04974263 0.05614382 0.07319998 -0.06935316 -0.02857679 0.04390984 -0.06967848 -0.02729481 0.04453986 -0.005304336 -0.074687 0.02811199 -0.04562038 0.059542 0.07437402 -0.07218497 -0.01951748 0.04723584 -0.07120406 -0.02358973 0.04563498 -0.0408625 0.06285494 0.07569497 -0.07384216 -0.01205939 0.04982084 -0.009573459 -0.07439661 0.02806997 -0.0152989 -0.0732944 0.02859497 -0.03708475 0.06520122 0.07633101 -0.07374697 -0.01370668 0.04905086 -0.07454597 -0.008330106 0.05090987 -0.07464629 -0.006514847 0.05168288 -0.02627944 0.06965911 0.078148 -0.07465606 -0.004178583 0.05255287 -0.03205049 0.06774276 0.07739698 -0.07486766 -0.004618585 0.05219286 -0.01903247 -0.07255518 0.02870595 -0.07473856 0.0031991 0.05510985 -0.02749228 0.06968647 0.07809102 -0.01733475 0.07297897 0.07915997 -0.07491898 0.001590788 0.05448395 -0.07501 0 0.05378997 -0.02748376 0.06979352 0.077919 -0.07482826 0.005217969 0.05559384 -0.02246308 0.07156741 0.07853204 -0.07430016 0.0086506 0.05699998 -0.03712725 -0.0649569 0.03148496 -0.03689783 -0.06530719 0.03121197 -0.07356607 0.01351964 0.05868798 -0.07237935 0.01884806 0.06053501 -0.07336419 0.01562684 0.059192 -0.04104375 -0.06278461 0.03208386 -0.0121904 0.07401204 0.079521 -0.00687021 0.07469433 0.07975202 -0.04813218 -0.05733484 0.03412687 -0.0720852 0.02074182 0.06095999 -0.00344187 0.07493066 0.07983499 -0.07046067 0.02572512 0.06268298 -0.06842297 0.03073734 0.06441599 -0.04537075 -0.05973249 0.03313899 -0.04916435 -0.05665117 0.0342049 -0.05399048 -0.05177903 0.036053 -0.05275428 -0.0533244 0.03535497 -0.06704211 0.03312849 0.065485 -0.0660662 0.03552114 0.06606996 -0.06213635 0.04159504 0.06841999 -0.063385 0.04011029 0.06765699 -0.05930107 -0.04567086 0.03816998 -0.05627536 -0.04959398 0.03664499 -0.05941665 -0.04578369 0.0379619 -0.06244587 -0.0412606 0.03969883 -0.06230336 -0.04177057 0.0393489 -0.04798173 0.05765604 -0.07899999 -0.05840265 0.04657465 -0.07899999 -0.05586522 0.05005556 -0.07899999 -0.0436452 0.06100475 -0.07899999 -0.05937016 0.04584395 -0.07899999 -0.06469208 0.03734982 -0.07899999 -0.06255817 0.04138809 -0.07899999 -0.06171995 0.04207998 -0.07899999 -0.0673024 0.03241091 -0.07899999 -0.0342977 0.06670957 -0.07899999 -0.07138127 0.02201819 -0.07899999 -0.07006049 0.02679586 -0.07899999 -0.0728271 0.01662224 -0.07899999 -0.0166223 0.0728271 -0.07899999 -0.005582332 0.07449108 -0.07899999 -0.07449108 0.005582332 -0.07899999 -0.07421058 0.01092135 -0.07899999 -0.07386571 0.01113349 -0.07899999 -0.002739489 0.07495999 -0.07899999 -0.07501 0 -0.07899999 -0.0748099 0.005475342 -0.07899999 0 0.07469999 -0.07899999 0.005582332 0.07449108 -0.07899999 -0.0748099 -0.005475342 -0.07899999 0.008203923 0.07455998 -0.07899999 -0.07321548 -0.01630938 -0.07899999 0.0166223 0.0728271 -0.07899999 -0.07182955 -0.02161026 -0.07899999 -0.0728271 -0.0166223 -0.07899999 -0.07138127 -0.02201819 -0.07899999 0.02421927 0.07099235 -0.07899999 -0.07006049 -0.02679586 -0.07899999 0.02729094 0.06953626 -0.07899999 -0.06953626 -0.02729094 -0.07899999 -0.0679177 -0.03183835 -0.07899999 -0.0673024 -0.03241097 -0.07899999 -0.06255817 -0.04138809 -0.07899999 0.0436452 0.06100475 -0.07899999 0.04657465 0.05840265 -0.07899999 -0.05840265 -0.04657471 -0.07899999 -0.05586522 -0.05005556 -0.07899999 0.05080878 0.05475884 -0.07899999 0.05475884 0.05080872 -0.07899999 0.05586522 0.05005556 -0.07899999 -0.05206245 -0.05399996 -0.07899999 -0.0436452 -0.06100475 -0.07899999 0.06171995 0.04207998 -0.07899999 0.0654124 0.03671103 -0.07899999 -0.03734987 -0.06469208 -0.07899999 0.0679177 0.03183835 -0.07899999 -0.02421927 -0.07099241 -0.07899999 0.06953626 0.02729094 -0.07899999 -0.01897245 -0.07257097 -0.07899999 -0.01362442 -0.07376229 -0.07899999 0.07421058 0.01092135 -0.07899999 0.07321548 0.01630938 -0.07899999 -0.005582332 -0.07449108 -0.07899999 -0.01113349 -0.07386571 -0.07899999 0.07449108 0.005582332 -0.07899999 0.07386571 0.01113349 -0.07899999 -0.008203923 -0.07455998 -0.07899999 0 -0.07469999 -0.07899999 0.07469999 0 -0.07899999 0.07449108 -0.005582332 -0.07899999 0.07421058 -0.01092135 -0.07899999 0.0166223 -0.0728271 -0.07899999 0.01113349 -0.07386571 -0.07899999 0.07386571 -0.01113349 -0.07899999 0.02201819 -0.07138127 -0.07899999 0.0728271 -0.0166223 -0.07899999 0.07006049 -0.02679586 -0.07899999 0.0673024 -0.03241097 -0.07899999 0.0654124 -0.03671103 -0.07899999 0.0679177 -0.03183835 -0.07899999 0.0342977 -0.06670957 -0.07899999 0.03907567 -0.06402796 -0.07899999 0.04207998 -0.06172001 -0.07899999 0.06171995 -0.04207998 -0.07899999 0.04798173 -0.05765604 -0.07899999 0.05840265 -0.04657471 -0.07899999 0.05586522 -0.05005556 -0.07899999 0.06255817 0.04138809 -0.07899999 0.05937016 0.04584395 -0.07899999 0.0672937 0.03313696 0.06524497 -0.07497346 0.002338767 0.01108801 -0.07499998 0.001226544 0.01838696 0.06935316 0.02857673 0.06366902 -0.07497239 0.002374887 0.02397495 -0.07496517 0.002593338 -0.008066952 0.07006049 0.02679586 -0.07899999 0.0683611 0.03087472 -0.04141396 -0.07499998 0.001226425 -0.01766598 -0.07498377 0.001981258 -0.02122884 0.071873 0.02146553 -0.03532522 0.0702387 0.02632546 -0.03843176 -0.07487028 0.004576742 0.003802001 -0.07490861 0.003898382 -0.003704965 -0.0748592 0.004754424 0.0266 0.07315069 0.01659727 -0.03229928 0.07182955 0.02161026 -0.07899999 -0.0742467 0.01067346 0.05747985 -0.07476019 0.006116211 -0.02565085 -0.07495528 0.002863407 -0.02268898 -0.0741167 0.01154184 0.031192 0.07434546 0.00996226 0.03020596 -0.07434546 0.00996226 -0.02820557 0.07468557 0.006968021 -0.02625083 0.0741167 0.01154184 -0.02919209 -0.07351958 0.01487827 -0.03124547 0.07489877 0.004083812 0.026012 0.07487028 0.004576742 -0.001801908 -0.071873 0.02146553 0.03732496 -0.07321548 0.01630938 -0.07899999 -0.0722987 0.01998484 -0.03439617 -0.07068288 0.02510821 -0.03764575 0.07496517 0.002593338 0.01006686 -0.07182955 0.02161026 -0.07899999 -0.0679177 0.03183835 -0.07899999 -0.0688548 0.02975755 -0.04067528 0.07492458 0.003578186 -0.004674971 0.0748099 0.005475342 -0.07899999 -0.06352698 0.03988504 0.04954797 0.07499068 0.001702547 0.01414996 -0.0654124 0.03671103 -0.07899999 0.0749315 0.003431737 -0.02335995 0.07501 0 -0.07899999 -0.06427258 0.03867197 -0.04670357 0.0749914 0.001671016 -0.02042794 -0.02933675 0.06903517 -0.07899999 -0.01771599 0.07288789 -0.07335418 -0.01362442 0.07376229 -0.07899999 -0.06038558 0.04449784 0.06917297 0.002739489 0.07495999 -0.07899999 0.01897245 0.07257097 -0.07899999 0.02933675 0.06903517 -0.07899999 0.0722987 0.01998484 0.03639584 0.05275428 0.05332434 0.07222497 0.03947407 0.06378304 0.06751501 0.02818137 0.06951475 0.07782202 0.008522868 0.07452404 0.07680803 -0.008129179 0.07456815 0.07701998 -0.05511665 0.05087876 -0.05547446 -0.03047746 0.06853902 0.071487 -0.03888237 0.06414556 0.06781202 -0.05429577 0.05175364 0.05796796 -0.05345839 0.05261832 0.07198101 -0.05206245 0.05399996 -0.07899999 -0.04127722 0.06263113 0.07544201 -0.03236037 0.06767052 0.07718402 -0.03547585 0.06609046 -0.06741845 -0.03907567 0.06402796 -0.07899999 -0.02421927 0.07099235 -0.07899999 -0.01897245 0.07257097 -0.07899999 0 0.07500994 0.07972198 -0.008203923 0.07455998 -0.07899999 0.02205044 0.07169562 0.07425898 0.01362442 0.07376229 -0.07899999 0.01709026 0.0730372 -0.07339835 0.02596884 0.07037115 -0.07107865 0.03547585 0.06609046 0.06941902 0.0342977 0.06670957 -0.07899999 0.03907567 0.06402796 -0.07899999 0.04364752 0.06100291 0.06526303 0.03888237 0.06414556 -0.06581157 0.04798173 0.05765604 -0.07899999 0.05206245 0.05399996 -0.07899999 0.06230336 0.04177051 0.06823098 0.05941665 0.04578363 0.06961798 0.07002717 -0.02329576 0.04640489 0.0687772 -0.02676159 0.04518193 0.07042789 -0.02205032 0.04693597 0.07042789 -0.02205032 0.04693597 -0.069628 0.02733469 -0.0876984 -0.07147657 0.02204906 -0.08769959 -0.06540369 0.03670614 -0.08744996 -0.06790858 0.03183424 -0.08744996 -0.003625214 0.08599996 -0.07391744 -0.009125649 0.08599996 -0.07347095 0.07480001 0 -0.08899998 0.0745908 0.005589783 -0.08899998 -0.02732735 0.06962937 -0.08899998 -0.02204757 0.07147675 -0.08899998 0.01686877 0.08599996 -0.07215064 0.01204228 0.08599996 -0.07307165 -0.005582332 -0.07449108 -0.07899999 -0.01113349 -0.07386571 -0.07899999 -0.05936217 0.04583787 -0.08879995 -0.06254976 0.04138255 -0.08879995 -0.07181996 0.02160727 -0.08880001 -0.0732057 0.01630729 -0.08880001 0.07289218 0.08599996 -0.01668095 0.07389569 0.08599996 -0.01184082 -0.0634976 0.08599996 0.04098397 -0.06598776 0.08599996 0.03670603 0.01770126 -0.07195222 0.02905982 0.02531147 -0.069696 0.02984184 0.02531147 -0.069696 0.02984184 0.06739217 -0.0324558 -0.08769971 0.069628 -0.02733469 -0.0876984 -0.07479989 -0.005474627 -0.08569997 -0.0742008 -0.01091998 -0.08569997 -0.06477659 -0.03740596 -0.08769857 -0.06180036 -0.04214179 -0.08769869 -0.02933675 -0.06903517 -0.07899999 -0.0342977 -0.06670957 -0.07899999 -0.0742008 0.01091992 -0.08744996 -0.07479989 0.005474627 -0.08744996 -0.002739489 -0.07495999 -0.07899999 -0.008203923 -0.07455998 -0.07899999 -0.02420938 0.07098549 -0.08778619 -0.02932214 0.069031 -0.08779025 -0.05588299 0.04848545 0.07099801 -0.05588299 0.04848545 0.07099801 -0.05909574 0.04448044 0.06962901 -0.07181996 0.02160727 -0.08744996 -0.0732057 0.01630729 -0.08744996 -0.0648126 0.03613305 0.06656199 -0.06835287 0.02916157 0.06410998 -0.07322859 0.08599996 0.01722699 -0.07200348 0.08599996 0.02201801 -0.05205547 0.05399262 -0.08879995 -0.04797548 0.05764842 -0.08879995 0.02299517 0.07037502 0.078574 0.01929366 0.07149577 0.078924 0.007066965 0.08599996 -0.07368415 -0.05775976 -0.046126 0.03812783 0.02384793 -0.06984752 0.03007584 0.03293508 -0.06620573 0.03113687 0.03293508 -0.06620573 0.03113687 0.03293508 -0.06620573 0.03113687 0.03293508 -0.06620573 0.03113687 -0.03907036 0.0640195 -0.08879995 -0.04363936 0.06099665 -0.08879995 0.03200995 0.06668841 0.077376 0.02773147 0.06866765 0.07795298 0.07147866 -0.02206355 -0.08769446 0.07268106 -0.01262289 0.05221194 0.07331568 -0.008056581 0.05393087 0.07331568 -0.008056581 0.05393087 -0.03907036 -0.0640195 -0.08744996 -0.03429317 -0.06670069 -0.08744996 -0.0732057 -0.01630729 -0.08880001 -0.07181996 -0.02160727 -0.08880001 -0.07292455 0.01664626 -0.08769959 0.07320475 0.01631027 -0.0877912 0.07181918 0.02161008 -0.08782351 0.05080878 0.05475884 -0.07899999 0.05475884 0.05080872 -0.07899999 0.06790649 0.03183865 -0.0877884 0.06540125 0.03671067 -0.08782267 0.06253689 0.04136562 0.068255 0.06253689 0.04136562 0.068255 0.06253689 0.04136562 0.068255 0.05940568 0.04570662 0.06975698 0.04537075 0.05973243 0.07444 0.04916435 0.05665117 0.07337498 0.02421927 0.07099235 -0.07899999 0.02933675 0.06903517 -0.07899999 0.0436452 0.06100475 -0.07899999 0.03907567 0.06402796 -0.07899999 0.01896995 -0.07256126 -0.08880001 0.01362276 -0.0737524 -0.08880001 0.06038558 -0.0444979 0.03840595 0.063385 -0.04011029 0.03992295 -0.04850977 0.0568841 0.07371997 -0.04363936 -0.06099671 -0.08880001 -0.03907036 -0.0640195 -0.08880001 0.07386571 -0.01113349 -0.08569997 0.0728271 -0.0166223 -0.08569997 -0.0326038 -0.06755363 0.03043586 -0.02818137 -0.06951481 0.02975696 0.06472575 -0.03757637 0.04097586 0.05895996 -0.0461114 0.03801697 0.06213635 -0.04172611 0.03953784 0.01897245 0.07257097 -0.07899999 -0.05087673 0.05483222 -0.08899998 -0.05483222 0.05087673 -0.08899998 0.07005006 -0.02679514 -0.08782088 0.06790548 -0.0318408 -0.08778697 -0.0742008 -0.01091998 -0.08880001 -0.02421927 -0.07099241 -0.07899999 0.06583166 -0.0332669 0.04516386 -0.03245449 0.06739246 -0.08899998 0.04657465 0.05840265 -0.08569997 0.04207998 0.06171995 -0.08569997 -0.02732563 -0.06963098 -0.08769911 -0.02204602 -0.07147806 -0.08769917 0.06540369 0.03670614 -0.08879995 0.06254976 0.04138255 -0.08879995 -0.01664423 -0.07292479 -0.08769977 0.0342977 0.06670957 -0.07899999 -0.07396459 0.01115018 -0.08769959 -0.07459175 0.005599856 -0.0876978 0.07421058 0.01092135 -0.07899999 0.07321548 0.01630938 -0.07899999 -0.05585777 0.05004876 -0.08879995 -0.07181996 0.02160722 -0.08569997 -0.07005119 0.02679216 -0.08569997 0.07292151 0.01667219 -0.08769667 0.07396477 0.01115775 -0.08769798 -0.01895827 -0.07256489 -0.08778947 -0.0136168 -0.07375377 -0.08778977 0.07250088 0.01622533 0.05962598 0.07047486 0.02312409 0.06205499 0.07296144 0.01238214 0.05843091 0.07161468 0.01849573 0.06057995 0.03739988 0.06477868 -0.08899998 0.03245449 0.06739246 -0.08899998 -0.06155246 -0.04043382 0.04286599 -0.06351709 -0.03749603 0.04374688 -0.05585777 0.05004876 -0.08744996 -0.05936217 0.04583787 -0.08744996 -0.02933287 -0.06902599 -0.08744996 -0.02421599 -0.07098299 -0.08744996 0.05616617 0.04967576 0.07111901 0.05616617 0.04967576 0.07111901 0.05616617 0.04967576 0.07111901 0.0718187 -0.02161175 -0.08783376 0.06587535 -0.0332728 0.04294598 0.06680679 -0.03135818 0.04357784 -0.06935316 -0.02857679 0.04390984 -0.07120406 -0.02358973 0.04563498 0.02204757 -0.07147681 -0.08899998 0.01664447 -0.07292461 -0.08899998 0.07449108 -0.005582332 -0.07899999 0.07386571 -0.01113349 -0.07899999 0.0673024 0.03241091 -0.07899999 0.06953626 0.02729094 -0.07899999 -8.52335e-4 0.07948422 0.075957 0.00230211 0.08035975 0.07596802 -0.01896995 -0.07256126 -0.08744996 0.06738257 0.03248137 -0.0876972 0.06962919 0.02732872 -0.08769965 0.02421599 0.07098299 -0.08744996 0.01896995 0.07256126 -0.08744996 0.07370746 -0.003014862 0.05546885 -0.07449108 -0.005582332 -0.07899999 -0.07386571 -0.01113349 -0.07899999 0.07046067 -0.02572518 0.04489588 0.06842297 -0.0307374 0.04316395 -0.03463006 0.08593016 0.06765103 -0.03298693 0.08599996 0.06848198 -0.03298693 0.08599996 0.06848198 0.07004916 0.02679717 -0.08778756 0.07396477 -0.01115781 -0.08769798 0.07459175 -0.005599856 -0.0876978 0.07459175 -0.005599856 -0.0876978 0.07459175 -0.005599856 -0.0876978 -0.005589783 0.0745908 -0.08899998 -0.01114839 0.07396447 -0.08899998 0.07458001 0.08599996 -0.006934881 0.07489496 0.08599996 -0.00297296 -0.06739246 0.03245449 -0.08899998 -0.06477868 0.03739982 -0.08899998 0.04213625 0.06180268 -0.08899998 -0.02421599 0.07098299 -0.08744996 -0.02933287 0.06902599 -0.08744996 0.0728271 -0.0166223 -0.07899999 -0.0147016 -0.07226473 0.03181886 -0.05937016 -0.04584395 -0.07899999 -0.06255817 -0.04138809 -0.07899999 0.06255817 -0.04138809 -0.07899999 0.05937016 -0.04584395 -0.07899999 -0.07006049 0.02679586 -0.07899999 -0.0679177 0.03183835 -0.07899999 -0.07200348 0.08599996 -0.02001798 -0.07045978 0.08599996 -0.02473795 0.04213374 0.06180536 -0.08769899 0.04660779 0.05850934 -0.08769637 0.06456059 -0.03575891 0.04204183 -0.05586522 -0.05005556 -0.07899999 0.07449108 -0.005582332 -0.08569997 -0.001294851 0.07478624 0.07992303 -0.00344187 0.07493066 0.07983499 -0.00344187 0.07493066 0.07983499 0.0166223 -0.0728271 -0.07899999 0.01113349 -0.07386571 -0.07899999 0.05585777 0.05004876 -0.08879995 0.05205547 0.05399262 -0.08879995 -0.0742008 -0.01091998 -0.08744996 -0.07479989 -0.005474627 -0.08744996 -0.06840687 0.08599996 -0.02979916 0.02933675 -0.06903517 -0.07899999 0.0342977 -0.06670957 -0.07899999 0.0720852 -0.02074182 0.04661995 0.07336419 -0.0156269 0.04838699 -0.07419717 -0.003745377 0.05270296 -0.07419717 -0.003745377 0.05270296 -0.07237839 -0.01644825 0.04829984 -0.07237839 -0.01644825 0.04829984 -0.07182955 0.02161026 -0.07899999 -0.04974263 0.05614382 0.07319998 -0.04562038 0.059542 0.07437402 -0.07005119 0.02679216 -0.08744996 0.002739489 -0.07495999 -0.07899999 -0.003052473 0.08245146 0.07593399 -0.003052473 0.08245146 0.07593399 -0.002312481 0.08037775 0.07596302 0.07237935 -0.01897913 0.047423 0.07138127 -0.02201819 -0.08569997 0.02933287 0.06902599 -0.08879995 0.02421599 0.07098299 -0.08879995 -0.02596884 0.07037115 0.07307898 0 -0.07480001 -0.08899998 0.005589783 -0.0745908 -0.08899998 -0.04663449 -0.05848413 -0.08769899 -0.05087429 -0.05483609 -0.08769887 0.03515416 0.08599996 -0.06537777 0.039837 0.08599996 -0.06267237 0.04663687 0.05848085 -0.08899998 -0.06254976 -0.04138255 -0.08880001 -0.05936217 -0.04583787 -0.08880001 -0.0728271 0.01662224 -0.08569997 -0.07386571 0.01113349 -0.08569997 -0.03777176 0.08599996 0.06592202 -0.03777176 0.08599996 0.06592202 -0.04194319 0.08599996 0.06330102 0.02201819 -0.07138127 -0.07899999 -0.002739131 -0.07494997 -0.08744996 0.002739131 -0.07494997 -0.08744996 -0.01114827 -0.07396471 -0.08769977 -0.04363507 0.06099987 -0.08778905 -0.04797089 0.05765259 -0.08783346 0.06790858 0.03183424 -0.08744996 0.06540369 0.03670614 -0.08744996 0.03245449 -0.06739246 -0.08899998 0.03739988 -0.06477868 -0.08899998 0.0326038 0.06755352 0.07714402 0.02818137 0.06951475 0.07782202 -0.069628 -0.02733469 -0.0876984 -0.07147198 -0.02207326 -0.0876969 -0.008202791 -0.07455009 -0.08744996 -0.03428345 -0.06670618 -0.08778989 -0.02932214 -0.069031 -0.08779031 0.005582332 -0.07449108 -0.08569997 0 -0.07469999 -0.08569997 -0.06469208 -0.03734987 -0.08569997 -0.06171995 -0.04207998 -0.08569997 0.06953626 -0.02729094 -0.08569997 0.0673024 -0.03241097 -0.08569997 0.04118186 0.06154787 0.075459 0.04118186 0.06154787 0.075459 0.04677897 0.057837 0.07405102 0.07424795 8.36816e-4 0.05429196 0.07377678 0.008951961 0.057105 0.0742008 -0.01091998 -0.08880001 0.0732057 -0.01630729 -0.08880001 -0.04663687 0.05848085 -0.08899998 -0.07138127 0.02201819 -0.08569997 -0.06953626 0.02729094 -0.08569997 -0.004772365 -0.07360649 0.03127795 0.008202791 -0.07455009 -0.08744996 0.01362276 -0.0737524 -0.08744996 -0.06738376 -0.03247869 -0.08769726 -0.001315236 0.08108115 0.07399702 -0.001315236 0.08108115 0.07399702 -0.001315236 0.08108115 0.07399702 -0.001323044 0.08352053 0.07398599 -0.001323044 0.08352053 0.07398599 -0.001323044 0.08352053 0.07398599 0.005582332 -0.07449108 -0.07899999 0.05936217 0.04583787 -0.08879995 -0.07264655 -0.01868081 0.04733186 -0.07374697 -0.01370668 0.04905086 -0.06962937 0.02732735 -0.08899998 -0.07147675 0.02204757 -0.08899998 -0.03906553 -0.06402266 -0.08783632 0.005632281 0.07479315 -0.08776599 0.002735376 0.07495015 -0.08789247 0.0056324 -0.07479321 -0.08776599 0.008202791 -0.07455009 -0.08789998 -0.07005119 -0.02679216 -0.08880001 -0.008202791 0.07455009 -0.08879995 -0.002739131 0.07494997 -0.08879995 0.04104375 0.06278455 0.075495 0.04104375 0.06278455 0.075495 0.04104375 0.06278455 0.075495 0.06975281 0.08599996 0.02860397 0.07157236 0.08599996 0.02344685 -0.0645774 0.03572624 0.066859 -0.06699711 0.03116935 0.06504601 -0.07322859 0.08599996 -0.01522696 -0.02395462 0.08599996 0.07217603 -0.0192666 0.08599996 0.07356798 -0.02732735 -0.06962937 -0.08899998 -0.02204757 -0.07147681 -0.08899998 0.005589306 -0.07459157 -0.08769917 0.01114743 -0.07396537 -0.08769917 -0.0672937 -0.03313702 0.04233396 -0.05895996 0.04598021 0.06993997 -0.05583596 0.04972255 0.07123702 -0.07421529 0.08599996 0.01183485 0.008202791 0.07455009 -0.08744996 0.002739131 0.07494997 -0.08744996 0.01896995 -0.07256126 -0.08744996 -0.07421529 0.08599996 -0.009834945 -0.06878298 0.02674067 0.06385302 -0.04213625 0.06180268 -0.08899998 0.02995663 0.08213585 -0.06776165 0.02596884 0.08258605 -0.06937694 0.05936217 0.04583787 -0.08744996 0.05585777 0.05004876 -0.08744996 0.02729094 -0.06953626 -0.07899999 0.03241097 -0.0673024 -0.07899999 0.06469208 -0.03734987 -0.08569997 -0.01896995 0.07256126 -0.08744996 -0.0728271 -0.0166223 -0.07899999 -0.07138127 -0.02201819 -0.07899999 0.0654124 -0.03671103 -0.07899999 0.0679177 -0.03183835 -0.07899999 0.0732057 -0.01630729 -0.08744996 0.0742008 -0.01091998 -0.08744996 0.05227404 0.08599996 0.05488801 0.05227404 0.08599996 0.05488801 0.05227404 0.08599996 0.05488801 0.05143529 0.08588832 0.05568999 -0.06540369 -0.03670614 -0.08880001 -0.06790858 -0.0318343 -0.08880001 0.004769802 0.07364404 0.08021998 0.01664078 -0.07292789 -0.0876978 -0.05847847 -0.04664176 -0.08769881 0.06469208 0.03734982 -0.07899999 0.06171995 0.04207998 -0.07899999 -0.0749998 2.06904e-6 -0.08781319 -0.07479947 0.005474686 -0.08779495 -0.0742008 0.01091992 -0.08569997 -0.0732057 0.01630729 -0.08569997 -0.0673024 -0.03241097 -0.08569997 -0.04211235 0.06182157 -0.08769798 -0.03739768 0.06478106 -0.08769905 0.07396447 0.01114839 -0.08899998 -0.02364969 -0.07118421 0.02917993 0.07147449 0.02202272 0.06163489 0.07147449 0.02202272 0.06163489 0.07289546 0.01675027 0.05980789 0.07366031 -0.009243905 0.05079787 -0.005589783 -0.0745908 -0.08899998 -0.05248206 0.0532462 0.07245898 0.004867315 0.07390385 0.07975298 -0.001097798 0.07404822 0.07981801 0.07271957 -0.01261961 0.05010396 0.07271957 -0.01261961 0.05010396 0.07186627 -0.01697468 0.048388 0.07186627 -0.01697468 0.048388 0.07186627 -0.01697468 0.048388 -0.008202791 -0.07455009 -0.08880001 -0.002739131 -0.07494997 -0.08880001 -0.008202791 -0.07455009 -0.08569997 -0.002739131 -0.07494997 -0.08569997 0.006914675 -0.0746904 0.02796787 0.003458619 -0.07493013 0.02788496 0.05840265 0.04657465 -0.08569997 0.06171995 0.04207998 -0.08569997 -0.04361855 -0.06101155 -0.08776301 -0.02421599 -0.07098299 -0.08880001 -0.02933287 -0.06902599 -0.08880001 0 -0.07501 0.02785784 0.07499998 0 -0.08744996 0.07479989 0.005474627 -0.08744996 -0.0742008 0.01091992 -0.08880001 -0.02421069 -0.07098507 -0.0877906 0.01114839 -0.07396447 -0.08899998 -0.07182955 -0.02161026 -0.07899999 -0.07006049 -0.02679586 -0.07899999 -0.06073325 0.08599996 0.0450859 0.04797548 0.05764842 -0.08744996 0.04363936 0.06099665 -0.08744996 -0.01362276 0.07375234 -0.08744996 0.0654124 0.03671103 -0.07899999 0.0679177 0.03183835 -0.07899999 0.07006049 -0.02679586 -0.07899999 0.07181996 -0.02160727 -0.08744996 0.07005119 -0.02679216 -0.08744996 -0.0654124 -0.03671103 -0.07899999 -0.0679177 -0.03183835 -0.07899999 0.07147586 0.02205568 -0.08769828 -0.07500016 0.08599996 9.99987e-4 -0.0748021 0.08599996 -0.004450917 0.06540191 -0.03670936 -0.0877881 0.06254547 -0.04138928 -0.08778798 -0.06254976 0.04138255 -0.08744996 -0.07420051 0.01092118 -0.08779168 0.05845838 0.04667246 -0.08769589 0.05482953 0.05088102 -0.08769887 -0.06493437 -0.03755021 0.04080796 -0.03428387 0.06670588 -0.08785647 -0.06953626 -0.02729094 -0.08569997 -0.07138127 -0.02201819 -0.08569997 0.005110144 -0.07377809 0.02846497 0.01019942 -0.07319712 0.02869486 -0.02853149 0.08599996 0.07047897 0.0675624 0.08599996 0.03361588 0.06526666 0.08599996 0.03801298 0.04363936 0.06099665 -0.08569997 0.03907036 0.0640195 -0.08569997 -0.07420015 -0.01092261 -0.08778309 -0.07320469 -0.01631158 -0.08782488 0.02732735 0.06962937 -0.08899998 0.04774206 0.08166927 0.05883193 0.008203923 -0.07455998 -0.07899999 -0.04797548 0.05764842 -0.08569997 -0.05205547 0.05399262 -0.08569997 -0.06790858 0.03183424 -0.08879995 -0.07005119 0.02679216 -0.08879995 0.01362276 0.07375234 -0.08744996 0.02204757 0.07147675 -0.08769994 0.01664423 0.07292479 -0.08769977 0.008202791 0.07455009 -0.08879995 0.01362276 0.07375234 -0.08879995 0.01362442 -0.07376229 -0.07899999 -0.01896995 -0.07256126 -0.08880001 0.02204757 0.07147675 -0.08899998 -0.06540119 0.03671056 -0.0877881 -0.06790709 0.03183758 -0.08778697 -0.03205049 0.06774276 0.07739698 -0.02749228 0.06968647 0.07809102 -0.008202791 0.07455009 -0.08744996 -0.04797136 -0.05765193 -0.0877887 -0.05482953 0.05088102 -0.08769887 -0.05845487 0.04667717 -0.08769577 0.07430016 -0.008781671 0.05095696 0.07430016 -0.008781671 0.05095696 0.07430016 -0.008781671 0.05095696 0.07482826 -0.005218029 0.05198585 0.07482826 -0.005218029 0.05198585 0.07482826 -0.005218029 0.05198585 -0.002739131 0.07494997 -0.08744996 0.07138127 0.02201819 -0.07899999 0.002739131 -0.07494997 -0.08880001 -0.05205547 0.05399262 -0.08744996 -0.06255817 0.04138809 -0.07899999 -0.0654124 0.03671103 -0.07899999 0.07157236 0.08599996 -0.02144694 0.0486201 0.08259886 0.05812299 0.07051956 -0.0220341 0.04652684 0.07051956 -0.0220341 0.04652684 0.01903247 0.07255512 0.07887303 0.02365589 0.07118177 0.07854402 -0.03739988 0.06477868 -0.08899998 0.06477659 -0.03740596 -0.08769857 -0.0342977 0.06670957 -0.07899999 -0.02933675 0.06903517 -0.07899999 0.0672937 0.03313696 0.06524497 0.06935316 0.02857673 0.06366902 -0.02924305 0.06754964 0.08019399 -0.03369909 0.06543505 0.07947897 0.06255817 0.04138809 -0.07899999 0.05937016 0.04584395 -0.07899999 0.00927633 0.07369422 0.079557 0.07356607 -0.01365059 0.04926997 0.07356607 -0.01365059 0.04926997 0.01219063 -0.07401269 0.02820187 0.06469208 0.03734987 -0.08569997 0.03429317 -0.06670069 -0.08880001 0.03907036 -0.0640195 -0.08880001 0.05570769 0.04811847 0.07349902 0.05224853 0.05186015 0.07477498 -0.06540369 0.03670614 -0.08879995 0.07473856 -0.003330171 0.05284684 0.07473856 -0.003330171 0.05284684 0.07473856 -0.003330171 0.05284684 -0.03241097 -0.0673024 -0.07899999 -0.03734987 -0.06469208 -0.07899999 -0.03251469 0.06736916 -0.08769607 -0.07256245 0.01270115 0.06122297 -0.07330876 0.007125139 0.05932086 -0.05085766 0.0548532 -0.08769786 0.0728271 0.01662224 -0.07899999 0.07386571 0.01113349 -0.07899999 -0.04363936 0.06099665 -0.08569997 -0.0166223 -0.0728271 -0.07899999 0.07387059 0.003478944 0.05536186 0.01429235 0.07274156 0.07930803 0.04460644 0.05901533 0.07469099 0.06171995 -0.04207998 -0.07899999 0.05840265 -0.04657471 -0.07899999 0.01664447 0.07292455 -0.08899998 0.05087673 -0.05483222 -0.08899998 0.05483222 -0.05087673 -0.08899998 0.07486766 0.004618585 0.05538588 0.07486766 0.004618585 0.05538588 0.07486766 0.004618585 0.05538588 0.07157099 -0.01960891 0.04720383 0.07157099 -0.01960891 0.04720383 -0.07193249 0.01729446 0.06012886 -0.072959 0.01187419 0.05832386 -0.0714004 0.01820641 0.06310397 -0.0714004 0.01820641 0.06310397 -0.04213374 -0.06180536 -0.08769899 0.07005119 0.02679216 -0.08569997 0.07181996 0.02160722 -0.08569997 -0.04777789 -0.05640751 0.03453898 -0.04777789 -0.05640751 0.03453898 -0.04777789 -0.05640751 0.03453898 -0.03938609 -0.06253713 0.03242099 -0.03938609 -0.06253713 0.03242099 -0.005442798 0.07388114 0.079737 -0.01082116 0.07323575 0.07954996 -0.06739246 -0.03245449 -0.08899998 -0.06477868 -0.03739988 -0.08899998 0.0742467 -0.01067352 0.05009984 -0.03755486 -0.06390601 0.03184884 -0.03755486 -0.06390601 0.03184884 0.04433608 0.08599996 -0.05961453 0.03241097 0.06730234 -0.08569997 0.02729094 0.06953626 -0.08569997 -0.02201819 0.07138127 -0.07899999 -0.0166223 0.0728271 -0.07899999 0.06171995 -0.04207998 -0.08569997 0.05840265 -0.04657465 -0.08569997 0.07301986 -0.01208978 0.04989695 0.07301986 -0.01208978 0.04989695 0.005582332 0.07449108 -0.08569997 0.01113349 0.07386565 -0.08569997 -0.04392105 -0.05933398 0.03365999 -0.0324524 -0.06739449 -0.08769911 -0.03739768 -0.06478106 -0.08769911 -0.06953626 -0.02729094 -0.07899999 0.04796802 0.05765485 -0.08784806 0.05204868 0.0539999 -0.08778846 -0.0732057 -0.01630729 -0.08569997 -0.07499998 0 -0.08744996 -0.01448702 0.08599996 -0.07264649 0 0.07469999 -0.08569997 0.04671609 0.0571298 0.07445901 0.04249948 0.06033229 0.075576 0.02622306 0.08599996 0.071379 0.03075248 0.08599996 0.06952804 0.06540369 0.03670614 -0.08569997 0.06790858 0.03183424 -0.08569997 -0.05769985 0.08599996 -0.04700446 -0.05406165 0.08599996 -0.05108582 0.07147675 0.02204757 -0.08899998 0.06962937 0.02732735 -0.08899998 -0.03777176 0.08599996 -0.06392168 -0.03298693 0.08599996 -0.06648159 0.07321548 -0.01630938 -0.07899999 0.07421058 -0.01092135 -0.07899999 0.04657465 -0.05840265 -0.07899999 0.04207998 -0.06172001 -0.07899999 0.063735 0.03678447 0.06958699 0.06129896 0.04073387 0.07100898 0.06129896 0.04073387 0.07100898 -0.005582332 -0.07449108 -0.08569997 -0.01113349 -0.07386571 -0.08569997 -0.03241097 0.06730234 -0.08569997 -0.02729094 0.06953626 -0.08569997 -0.0166223 -0.0728271 -0.08569997 0.0742008 -0.01091998 -0.08569997 0.0732057 -0.01630729 -0.08569997 0.001989901 0.08599996 0.075975 -0.003625214 0.08599996 0.07591801 0.07005119 0.02679216 -0.08879995 0.06790858 0.03183424 -0.08879995 -0.02201819 0.07138127 -0.08569997 0.05205547 0.05399262 -0.08744996 -0.07147675 -0.02204757 -0.08899998 -0.07292455 -0.01664447 -0.08899998 0.03075248 0.08599996 -0.06752789 0.0748099 -0.005475342 -0.07899999 -0.04657465 -0.05840265 -0.07899999 -0.05080878 -0.05475884 -0.07899999 0.001315057 0.08371883 0.07399702 0.001315057 0.08371883 0.07399702 0.001315057 0.08371883 0.07399702 0.01897245 -0.07257097 -0.07899999 -0.06213635 0.04159504 0.06841999 -0.07012927 0.02586466 0.062967 -0.06704211 0.03312849 0.065485 -0.06704211 0.03312849 0.065485 -0.07004928 -0.02679747 -0.08782249 -0.06790715 -0.03183716 -0.08778846 -0.03429317 0.06670069 -0.08879995 -0.04207998 -0.06172001 -0.07899999 -0.05840265 -0.04657465 -0.08569997 0.03907567 -0.06402796 -0.07899999 -0.01896995 0.07256126 -0.08569997 -0.01362276 0.07375234 -0.08569997 0.0673024 0.03241097 -0.08569997 0.05080878 -0.05475884 -0.07899999 0.05475884 -0.05080878 -0.07899999 -0.06598776 0.08599996 -0.03470593 -0.0634976 0.08599996 -0.03898406 0.02421599 -0.07098299 -0.08880001 0.02933287 -0.06902599 -0.08880001 0.01361209 -0.0737549 -0.08779132 0.01896595 -0.07256257 -0.08779168 -0.0748021 0.08599996 0.006450951 -0.02729094 0.06953626 -0.07899999 -0.03241097 0.06730234 -0.07899999 -0.01981556 0.07083714 0.08142101 -0.02591145 0.06882184 0.08070302 -0.07095199 0.01811033 0.06327897 -0.07200527 0.01336991 0.06163597 0.07289218 0.08599996 0.01868093 0.03734987 -0.06469208 -0.07899999 -0.001008391 0.08384543 0.07464402 -0.01571989 0.07233041 0.07928502 -0.0673024 -0.03241097 -0.07899999 -0.01480847 0.07208436 0.08179199 0.02748376 -0.06979364 0.02966094 0.02246308 -0.07156753 0.02904796 -0.02421599 0.07098299 -0.08569997 -0.05475884 -0.05080878 -0.07899999 0.07355338 -0.007443368 0.05159199 0.07355338 -0.007443368 0.05159199 -0.05769985 0.08599996 0.04900497 -0.05483222 -0.05087673 -0.08899998 -0.05087673 -0.05483222 -0.08899998 0.02752023 -0.06957721 -0.0876888 0.03244757 -0.06739848 -0.08769768 0.03244757 -0.06739848 -0.08769768 -0.07480001 0 -0.08899998 -0.0745908 0.005589783 -0.08899998 0.04797548 0.05764842 -0.08569997 0.05205547 0.05399262 -0.08569997 -0.06073325 0.08599996 -0.04308617 0.05275428 0.05332434 0.07222497 0.0508517 0.05485916 -0.08769768 0.0166223 0.0728271 -0.08569997 -0.05585777 0.05004876 -0.08569997 0.07479989 -0.005474627 -0.08744996 0.04390019 0.06050914 0.07497602 0.04390019 0.06050914 0.07497602 0.04390019 0.06050914 0.07497602 0.008202791 0.07455009 -0.08569997 0.002739131 0.07494997 -0.08569997 0.02421927 -0.07099241 -0.07899999 -0.06171995 -0.04207998 -0.07899999 -0.06469208 -0.03734987 -0.07899999 -0.01118475 0.07267856 0.08204603 0.05480736 -0.05090993 -0.08769607 0.05848056 -0.04663789 -0.08769971 -0.05936217 0.04583787 -0.08569997 0.06254976 -0.04138255 -0.08880001 0.06540369 -0.03670614 -0.08880001 0.01733309 -0.07297992 0.02855998 -0.002739131 0.07494997 -0.08569997 -0.05205547 -0.05399268 -0.08880001 -0.04797548 -0.05764842 -0.08880001 -0.00481832 0.07364076 0.08021998 -0.0745908 -0.005589783 -0.08899998 -0.01433593 -0.07248491 0.02895897 -0.009696245 -0.07320868 0.02875298 0.06953626 0.02729094 -0.08569997 -0.01113349 0.07386565 -0.07899999 -0.005582332 0.07449108 -0.07899999 0.05585438 0.05005306 -0.08782798 0.05935728 0.04584485 -0.08784049 0.04657465 0.05840265 -0.07899999 -0.04793858 -0.05606532 0.03734683 -0.04431927 -0.05895853 0.0363729 0.002739131 -0.07494997 -0.08569997 -0.07046669 0.02225732 0.06194597 -0.07046669 0.02225732 0.06194597 -0.06180268 -0.04213625 -0.08899998 0.02420938 -0.07098549 -0.08778619 0.02201819 0.07138127 -0.08569997 0.03663837 -0.06443369 0.03166699 -0.06793218 -0.0287221 0.04677796 -0.0658766 -0.03317409 0.04522097 -0.05013793 0.08599996 -0.05488955 0.03689783 0.06530714 0.07636702 0.03689783 0.06530714 0.07636702 0.03739768 0.06478106 -0.08769905 0.05070734 0.05361837 0.073233 0.02159857 0.08599996 0.07291901 0.02729094 -0.06953626 -0.08569997 0.02201819 -0.07138127 -0.08569997 0.07120406 0.02358973 0.06194496 0.02732735 -0.06962937 -0.08899998 0.01204228 0.08599996 0.07507199 0.007066965 0.08599996 0.07568401 0.06396847 0.03699827 0.06706202 0.06631726 0.03262799 0.06552201 -0.008202791 0.07455009 -0.08569997 0.003073275 0.08274024 0.07593202 -0.05206245 -0.05399996 -0.07899999 -0.04207998 0.06171995 -0.07899999 -0.04657465 0.05840265 -0.07899999 0.02933287 0.06902599 -0.08744996 -0.01362276 -0.0737524 -0.08744996 0.06254976 0.04138255 -0.08744996 -0.06540369 0.03670614 -0.08569997 -0.06790858 0.03183424 -0.08569997 0.0324524 0.06739449 -0.08769905 0.07182955 -0.02161026 -0.07899999 0.07499939 -2.36794e-6 -0.08779156 0.07499939 -2.36794e-6 -0.08779156 0.07499939 -2.36794e-6 -0.08779156 0.07479977 0.005475342 -0.08784329 -0.07459175 -0.005599856 -0.0876978 -0.07480078 0 -0.08769905 -0.03734987 0.06469208 -0.07899999 -0.0408625 0.06285494 0.07569497 0.04798173 0.05765604 -0.07899999 0.02204757 -0.07147681 -0.0877 -0.04207998 0.06171995 -0.08569997 -0.04657465 0.05840265 -0.08569997 -0.07396447 -0.01114839 -0.08899998 0.03734987 0.06469208 -0.08569997 0.05893135 0.04413944 0.07209199 -0.07421529 0.01446652 0.02149289 -0.0741167 0.01507437 0.02204591 0.03482377 0.06640505 0.07693201 0.03482377 0.06640505 0.07693201 0.03482377 0.06640505 0.07693201 0.05936217 -0.04583787 -0.08880001 -0.01113349 0.07386565 -0.08569997 -0.005582332 0.07449108 -0.08569997 -6.42344e-4 0.08077502 0.07463103 -0.001742005 0.08219593 0.07461303 -0.001742005 0.08219593 0.07461303 -0.001742005 0.08219593 0.07461303 -0.001742005 0.08219593 0.07461303 -0.06180268 0.04213625 -0.08899998 -0.05848085 0.04663687 -0.08899998 0.05206245 0.05399996 -0.07899999 0 0.07500994 0.07972198 0.02421599 -0.07098299 -0.08569997 0.01896995 -0.07256126 -0.08569997 -0.06701999 -0.03139293 0.04318284 -0.07025825 -0.02400928 0.04567897 0.07005119 0.02679216 -0.08744996 -0.0520516 -0.05399686 -0.08783072 0.05952787 0.08599996 0.04670786 0.06269538 0.08599996 0.04223603 0.07501 0 0.05378997 0.07501 0 0.05378997 0.07501 0 0.05378997 0.07459628 0.0055902 -0.08769476 0.02371299 -0.06984013 0.032588 0.05205547 -0.05399268 -0.08880001 0.05585777 -0.05004876 -0.08880001 0.07277637 0.01203835 0.06063199 0.07261365 0.01220405 0.06107884 0.07261365 0.01220405 0.06107884 0.04213625 -0.06180268 -0.08899998 0.0166223 -0.0728271 -0.08569997 0.06526666 0.08599996 -0.03601282 0.0675624 0.08599996 -0.03161615 0.05870199 -0.04494458 0.03853297 0.06180953 -0.0405876 0.04003185 0.07138127 0.02201819 -0.08569997 0.07479989 0.005474627 -0.08880001 0.0742008 0.01091992 -0.08880001 -0.06889545 -0.02646082 0.04528784 -0.06994366 -0.02395367 0.04582995 0.008202791 -0.07455009 -0.08569997 -0.05585777 -0.05004876 -0.08880001 -0.03429317 0.06670069 -0.08744996 0.0748099 0.005475342 -0.07899999 0.07501 0 -0.07899999 0.04796802 -0.05765485 -0.08784806 0.04363137 -0.06100291 -0.08785182 0.001322925 0.08127915 0.07398599 0.001322925 0.08127915 0.07398599 0.001322925 0.08127915 0.07398599 5.05777e-4 0.08070462 0.074705 -0.05627536 -0.04959398 0.03664499 -0.05941665 -0.04578369 0.0379619 -0.04797548 0.05764842 -0.08744996 -0.05205547 -0.05399268 -0.08569997 -0.04797548 -0.05764842 -0.08569997 -0.02204757 0.07147675 -0.08769994 -0.01664423 0.07292479 -0.08769977 -0.04966676 -0.05504649 0.03491985 -0.04966676 -0.05504649 0.03491985 0.001620352 0.08176082 0.074602 0.001620352 0.08176082 0.074602 -0.04798173 -0.05765604 -0.07899999 -0.07237935 0.01884806 0.06053501 -0.01896995 0.07256126 -0.08879995 -0.02421599 0.07098299 -0.08879995 -0.03907036 0.0640195 -0.08744996 -0.03429317 -0.06670069 -0.08569997 -0.02933287 -0.06902599 -0.08569997 -0.01362276 -0.0737524 -0.08569997 -0.01896995 -0.07256126 -0.08569997 -0.07062506 -0.02141755 0.04704588 -0.07259386 -0.01403999 0.04927498 -0.07259386 -0.01403999 0.04927498 -0.004791796 -0.07485681 0.02791082 0.01362276 -0.0737524 -0.08569997 0.001762032 0.08492827 0.07597303 0.07469999 0 -0.08569997 0.07449108 0.005582332 -0.08569997 0.03361874 0.06543236 0.079472 0.02855545 0.06779223 0.08032602 -0.01114839 0.07396447 -0.08769994 0.07292419 -0.01665318 -0.0876981 -0.05585116 0.05005687 -0.08782809 -0.0593574 0.04584467 -0.08784049 0.05585277 -0.05005484 -0.08778899 0.05585277 -0.05005484 -0.08778899 0.05585277 -0.05005484 -0.08778899 0.05204772 -0.05400079 -0.08783078 0.07181996 0.02160727 -0.08880001 0.0732057 0.01630729 -0.08880001 -0.01361626 0.07375389 -0.08778715 -0.008190035 0.07455199 -0.08778768 -0.07181996 -0.02160727 -0.08744996 -0.07005119 -0.02679216 -0.08744996 0.07388788 -0.001917362 0.05352801 0.05605298 0.08599996 0.05092597 0.03429317 -0.06670069 -0.08744996 0.03907036 -0.0640195 -0.08744996 -0.04363936 0.06099665 -0.08744996 0.07419997 0.01092356 -0.08779299 -0.07126659 -0.01865613 0.05041188 -0.06790858 -0.0318343 -0.08744996 -0.07005119 -0.02679216 -0.08569997 -0.06790858 -0.0318343 -0.08569997 -0.04271256 0.05996513 0.07758301 -0.0473544 0.05640316 0.07630401 -0.0436452 0.06100475 -0.07899999 -0.03907567 0.06402796 -0.07899999 -0.02421599 -0.07098299 -0.08569997 -0.04207998 -0.06171995 -0.08569997 -0.04657465 -0.05840265 -0.08569997 -0.063385 0.04011029 0.06765699 -0.063385 0.04011029 0.06765699 -0.063385 0.04011029 0.06765699 -0.06038558 0.04449784 0.06917297 -0.01362442 0.07376229 -0.07899999 -0.01897245 0.07257097 -0.07899999 -0.03734987 0.06469208 -0.08569997 0.02999317 0.06846863 0.077735 0.06269538 0.08599996 -0.04023563 -0.07181996 -0.02160727 -0.08569997 0.0732057 0.01630729 -0.08569997 -0.07045978 0.08599996 0.02673786 0.0728271 0.01662224 -0.08569997 0.05599135 -0.04808533 0.03769797 0.04433608 0.08599996 0.06161499 0.039837 0.08599996 0.06467199 0.008202791 -0.07455009 -0.08880001 -0.05475884 -0.05080878 -0.08569997 -0.05080878 -0.05475884 -0.08569997 0.01113349 -0.07386571 -0.08569997 -0.07321548 0.01630938 -0.07899999 0.02732145 0.06963425 -0.08769768 0.02732145 0.06963425 -0.08769768 0.02732145 0.06963425 -0.08769768 0.06062614 0.04169982 0.07126998 -0.07336419 0.01562684 0.059192 -0.0742467 0.01067346 0.05747985 -0.0521481 0.0530864 0.07240301 -0.0521481 0.0530864 0.07240301 -0.04798173 0.05765604 -0.07899999 -0.05206245 0.05399996 -0.07899999 -0.02933287 0.06902599 -0.08879995 -0.03734987 -0.06469208 -0.08569997 -0.008203923 0.07455998 -0.07899999 0.02421599 0.07098346 -0.08778929 0.02421599 0.07098346 -0.08778929 0.01896446 0.07256299 -0.08778947 0.01362442 0.07376229 -0.07899999 0.06254976 0.04138255 -0.08569997 0.07320517 -0.01630944 -0.08782595 -0.07396459 -0.01115018 -0.08769959 -0.0436452 -0.06100475 -0.07899999 -0.07320445 0.01631164 -0.08778989 -0.06986159 -0.02364611 0.04852396 -0.03907036 -0.0640195 -0.08569997 -0.05840265 -0.04657471 -0.07899999 -0.02421927 0.07099235 -0.07899999 -0.05848085 -0.04663687 -0.08899998 0.03241097 0.06730234 -0.07899999 0.03734987 0.06469208 -0.07899999 -0.04916435 -0.05665117 0.0342049 -0.05275428 -0.0533244 0.03535497 0.06493437 0.03755015 0.06677103 0.07497799 0.01331835 0.001924932 0.07499438 0.02252316 2.45006e-4 0.07500016 0.08599996 9.99987e-4 0.05936217 0.04583787 -0.08569997 0.001989901 0.08599996 -0.07397514 0.07479989 -0.005474627 -0.08569997 0.07499998 0 -0.08569997 -0.004717469 -0.07367169 0.028584 0.0593574 -0.04584467 -0.0878399 -0.03241097 -0.0673024 -0.08569997 0.03359144 0.08155244 -0.06604045 0.06469208 -0.03734987 -0.07899999 0.0673024 -0.03241097 -0.07899999 0.02201819 0.07138127 -0.07899999 0.02729094 0.06953626 -0.07899999 -0.06230336 -0.04177057 0.0393489 0.04663687 -0.05848085 -0.08899998 0.005589783 0.0745908 -0.08899998 0 0.07479995 -0.08899998 0.01114839 0.07396447 -0.08899998 -0.03429317 0.06670069 -0.08569997 -0.03907036 0.0640195 -0.08569997 0.05706185 -0.04868716 0.03695785 0.05345839 -0.05261838 0.03559899 -0.04274708 0.06074285 0.07510197 -0.0192666 0.08599996 -0.07156747 0.02400869 0.06951975 0.08092904 0.06178724 -0.04216337 -0.0876975 -0.0660662 0.03552114 0.06606996 -0.04537075 -0.05973249 0.03313899 -0.05114275 0.07758325 -0.05373746 -0.05013787 0.07214558 -0.05450254 0.04974263 -0.05614387 0.03437983 0.04562038 -0.05954205 0.03320497 0.07386571 0.01113349 -0.08569997 -0.07181876 -0.02161169 -0.08782356 0.02933984 -0.06902331 -0.08787059 0.02933984 -0.06902331 -0.08787059 0.02933984 -0.06902331 -0.08787059 -0.06962937 -0.02732735 -0.08899998 -0.002737164 0.07495015 -0.08778709 0.06967848 0.02716362 0.06341701 0.06967848 0.02716362 0.06341701 0.07264655 0.01868075 0.06024789 0.07264655 0.01868075 0.06024789 0.07264655 0.01868075 0.06024789 -0.06842297 0.03073734 0.06441599 -0.06540125 -0.03671067 -0.08782279 -0.05797153 0.04539173 0.07251995 -0.06092166 0.04107815 0.07113802 -0.05840265 0.04657465 -0.07899999 -0.06171995 0.04207998 -0.07899999 0.06189227 0.04013717 0.07030099 0.05205547 -0.05399268 -0.08744996 0.04797548 -0.05764842 -0.08744996 -0.07181841 0.02161192 -0.08778846 -0.07479989 0.005474627 -0.08880001 0.04657465 -0.05840265 -0.08569997 0.04207998 -0.06171995 -0.08569997 -0.05825787 -0.0450986 0.04125601 0.07499998 0 -0.08880001 0 0.07479995 -0.08769994 0.005589306 0.07459157 -0.08769917 0.01686877 0.08599996 0.07415103 -0.07479989 -0.005474627 -0.08880001 -0.0459395 0.08599996 0.060404 -0.0121904 0.07401204 0.079521 -0.00687021 0.07469433 0.07975202 0.05605298 0.08599996 -0.04892647 0.05952787 0.08599996 -0.04470777 -0.02729094 -0.06953626 -0.08569997 -0.07004916 0.02679729 -0.08778899 0.07389569 0.08599996 0.01384097 -0.009568274 -0.07314032 0.03129982 0.02879369 -0.06791192 0.03309983 0.04363936 -0.06099671 -0.08880001 0.04797548 -0.05764842 -0.08880001 0.02159857 0.08599996 -0.07091856 -0.06840687 0.08599996 0.03179889 -0.03090202 0.06722027 0.07753998 -0.03528678 0.06506025 0.07674401 -0.03528678 0.06506025 0.07674401 -0.03528678 0.06506025 0.07674401 0.05205547 -0.05399268 -0.08569997 0.04797548 -0.05764842 -0.08569997 -0.07396447 0.01114839 -0.08899998 0.03429388 -0.06670039 -0.08788561 0.03906595 -0.06402242 -0.0878542 0.03241097 -0.0673024 -0.08569997 0.03734987 -0.06469208 -0.08569997 -0.04663687 -0.05848085 -0.08899998 0.06790858 -0.0318343 -0.08569997 0.07005119 -0.02679216 -0.08569997 -0.05585777 -0.05004876 -0.08744996 -0.05936217 -0.04583787 -0.08744996 -0.07343417 -0.007322311 0.05196595 0.05585777 -0.05004876 -0.08569997 0.05936217 -0.04583787 -0.08569997 -0.05080878 0.05475884 -0.07899999 -0.05475884 0.05080872 -0.07899999 0.001560032 0.08310246 0.07464897 0.001560032 0.08310246 0.07464897 6.43154e-4 0.08402442 0.07463103 0.05426996 -0.04994899 0.03943789 -0.05110085 0.05297887 0.07515698 0.05080878 -0.05475884 -0.08569997 -0.04851675 0.05588763 0.07355999 -0.04851675 0.05588763 0.07355999 -0.05188375 0.05275577 0.07248401 -0.06254547 -0.04138928 -0.08778798 -0.02510964 -0.06940299 0.030312 -0.0203101 -0.07110422 0.02944386 -0.0203101 -0.07110422 0.02944386 0.03429317 -0.06670069 -0.08569997 0.02933287 -0.06902599 -0.08569997 0.03906577 0.0640226 -0.08778935 0.03267705 0.06750619 -0.08776545 0.03267705 0.06750619 -0.08776545 0.03267705 0.06750619 -0.08776545 -0.07499998 0 -0.08880001 0.01361817 0.07375347 -0.08779126 0.06477868 -0.03739988 -0.08899998 0.06180268 -0.04213625 -0.08899998 -0.05935758 -0.04584425 -0.08778798 -0.005589783 0.0745908 -0.08769994 -0.02933287 0.06902599 -0.08569997 0.02933287 0.06902599 -0.08789998 0.06739246 -0.03245449 -0.08899998 0.008202791 0.07455009 -0.08789998 -0.0720852 0.02074182 0.06095999 -0.05345839 0.05261832 0.07198101 0.07181996 -0.02160727 -0.08880001 -0.02996206 -0.06759911 0.03065598 -0.04213625 -0.06180268 -0.08899998 0.01114743 0.07396537 -0.08769917 -0.06469208 0.03734982 -0.07899999 0.0486201 0.08599996 -0.05622071 0.05227404 0.08599996 -0.05288773 0.002292454 0.07345062 0.08237498 -0.006040692 0.07330012 0.08223402 0.0612396 0.04152035 0.06855797 0.07419908 -0.01092815 -0.08778446 0.07419908 -0.01092815 -0.08778446 0.07419908 -0.01092815 -0.08778446 0.01435273 0.07362341 0.07938098 0.06030786 0.04144263 0.07136702 0.01936525 0.07098186 0.08141601 -0.07499998 0 -0.08569997 -0.06540369 -0.03670614 -0.08569997 -0.07469999 0 -0.08569997 -0.07449108 0.005582332 -0.08569997 0.0747931 0.005699992 9.99987e-4 0.06953626 -0.02729094 -0.07899999 -0.0166223 0.0728271 -0.08569997 0.009665548 0.07294213 0.08209502 -0.05013793 0.08599996 0.05689001 -0.06599026 0.03264242 0.06814098 -0.06785976 0.02798599 0.06664901 -0.06785976 0.02798599 0.06664901 -0.06785976 0.02798599 0.06664901 -0.03546679 0.06545025 0.076689 -0.03546679 0.06545025 0.076689 -0.01448702 0.08599996 0.07464599 -0.0635671 0.03715163 0.06965601 -0.05937016 0.04584395 -0.07899999 -3.61762e-4 0.08547061 0.075984 -0.002296805 0.08443844 0.07596302 -0.05460757 0.04945302 0.073879 -0.02853149 0.08599996 -0.06847935 -0.07349169 -0.008201062 0.05129688 0.07138127 -0.02201819 -0.07899999 -0.064763 0.03743427 -0.08769649 -0.06178009 0.04217797 -0.08769547 -0.07469999 0 -0.07899999 -0.02201819 -0.07138127 -0.08569997 -0.05936217 -0.04583787 -0.08569997 -0.05585777 -0.05004876 -0.08569997 -0.06941801 0.02327054 0.065068 -0.06941801 0.02327054 0.065068 0.05741268 -0.04630291 0.04067283 0.06790858 -0.0318343 -0.08744996 0.06540369 -0.03670614 -0.08744996 -0.05586522 0.05005556 -0.07899999 0.03779447 -0.06333982 0.03481996 0.04227727 -0.06044089 0.03577697 -0.05023467 0.07398104 -0.05446726 0.07005119 -0.02679216 -0.08880001 0.05865776 0.04543316 0.06980097 -0.07421058 -0.01092135 -0.07899999 -0.07321548 -0.01630938 -0.07899999 -0.04104375 -0.06278461 0.03208386 -0.06254976 -0.04138255 -0.08569997 0.06254696 0.04138684 -0.08778786 -0.05706185 0.04868716 0.07062101 0.07181996 -0.02160727 -0.08569997 0.03767877 0.06322222 0.078709 -0.06992775 0.02301442 0.064785 0.06825071 0.02749913 0.06639301 0.06649267 0.03175336 0.06777 -0.05406165 0.08599996 0.05308598 -0.0204612 -0.07202273 0.02903598 0.03515416 0.08599996 0.06737798 -0.06254565 0.04138916 -0.08783769 0.07458001 0.08599996 0.008934974 -0.02732563 0.06963098 -0.08769905 -0.07366716 0.006502807 0.05641198 0.06790858 -0.0318343 -0.08880001 -0.07046067 0.02572512 0.06268298 -0.05491375 -0.04918068 0.03978788 -0.05165046 -0.05262732 0.03857797 0.06254976 -0.04138255 -0.08569997 0.03907036 -0.0640195 -0.08569997 0.03429317 0.06670069 -0.08569997 0.02933287 0.06902599 -0.08569997 -0.07449108 -0.005582332 -0.08569997 -0.07386571 -0.01113349 -0.08569997 0.009166002 -0.07310092 0.03161984 -0.06953626 0.02729094 -0.07899999 -0.0673024 0.03241091 -0.07899999 0.06254976 -0.04138255 -0.08744996 -0.0081923 -0.07455176 -0.08779162 -0.05480736 -0.05090993 -0.08769607 0.03361165 -0.06565493 0.03403598 -0.07479989 0.005474627 -0.08569997 0.05444109 0.04982393 0.07190901 0.07479166 0.001588523 0.05455183 0.07479166 0.001588523 0.05455183 -0.01921439 0.07125282 0.07938802 0.04207998 0.06171995 -0.07899999 -0.07138127 0.02201819 -0.07899999 0.06540369 -0.03670614 -0.08569997 -0.06540369 -0.03670614 -0.08744996 -0.06254976 -0.04138255 -0.08744996 -0.06739217 0.0324558 -0.08769965 0.03236037 -0.06767064 0.03039497 -0.01895827 0.07256489 -0.08778947 0.02873796 0.06797313 0.078224 -0.03689783 -0.06530719 0.03121197 0.07028388 0.02189731 0.06448996 0.07144141 0.01768904 0.06298202 0.01432657 0.07210773 0.08183199 -0.05585259 -0.05005508 -0.0877887 0.04660046 -0.05851554 -0.08769619 0.0508517 -0.05485916 -0.08769768 0.07182955 0.02161026 -0.07899999 0.07454597 0.008330047 0.05666995 0.07480078 0 -0.08769905 -0.05475884 0.05080872 -0.08569997 -0.05840265 0.04657465 -0.08569997 -0.07468557 0.01079684 0.01815599 0.07479989 -0.005474627 -0.08880001 -0.07292455 0.01664447 -0.08899998 -0.009573459 -0.07439661 0.02806997 -0.01362276 -0.0737524 -0.08880001 0.05848085 -0.04663687 -0.08899998 0.05475884 -0.05080878 -0.08569997 -0.0748099 -0.005475342 -0.07899999 0.07006049 0.02679586 -0.07899999 -0.04363936 -0.06099671 -0.08744996 -0.02246308 0.07156741 0.07853204 0.06061244 -0.04202508 0.04216986 0 0.07469999 -0.07899999 0.005582332 0.07449108 -0.07899999 -0.0732057 -0.01630729 -0.08744996 -0.07501 0 0.05378997 -0.07430016 0.0086506 0.05699998 0.06572449 0.03221625 0.06816899 -0.02381199 0.06985121 0.07887703 0.04362976 0.06100404 -0.08783614 -0.02094423 0.07095706 0.07884603 0.06509637 -0.0350821 0.04195994 -0.01733475 0.07297897 0.07915997 -0.07501 0 -0.07899999 -0.06171995 0.04207998 -0.08569997 -0.06469208 0.03734987 -0.08569997 -0.03739988 -0.06477868 -0.08899998 -0.05080878 0.05475884 -0.08569997 0.06739246 0.03245449 -0.08899998 0.06477868 0.03739982 -0.08899998 0.0394513 0.06349635 0.07601201 -0.005589306 -0.07459157 -0.08769917 -0.04194319 0.08599996 -0.06130069 0.03735637 -0.06480866 -0.08769637 0.03735637 -0.06480866 -0.08769637 0.0686447 -0.0273503 0.04468697 -0.01664447 -0.07292461 -0.08899998 0.02421599 0.07098299 -0.08569997 0.01896995 0.07256126 -0.08569997 0.008203923 0.07455998 -0.07899999 -0.03429317 -0.06670069 -0.08880001 -0.06472575 0.03744524 0.06698203 -0.06472575 0.03744524 0.06698203 0.05936217 -0.04583787 -0.08744996 0.05483222 0.05087673 -0.08899998 0.05848085 0.04663687 -0.08899998 0.07452046 0.006506264 0.05625689 0.07384216 0.01192826 0.05813586 -0.0728271 -0.0166223 -0.08569997 0.04643976 0.05703336 0.07662898 0.05941665 0.04578363 0.06961798 0.04209613 -0.06183511 -0.08769631 -0.01114839 -0.07396447 -0.08899998 0.07181996 0.02160727 -0.08744996 -0.03245449 -0.06739246 -0.08899998 0.06180268 0.04213625 -0.08899998 -0.002739489 0.07495999 -0.07899999 0.05585777 -0.05004876 -0.08744996 -0.04663449 0.05848413 -0.08769899 0.03804975 0.06323367 0.07657396 -0.0673024 0.03241097 -0.08569997 -0.07479977 -0.005471885 -0.08779507 -0.03907567 -0.06402796 -0.07899999 -0.0748099 0.005475342 -0.07899999 0.05211722 0.05246573 0.07243996 0.04813218 0.05720371 0.07383102 0.04813218 0.05720371 0.07383102 0.05212748 0.05359303 0.07257902 0.06960016 0.02272301 0.06487798 -0.07491898 0.001590788 0.05448395 0.05087673 0.05483222 -0.08899998 -0.06254976 0.04138255 -0.08569997 -0.07449108 0.005582332 -0.07899999 0.0732057 0.01630729 -0.08744996 0.0716198 -0.01770359 0.05056285 -0.04363936 -0.06099671 -0.08569997 0.03907036 0.0640195 -0.08744996 0.04116588 0.06100857 0.07795602 0.07374697 0.01370662 0.05852788 0.02385938 0.06983506 0.078875 0.07479989 0.005474627 -0.08569997 0.07396447 -0.01114839 -0.08899998 0.0745908 -0.005589783 -0.08899998 0.07489496 0.08599996 0.004972994 -0.009125649 0.08599996 0.07547098 0.05586522 -0.05005556 -0.07899999 0.03708475 -0.06520134 0.03124886 -0.03905987 0.06402635 -0.08783859 -0.03268265 0.06616652 0.07761299 -0.03690958 0.06390506 0.07682299 0.05406045 0.05106115 0.071702 0.07292455 0.01664447 -0.08899998 -0.0459395 0.08599996 -0.05840367 0.06477659 0.03740596 -0.08769857 0.06178385 0.04216992 -0.08769667 -0.0728271 0.01662224 -0.07899999 -0.03936946 0.06214565 0.078336 -0.07292169 -0.01667118 -0.08769667 -0.07482826 0.005217969 0.05559384 0.03429317 0.06670069 -0.08744996 0.05840265 0.04657465 -0.07899999 -0.02395462 0.08599996 -0.07017618 -0.02647405 0.06908875 0.07818597 -0.07396125 -0.0023579 0.05328989 0 -0.07480001 -0.0877 -0.05205547 -0.05399268 -0.08744996 0.002739489 0.07495999 -0.07899999 0.05080878 0.05475884 -0.08569997 0.05475884 0.05080872 -0.08569997 -0.002727866 -0.07495087 -0.0877918 0.002739131 -0.07494997 -0.08789998 -0.02830845 0.06815284 0.07830601 0 -0.07469999 -0.07899999 0.04127722 -0.06263118 0.03213685 0.07292455 -0.01664447 -0.08899998 0.04838389 0.05599814 0.073596 0.05858397 -0.04553872 0.03821599 0.04938286 -0.05528938 0.03483599 0.01896995 0.07256126 -0.08879995 -0.04797548 -0.05764842 -0.08744996 0.05586522 0.05005556 -0.07899999 0.0436452 -0.06100475 -0.07899999 0.04798173 -0.05765604 -0.07899999 -0.07386571 0.01113349 -0.07899999 0.07469999 0 -0.07899999 0.07479977 -0.005475163 -0.0878458 0.00479418 0.07485586 0.07981097 0.00479418 0.07485586 0.07981097 -0.02729094 -0.06953626 -0.07899999 0.04363936 -0.06099671 -0.08744996 0.03907036 0.0640195 -0.08879995 0.03429317 0.06670069 -0.08879995 0.009579598 0.07439494 0.07965302 0.04363936 -0.06099671 -0.08569997 0.07020699 0.02315264 0.06224596 0.06827187 0.02833867 0.06404799 -0.07356607 0.01351964 0.05868798 0.06975281 0.08599996 -0.02660381 0.0742008 0.01091992 -0.08744996 -0.01664447 0.07292455 -0.08899998 0.04797548 0.05764842 -0.08879995 0.04363936 0.06099665 -0.08879995 0.05585777 0.05004876 -0.08569997 -0.06201624 0.04041051 0.06814801 -0.06201624 0.04041051 0.06814801 -0.04899907 0.05518382 0.07378 0.07449108 0.005582332 -0.07899999 0.07344669 0.002061188 0.05767798 0.07329946 0.007447719 0.05937701 0.07147675 -0.02204757 -0.08899998 0.01113349 0.07386565 -0.07899999 0.0166223 0.0728271 -0.07899999 -0.02201819 -0.07138127 -0.07899999 -0.07421058 0.01092135 -0.07899999 -0.05205208 0.05399638 -0.08778846 -0.01434254 -0.07362604 0.02833586 -0.03615659 0.06542634 0.07668101 -0.03615659 0.06542634 0.07668101 0.06605195 0.03239047 0.06811296 0.0642094 0.03593194 0.06935399 0.05206245 -0.05399996 -0.07899999 0.02853858 -0.06807219 0.03075093 0.01362276 0.07375234 -0.08569997 0.002739131 0.07494997 -0.08879995 -0.01362276 0.07375234 -0.08879995 0.0742008 0.01091992 -0.08569997 0.07178497 0.01699346 0.06244885 -0.01903247 -0.07255518 0.02870595 0.07219767 0.01230067 0.06126499 0.0660662 -0.0355212 0.04150986 -0.01897245 -0.07257097 -0.07899999 0.0644074 0.03596574 0.068861 0.06962937 -0.02732735 -0.08899998 -0.01362442 -0.07376229 -0.07899999 0.0699926 -0.02326363 0.04864984 0.5768777 -0.7277352 0.3709633 0.5344244 -0.7703695 0.3477375 0.5774236 -0.7581278 0.3030253 0.7168835 -0.6965276 0.03045761 0.7335307 -0.6793895 0.01904398 0.7013906 -0.6909226 0.1751493 0.7776392 -0.6284289 0.01883053 0.7653334 -0.6216483 0.1667882 0.8115525 -0.5798155 0.07208776 0.8042938 -0.5672844 0.1769177 0.7268558 -0.5540231 0.4058807 0.8114734 -0.5470251 0.2056074 0.8877487 -0.4589815 0.03518885 0.6209141 -0.04381024 -0.7826535 0.8874456 -0.4605391 0.01855581 0.905937 -0.4231816 0.01397782 0.9067824 -0.4210715 0.02108865 0.7848401 -0.4624319 0.4125324 0.9099639 -0.3930887 0.1320875 0.9293991 -0.3684574 0.02136337 0.9470173 -0.3199045 0.02862715 0.9426915 -0.3331114 0.01922672 -0.7955796 0.4354951 -0.4211857 0.9736754 -0.2244055 -0.03997975 0.9560761 -0.2925574 0.01812839 0.9824675 -0.1859522 0.01339781 0.985218 -0.1703882 0.01770108 0.9935014 -0.1115453 0.02264475 0.9663791 -0.1760632 0.1873856 0.9983213 -0.04623693 0.03488367 0.9849249 -0.09912729 0.141763 0.06970483 0.964179 0.2559303 0.009247243 0.9700759 0.2426258 -0.01065123 0.9360275 0.3517657 0.009949266 -0.9504606 0.3106858 0.05145508 -0.9784706 0.1998691 0.06628715 -0.9830763 0.170784 -0.003936886 -0.9471452 0.3207811 0.9630632 -0.06354093 0.2616714 0.9874135 -0.03744685 0.1536634 0.1249743 -0.9819717 0.1418206 0.1141109 -0.9825626 0.1467969 0.9883793 0.03958272 0.1467645 0.9946208 0.07114022 0.07529085 0.9421277 -7.6298e-4 0.3352534 0.9679637 0.07116973 0.240793 0.1889438 -0.9686154 0.1615071 0.8666512 0.1921178 0.4604418 0.9714401 0.1602535 0.1749941 0.1668475 -0.9646762 0.2038671 0.9437747 0.26277 0.2006024 0.2143985 -0.9186939 0.331715 0.9564192 0.2381739 0.1689252 0.9414829 0.3153231 0.1190854 0.2323737 -0.9592209 0.1609281 0.9025806 0.2275525 0.3654697 0.9270107 0.3574675 0.1134386 0.8791885 0.4456065 0.1687085 0.9108498 0.3853067 0.1479578 0.321672 -0.946613 0.0212413 0.8583897 0.4755539 0.1923945 0.8885025 0.4272369 0.1674277 0.877543 0.4575407 0.1434392 0.8199011 0.5447696 0.1760353 0.8392975 0.4969376 0.2205288 0.2595322 -0.9093089 0.3252698 0.7867253 0.521574 0.3301877 0.7814254 0.5955606 0.1862309 0.742294 0.6695966 0.02530062 0.6918681 0.7217159 0.02108865 0.733556 0.6545726 0.1828396 0.682739 0.6571336 0.3194416 0.6392022 0.7687579 0.02078384 0.6264688 0.7244051 0.2877051 0.5831811 0.8121016 0.01977604 0.5793705 0.7912021 0.1957786 0.5232558 0.8518891 0.02209603 0.5136144 0.8297364 0.2184898 0.4859253 0.863172 0.1371529 0.452783 0.8439171 0.2877351 0.398279 0.9170184 0.02124154 0.3841729 0.8880411 0.252575 0.3776113 -0.9254879 0.02969491 0.3297624 0.9439048 0.01733499 0.3229815 0.9039392 0.2803161 0.2695768 0.9296782 0.2510516 0.3892127 -0.9208589 0.02307254 0.2628899 0.9359846 0.2341411 0.2185778 0.906997 0.3599727 0.1909595 0.9375848 0.2906357 0.4553525 -0.8899517 0.02530074 0.4115459 -0.8874256 0.2076193 0.1439566 0.9806264 0.1328478 0.07385551 0.9407121 0.3310682 0.5012388 -0.8652656 -0.00866729 0.5132706 -0.857984 0.02041727 0.5002344 -0.8401533 0.2095424 0.5723575 -0.8195327 0.027803 0.5473011 -0.7927672 0.2682944 0.3525531 0.4118816 0.8402738 0.4173787 0.5361589 0.7337089 0.3596666 0.4128003 0.8368009 -0.4531477 0.2877952 0.8437008 -0.5263962 0.3743187 0.7634087 -0.4337987 0.3230448 0.8411069 -0.4261767 0.2662842 0.8645613 0.2978958 0.406055 0.8639314 0.339887 0.5005375 0.7962029 0.1889464 0.3017466 0.9344775 0.2547165 0.4598082 0.8507032 -0.3383655 0.1809784 0.9234477 -0.390184 0.1984948 0.8990864 -0.3390648 0.1506107 0.9286288 0.1419139 0.2830954 0.9485344 0.1349877 0.2914306 0.9470198 -0.4101741 0.1710585 0.8958216 0.426599 0.7792192 0.4591631 0.5206279 0.853776 0.003601253 0.4554057 0.8613705 0.2250477 -0.3653466 0.13047 0.921683 0.4039545 0.8618367 -0.3066892 0.3912556 0.9202747 -0.003692805 0.1307446 0.2989364 0.945274 -0.4075278 0.1355679 0.9030739 -0.3727249 0.1034889 0.9221531 0.2751343 0.6742393 0.6853485 0.2931095 0.7176972 0.6316627 0.2672283 0.6551303 0.7066777 0.2569456 0.6302326 0.7326568 0.1127667 0.3256959 0.9387257 0.143257 0.4754583 0.8679959 0.1118237 0.3212491 0.9403694 -0.4147225 0.1035814 0.9040333 -0.3904382 0.07858812 0.9172688 0.08868777 0.3320146 0.9390957 0.1093186 0.4912317 0.8641418 -0.4213495 0.07114046 0.9041038 -0.4077945 0.05288934 0.9115407 0.06564706 0.3501788 0.9343797 0.08050996 0.5955541 0.7992706 -0.4414868 0.03799605 0.896463 -0.4373654 0.02621567 0.8989017 0.02597177 0.3339096 0.9422472 0.02215707 0.3103211 0.9503736 -0.4958398 -6.10377e-5 0.8684141 -0.4975023 6.10395e-5 0.8674626 0.1065115 0.8039944 0.5850201 0.06674581 0.9194871 -0.3874127 0.1089518 0.9940252 -0.006592035 0.03668361 0.9993093 -0.005951166 0.009216785 0.6187478 0.7855356 -0.01211613 0.3389475 0.9407274 0.01254338 0.7575789 0.6526232 -0.03598189 0.4905927 0.8706458 -0.03741693 0.3333038 0.9420768 0.6024539 0.04898369 0.7966492 0.4223474 -2.74668e-4 0.906434 0.4250679 -6.10379e-5 0.9051615 0.6036665 0.03982728 0.7962416 -0.07156717 0.4862907 0.8708614 -0.06204479 0.3291457 0.9422386 0.4234271 0.05496555 0.9042613 0.4352354 0.07367354 0.8972973 -0.1081604 0.4832732 0.8687626 -0.08853656 0.3314093 0.9393238 0.4096835 0.08291941 0.9084513 0.4452465 0.116187 0.8878379 -0.140542 0.467985 0.8724896 -0.1101732 0.3164809 0.9421793 0.4902057 0.1360258 0.8609271 -0.1782599 0.4636101 0.8679223 0.5212385 0.1681306 0.8366855 -0.1433167 0.3302143 0.9329624 0.3580174 0.1283931 0.9248453 0.3997135 0.1671551 0.9012705 -0.2903892 0.5967707 0.7480233 -0.2449482 0.4450635 0.8613472 0.3532315 0.156656 0.9223267 0.4240995 0.2211446 0.8781998 -0.2670736 0.428917 0.8629612 -0.3612883 0.6771943 0.6409983 0.4294632 0.2298995 0.8733313 -0.3015639 0.4674072 0.8310173 0.4802196 0.2847747 0.829634 -0.2144886 0.2920073 0.9320549 0.3210046 0.2037789 0.9248948 0.4144488 0.3039088 0.8578297 -0.3712953 0.4691396 0.801279 -0.3215761 0.3758079 0.869113 -0.3507267 0.3998016 0.8468469 0.4309924 0.3208181 0.8433988 0.5006093 0.4120726 0.7613058 -0.2580093 0.2595657 0.9306218 0.3712337 0.3221591 0.8708612 -0.4229029 0.3981825 0.814005 0.4230248 0.3983654 0.8138521 -0.3715093 0.3224956 0.8706191 0.2587434 0.2604525 0.9301701 -0.5014241 0.4127975 0.7603763 -0.9973363 -0.07294082 -2.74673e-4 -0.9893469 -0.1455774 -3.66233e-4 -0.9058941 -0.2314557 0.3546608 -0.1093797 -0.9940001 -3.66227e-4 -0.03653079 -0.9993324 -4.27261e-4 -0.06436365 -0.9979265 3.05186e-4 0.3228579 -0.9464475 -3.96744e-4 0.3689426 -0.9294522 3.66226e-4 0.3016235 -0.9534271 3.66233e-4 -0.9999999 -2.13636e-4 -5.1883e-4 -0.9083006 -0.1732552 0.3807526 -0.999954 0.006744682 0.006836235 0.9999999 -2.44156e-4 -4.27272e-4 0.9999905 0.004333674 6.40902e-4 0.99802 -0.06289887 0 0.9973319 -0.07300156 -6.10381e-5 0.991046 -0.1335209 6.10382e-5 -0.639689 -0.7686339 -3.66234e-4 -0.6072654 -0.794499 3.35708e-4 -0.6588848 -0.7522439 2.44156e-4 -0.5818789 -0.8132753 -3.96749e-4 0.2529126 -0.9674891 -3.6623e-4 0.2329804 -0.9724814 3.35707e-4 0.9893513 -0.1455476 -9.15586e-5 -0.181651 -0.9833632 -2.74674e-4 -0.1286085 -0.9916954 2.13635e-4 -0.1925113 -0.9812948 9.15558e-5 0.9793592 -0.2021282 9.15574e-5 0.9760792 -0.2174151 -1.83112e-4 0.9629707 -0.2696062 1.52596e-4 -0.6940948 -0.7198835 -2.74672e-4 -0.707412 -0.7068016 1.52598e-4 0.9576085 -0.288073 -2.13636e-4 0.9415506 -0.3368719 2.13635e-4 0.181616 -0.9833695 -3.66223e-4 0.1634586 -0.9865502 3.05188e-4 0.9340305 -0.3571931 -2.44151e-4 0.9150894 -0.4032509 2.44154e-4 -0.2529411 -0.9674817 -1.83114e-4 -0.2554756 -0.9668156 0 -0.3173354 -0.9483134 6.10378e-5 0.9054575 -0.424437 -2.74677e-4 0.8838992 -0.4676774 2.74673e-4 -0.7447886 -0.6673006 -1.83115e-4 -0.7533966 -0.6575664 6.10384e-5 0.872056 -0.4894061 -3.05192e-4 0.8483787 -0.5293898 2.74675e-4 0.8340031 -0.5517598 -3.35713e-4 0.808542 -0.5884384 3.05191e-4 -0.3228305 -0.9464569 -2.44152e-4 -0.3781004 -0.9257646 1.83114e-4 0.7914903 -0.6111817 -3.35714e-4 -0.7915018 -0.611167 -1.22075e-4 -0.7955284 -0.6059164 0 0.7644457 -0.6446882 3.35712e-4 -0.8338676 -0.5519647 3.05189e-5 0.1093495 -0.9940034 -3.35709e-4 0.7447885 -0.6673005 -3.66229e-4 0.7168356 -0.6972423 3.35711e-4 0.09879058 -0.9951083 6.10384e-5 -0.833989 -0.5517811 -1.52594e-4 -0.8687386 -0.495271 1.52598e-4 0.6940637 -0.7199134 -3.96747e-4 0.6662541 -0.7457249 3.66225e-4 -0.3910702 -0.9203608 -3.35709e-4 -0.4373717 -0.8992809 2.74673e-4 0.639674 -0.7686464 -3.96744e-4 0.611853 -0.7909714 3.96752e-4 -0.872043 -0.4894294 -2.74669e-4 -0.8998889 -0.4361192 2.44154e-4 0.03656125 -0.9993314 -3.66223e-4 0.04648047 -0.9989193 -3.05191e-5 -0.004364192 -0.9999905 2.13634e-4 0.5818587 -0.8132898 -4.27276e-4 0.5547495 -0.8320174 3.66232e-4 -0.4572322 -0.8893474 -3.96744e-4 -0.4948665 -0.8689691 3.05191e-4 -0.9054457 -0.424462 -3.35712e-4 -0.927499 -0.3738256 3.05189e-4 0.5209351 -0.8535962 -4.27271e-4 0.4963394 -0.8681286 3.66235e-4 -0.9340165 -0.3572299 -3.35714e-4 -0.9511978 -0.3085815 3.05194e-4 -0.5209351 -0.8535962 -4.27271e-4 -0.5516893 -0.8340497 3.66226e-4 -0.9576001 -0.288101 -3.6623e-4 0.4572445 -0.8893409 -4.27274e-4 -0.9701659 -0.2424423 3.05189e-4 0.4343863 -0.9007266 3.96756e-4 -0.9760792 -0.2174151 -3.66224e-4 -0.9846499 -0.1745408 3.35714e-4 0.391096 -0.9203498 -3.96742e-4 -0.07471024 -0.9972053 0 -0.1490224 -0.9888339 0 1 0 0 0.9972053 -0.07471024 0 0 -1 0 0.07471024 -0.9972053 0 -0.9972053 -0.07471024 0 -1 0 0 0.1490224 -0.9888339 0 -0.9888339 -0.1490224 0 0.222514 -0.9749296 0 -0.9749296 -0.222514 0 0.2947506 -0.9555743 0 -0.9555743 -0.2947506 0 0.3653473 -0.9308713 0 -0.9308713 -0.3653473 0 -0.9009695 -0.4338826 0 0.4338826 -0.9009695 0 -0.866024 -0.5000025 0 -0.8262397 -0.5633188 0 0.5000025 -0.866024 0 0.5633188 -0.8262397 0 -0.7818415 -0.6234773 0 0.6234773 -0.7818415 0 0.6234959 -0.7818266 0 0.6801831 -0.7330424 0 -0.7330424 -0.6801831 0 0.7330424 -0.6801831 0 -0.6801831 -0.7330424 0 -0.6234773 -0.7818415 0 -0.5633188 -0.8262397 0 0.7818415 -0.6234773 0 -0.5000025 -0.866024 0 0.8262397 -0.5633188 0 0.866024 -0.5000025 0 -0.4999893 -0.8660317 0 -0.4338826 -0.9009695 0 0.9009695 -0.4338826 0 -0.3653473 -0.9308713 0 -0.2947506 -0.9555743 0 0.9308713 -0.3653473 0 0.9555743 -0.2947506 0 0.9749296 -0.222514 0 -0.222514 -0.9749296 0 0.9888339 -0.1490224 0 -0.6182855 -0.2989646 0.7268723 -0.6256684 0.2615777 0.7349261 -0.7115982 -0.0878663 0.6970708 -0.1080388 -0.7346029 0.6698406 -0.4243751 -0.5834432 0.6924592 0.08295041 -0.7097481 0.6995548 0.2918257 -0.6892786 0.6631236 0.5336581 -0.4588556 0.7103946 0.6398001 -0.273969 0.7180508 0.6595845 -0.2611236 0.7048141 -0.9325997 -0.03439491 0.3592697 -0.9085031 -0.01257401 0.4176892 -0.9503591 -0.0534386 0.306532 -0.6534308 -0.657146 -0.3757489 0.08581918 -0.8735125 0.4791775 0.8903033 -0.3916282 -0.2323525 -0.1093495 -0.9940034 0 -0.1816214 -0.9833685 0 0.9973341 -0.07297116 0 0.9893469 -0.1455774 0 -0.03650027 -0.9993336 0 0.03650027 -0.9993336 0 -0.9973341 -0.07297116 0 0.1093495 -0.9940034 0 0.1816214 -0.9833685 0 -0.9893469 -0.1455774 0 -0.9760778 -0.2174216 0 0.2529126 -0.9674892 0 -0.9576001 -0.288101 0 0.3228579 -0.9464475 0 0.3019857 -0.8782154 -0.3708671 -0.9340204 -0.3572197 0 -0.9054458 -0.4244621 0 0.391096 -0.9203498 0 0.3689477 -0.8572563 -0.359151 0.4572446 -0.8893411 0 -0.872056 -0.4894061 0 -0.8340031 -0.5517598 0 0.5209352 -0.8535963 0 0.5818588 -0.8132899 0 0.5818443 -0.8133003 0 -0.7914904 -0.6111817 0 0.639674 -0.7686464 0 -0.7447886 -0.6673006 0 0.6940639 -0.7199135 0 -0.6940639 -0.7199135 0 -0.639674 -0.7686464 0 0.7447886 -0.6673006 0 0.7914904 -0.6111817 0 -0.5818588 -0.8132899 0 0.8340031 -0.5517598 0 -0.5209352 -0.8535963 0 -0.4572446 -0.8893411 0 0.872056 -0.4894061 0 0.9054458 -0.4244621 0 -0.391096 -0.9203498 0 0.9340204 -0.3572197 0 -0.3228579 -0.9464475 0 0.9576001 -0.288101 0 0.9760778 -0.2174216 0 -0.2529126 -0.9674892 0 0.06201457 -0.8448883 -0.531336 0.02664321 -0.7150027 -0.6986139 0 -0.847085 -0.5314576 0.07980734 -0.711216 -0.6984286 0.1236627 -0.8376229 -0.5320672 -0.7772319 -0.04992938 -0.6272302 -0.7806902 0 -0.6249184 -0.7796828 -0.06509816 -0.6227818 0.132668 -0.7042359 -0.6974605 -0.7809879 0 -0.6245462 0.1847649 -0.8267423 -0.5313748 0.1850385 -0.693246 -0.6965421 -0.7683246 -0.1022398 -0.6318421 -0.7758856 -0.1277832 -0.6177969 -0.7537018 -0.1539998 -0.6389192 0.244032 -0.8083944 -0.5356742 -0.7704291 -0.1901962 -0.6084938 0.3096778 -0.8059192 -0.5045731 -0.7357589 -0.2064019 -0.645025 -0.7592543 -0.2500735 -0.6008296 0.3590546 -0.762789 -0.5378037 0.3365058 -0.6403255 -0.6904687 -0.7142142 -0.2577666 -0.6507339 0.4136347 -0.7340314 -0.5386132 -0.7420488 -0.3076967 -0.5955557 0.3837262 -0.6145175 -0.6892912 -0.6881552 -0.3067216 -0.6575441 0.4649025 -0.6996275 -0.5425745 -0.7229194 -0.3653509 -0.5864354 0.4297974 -0.5862678 -0.6867054 -0.6601314 -0.3547253 -0.6621152 0.5132416 -0.6613208 -0.5470263 -0.6969488 -0.4196769 -0.5814926 0.4739079 -0.5545712 -0.6840046 -0.6282992 -0.3998323 -0.6673638 0.5572469 -0.618285 -0.554256 -0.6678811 -0.4728027 -0.5747977 0.5161682 -0.5203493 -0.6802994 -0.5936559 -0.4427088 -0.6719982 0.5993679 -0.5739148 -0.5580143 -0.6345021 -0.5235024 -0.5686408 0.5556962 -0.4822972 -0.6771937 -0.5557206 -0.4823223 -0.6771557 0.6119357 -0.479758 -0.6287822 0.5940681 -0.4431477 -0.6713443 -0.5993679 -0.5739148 -0.5580143 0.6689305 -0.4737876 -0.572763 -0.5167812 -0.5209318 -0.6793874 0.6276547 -0.3993722 -0.6682451 -0.5559435 -0.6166773 -0.5573474 0.696905 -0.4196384 -0.5815731 -0.4743313 -0.5547803 -0.6835413 0.6599447 -0.3545097 -0.6624167 -0.5111714 -0.6583974 -0.5524643 0.7213223 -0.3642472 -0.5890825 -0.4300516 -0.5860983 -0.686691 0.6889526 -0.3072117 -0.6564796 -0.4630126 -0.696518 -0.5481624 0.7420963 -0.3077215 -0.5954837 -0.3840216 -0.6143187 -0.6893041 0.714331 -0.2579172 -0.650546 0.761298 -0.250592 -0.598021 -0.4120408 -0.7309672 -0.5439756 0.7380235 -0.2067698 -0.6423143 -0.3362357 -0.6392049 -0.6916378 0.7691181 -0.1894336 -0.610387 -0.3582634 -0.7612372 -0.5405232 0.7539729 -0.1544568 -0.6384888 -0.2869395 -0.6606444 -0.6936964 0.7762314 -0.1281511 -0.6172862 -0.3021673 -0.7871427 -0.5376814 0.7680433 -0.1019337 -0.6322334 -0.2364339 -0.6785991 -0.6954153 0.7796828 -0.06509816 -0.6227818 -0.2440953 -0.8085542 -0.5354043 0.7772468 -0.04993033 -0.6272116 -0.1849756 -0.6931473 -0.696657 0.7807438 0 -0.6248514 0.7809879 0 -0.6245462 -0.1844276 -0.8251478 -0.533964 -0.1326952 -0.7040661 -0.6976267 -0.1236342 -0.8373308 -0.5325334 -0.07980901 -0.7112312 -0.698413 -0.06201457 -0.8448883 -0.531336 -0.02664321 -0.7150027 -0.6986139 -0.1090145 -0.9940401 -4.88307e-4 -0.1814684 -0.9833968 -2.74675e-4 -0.1095036 -0.9939863 -4.57791e-4 0.9973319 -0.07300156 1.22076e-4 0.989365 -0.1454535 4.57785e-4 0.9973341 -0.07297116 6.10382e-5 0.9999999 3.05194e-5 5.1883e-4 0.9893021 -0.1458805 4.88303e-4 0.9999999 -6.10389e-5 5.79869e-4 -0.03640884 -0.9993368 -5.18819e-4 -0.03671473 -0.9993258 -3.96752e-4 0.03680557 -0.9993048 -0.005951106 -0.997334 -0.07297116 3.05191e-4 -1 -3.05185e-5 1.52593e-4 -0.9973296 -0.0730319 3.0519e-4 -1 3.05185e-5 1.83111e-4 0.1089819 -0.9940215 -0.00665301 0.181804 -0.9833347 -4.57791e-4 -0.9893469 -0.1455774 2.44155e-4 -0.9893563 -0.1455133 2.4415e-4 -0.9760922 -0.217357 6.10382e-5 0.1814979 -0.9833913 -4.88308e-4 -0.9760649 -0.2174797 3.05192e-5 0.2527909 -0.967521 -2.44154e-4 0.2529698 -0.9674742 -2.74669e-4 -0.9575639 -0.2882214 -9.1557e-5 -0.9576278 -0.2880086 -6.10382e-5 0.3225564 -0.9465502 -2.13634e-4 0.3230132 -0.9463945 -2.13633e-4 0.3909558 -0.9204095 -2.44157e-4 -0.9339234 -0.3574734 -9.15581e-5 -0.9340549 -0.3571296 -3.05187e-5 -0.9054169 -0.4245238 3.05193e-5 -0.9054747 -0.4244003 6.10384e-5 0.391107 -0.9203453 -2.74675e-4 -0.8721097 -0.4893105 -9.15569e-5 0.4572687 -0.8893287 -9.15575e-5 0.457208 -0.8893598 -9.15576e-5 -0.8719893 -0.489525 -6.1038e-5 -0.8340638 -0.5516681 -1.52597e-4 0.5208771 -0.8536317 -2.13637e-4 0.5210512 -0.8535254 -1.83114e-4 -0.8338068 -0.5520564 -1.52594e-4 0.5820029 -0.8131866 -3.66232e-4 0.5816105 -0.8134673 -4.27273e-4 -0.7913562 -0.6113555 -2.7467e-4 -0.7915836 -0.6110609 -2.13636e-4 0.6398062 -0.7685364 -2.13635e-4 0.6394726 -0.7688138 -3.05194e-4 -0.7446736 -0.667429 -3.35714e-4 -0.7448748 -0.6672043 -2.7467e-4 0.6942192 -0.7197637 -3.6623e-4 0.6939085 -0.7200632 -4.57784e-4 0.7448748 -0.6672043 -2.44151e-4 -0.6939706 -0.7200034 -2.44152e-4 -0.6941881 -0.7197937 -2.13634e-4 -0.63974 -0.7685914 -1.52595e-4 0.7446008 -0.66751 -3.05189e-4 0.7915836 -0.6110609 -3.05195e-4 -0.6394066 -0.7688689 -6.10383e-5 -0.5822367 -0.8130194 -3.05187e-5 0.79133 -0.6113893 -3.66229e-4 0.8340638 -0.5516681 -1.52597e-4 -0.5813826 -0.8136304 3.05188e-5 -0.5209932 -0.8535609 -2.13634e-4 0.8339283 -0.5518729 -1.52594e-4 -0.5207833 -0.853689 -1.52597e-4 -0.45739 -0.8892661 -3.96749e-4 0.8720024 -0.4895018 0 0.8720893 -0.4893467 0 0.905492 -0.4243635 3.05188e-5 -0.4568438 -0.8895468 -4.5779e-4 0.9053182 -0.4247341 3.05191e-5 -0.3907677 -0.9204893 -4.57788e-4 -0.3912841 -0.9202699 -3.66228e-4 0.9339578 -0.3573834 0 0.9340509 -0.3571398 0 0.9576195 -0.2880366 -6.10376e-5 -0.3227391 -0.946488 -2.74672e-4 -0.3229858 -0.9464038 -2.44154e-4 -0.2530915 -0.9674423 -4.88299e-4 0.9575555 -0.2882493 -6.10374e-5 -0.2525473 -0.9675844 -5.49348e-4 0.9760857 -0.217386 3.05189e-5 -0.181769 -0.9833413 -2.44149e-4 0.05768126 -0.7862054 -0.6152673 0.02356082 -0.6321142 -0.7745169 0 -0.7886352 -0.6148614 0.07059168 -0.6288552 -0.7743113 0.1149958 -0.7792132 -0.6161192 -0.7034264 -0.04492354 -0.709347 -0.7068321 0 -0.7073814 -0.706399 -0.05923789 -0.7053307 0.117377 -0.6225925 -0.7736933 -0.7071068 0 -0.7071068 0.1715176 -0.7677704 -0.6173415 0.1637052 -0.6131622 -0.7728084 -0.6936471 -0.09140586 -0.7144919 -0.7042813 -0.1169478 -0.7002223 -0.6795622 -0.1384943 -0.7204268 0.2268162 -0.7515881 -0.6194109 0.2093327 -0.6005308 -0.7717142 -0.6993517 -0.1730144 -0.6935225 0.2805302 -0.7311118 -0.6219151 -0.6619709 -0.1851321 -0.7263063 0.2541347 -0.5848423 -0.7703084 -0.6906798 -0.2278867 -0.6863155 0.3323255 -0.7064016 -0.6249454 0.2979295 -0.5661638 -0.7685679 -0.6410179 -0.2306309 -0.7320556 0.3818009 -0.6777195 -0.6284303 -0.6777701 -0.2816921 -0.679174 0.3405668 -0.544559 -0.766466 -0.6170277 -0.274577 -0.7374852 0.4286713 -0.6452653 -0.6323558 -0.66056 -0.334278 -0.6722489 0.3817982 -0.5199597 -0.7641152 -0.5903893 -0.3166647 -0.7424042 0.4725337 -0.6090474 -0.6370033 -0.6392257 -0.3854581 -0.6654417 0.4214149 -0.4925256 -0.7614645 -0.5611246 -0.3566465 -0.7469555 0.513155 -0.5696466 -0.6420084 -0.6136502 -0.4349601 -0.6589713 0.4592576 -0.4623401 -0.7585014 -0.52935 -0.3943344 -0.7511917 0.550441 -0.5273382 -0.6472473 -0.5839846 -0.4822946 -0.6529579 0.4953857 -0.4296476 -0.754981 -0.4953857 -0.4296476 -0.754981 0.5839846 -0.4822946 -0.6529579 0.5293621 -0.3943435 -0.7511785 -0.5504499 -0.5273161 -0.6472578 0.6136502 -0.4349601 -0.6589713 -0.4592816 -0.4623335 -0.7584907 0.5611246 -0.3566465 -0.7469555 -0.5131775 -0.5696377 -0.6419984 0.6392257 -0.3854581 -0.6654417 -0.4214149 -0.4925256 -0.7614645 0.5903893 -0.3166647 -0.7424042 -0.4725248 -0.6090666 -0.6369915 0.6605908 -0.3342782 -0.6722187 -0.3817982 -0.5199597 -0.7641152 0.6170329 -0.2745488 -0.7374913 -0.4286962 -0.6452568 -0.6323475 0.6777701 -0.2816921 -0.679174 -0.3405668 -0.544559 -0.766466 0.6409999 -0.2306354 -0.7320699 0.6906798 -0.2278867 -0.6863155 -0.3818009 -0.6777195 -0.6284303 0.6619709 -0.1851321 -0.7263063 -0.2979295 -0.5661638 -0.7685679 0.6993517 -0.1730144 -0.6935225 -0.3323255 -0.7064016 -0.6249454 0.6795622 -0.1384943 -0.7204268 -0.2541347 -0.5848423 -0.7703084 0.7042813 -0.1169478 -0.7002223 -0.2805302 -0.7311118 -0.6219151 0.6936471 -0.09140586 -0.7144919 -0.2093327 -0.6005308 -0.7717142 0.706399 -0.05923789 -0.7053307 -0.2268162 -0.7515881 -0.6194109 0.7034264 -0.04492354 -0.709347 -0.1637052 -0.6131622 -0.7728084 0.7068321 0 -0.7073814 0.7071068 0 -0.7071068 -0.1715176 -0.7677704 -0.6173415 -0.117377 -0.6225925 -0.7736933 -0.1149958 -0.7792132 -0.6161192 -0.07059168 -0.6288552 -0.7743113 -0.05768126 -0.7862054 -0.6152673 -0.02356082 -0.6321142 -0.7745169 -1.4039e-7 0 -1 4.18807e-7 0 -1 -4.17646e-7 0 -1 -2.08821e-7 0 -1 5.58423e-7 0 -1 2.80774e-7 0 -1 0 0 -1 -4.29555e-7 0 -1 -4.35675e-7 0 -1 0 0 -1 9.04303e-7 0 -1 -3.85602e-7 0 -1 9.49978e-7 0 -1 -8.96793e-7 0 -1 -1.68464e-7 0 -1 -4.36609e-7 0 -1 -6.81358e-7 0 -1 2.46631e-6 0 -1 -1.93885e-6 0 -1 8.89358e-7 0 -1 8.63863e-7 0 -1 -9.17016e-7 0 -1 -1.06409e-6 0 -1 2.61676e-6 0 -1 -3.76029e-6 0 -1 1.17585e-6 0 -1 8.55148e-7 0 -1 3.75751e-7 0 -1 2.28735e-6 0 -1 -2.78512e-6 0 -1 1.30742e-6 0 -1 -3.57261e-6 0 -1 3.43093e-7 0 -1 2.40527e-6 0 -1 2.85671e-6 0 -1 -2.85799e-6 0 -1 -7.29476e-6 0 -1 3.6232e-6 0 -1 1.00886e-5 0 -1 -1.50614e-5 0 -1 8.3359e-6 0 -1 1.66277e-5 0 -1 -2.49014e-5 0 -1 8.33594e-6 0 -1 -1.00418e-5 0 -1 -3.36251e-6 0 -1 1.20778e-6 0 -1 3.64707e-6 0 -1 -2.14353e-6 0 -1 2.30592e-6 0 -1 -2.38055e-6 0 -1 8.01805e-7 0 -1 1.71541e-6 0 -1 -7.44293e-7 0 -1 1.56891e-6 0 -1 -6.38252e-7 0 -1 -1.76748e-6 0 -1 -2.8181e-7 0 -1 2.56551e-7 0 -1 1.33261e-6 0 -1 3.97723e-7 0 -1 -8.05151e-7 0 -1 3.12977e-7 0 -1 -5.86874e-7 0 -1 -2.2115e-7 0 -1 5.23163e-7 0 -1 -3.97697e-7 0 -1 2.84577e-7 0 -1 -3.6338e-7 0 -1 0 0 -1 1.68463e-7 0 -1 3.26102e-7 0 -1 -1.58335e-7 0 -1 0 0 -1 -1.41579e-7 0 -1 -0.05771082 0.7862833 -0.6151651 -0.02356046 0.6321659 -0.7744749 0 0.7886089 -0.6148952 -0.0706216 0.6289722 -0.7742136 -0.1150253 0.7793582 -0.6159301 0.7034264 0.04492354 -0.709347 -0.1174098 0.6227572 -0.7735557 0.706399 0.05923789 -0.7053307 -0.1715208 0.7677848 -0.6173226 -0.163706 0.6131957 -0.7727817 0.693663 0.09140396 -0.7144768 0.7042964 0.1169503 -0.7002066 0.6795622 0.1384943 -0.7204268 -0.2268214 0.7515748 -0.6194251 -0.2093641 0.6005942 -0.7716563 0.6993517 0.1730144 -0.6935225 -0.2805671 0.7311896 -0.6218072 0.6619709 0.1851321 -0.7263063 -0.2541675 0.5849393 -0.770224 0.6906798 0.2278867 -0.6863155 -0.3323565 0.7064942 -0.6248242 -0.2979907 0.5662556 -0.7684766 0.6411103 0.2306617 -0.731965 -0.3818513 0.6777921 -0.6283212 0.67787 0.2817261 -0.6790602 -0.3406276 0.5446502 -0.7663741 0.6171676 0.2746155 -0.7373539 -0.428795 0.6453899 -0.6321446 0.6606831 0.3343396 -0.6720974 -0.3818906 0.5201135 -0.7639644 0.5904635 0.3167023 -0.7423292 -0.4726155 0.6091571 -0.6368377 0.6392492 0.3854844 -0.6654039 -0.4214754 0.4926165 -0.761372 0.5611925 0.3567119 -0.7468731 -0.513311 0.569742 -0.641799 0.6137485 0.4349954 -0.6588564 -0.4593811 0.4624636 -0.7583512 0.5294517 0.3944031 -0.7510841 -0.5505415 0.5274077 -0.6471052 0.5840541 0.4823627 -0.6528453 -0.4954873 0.4297174 -0.7548743 0.495476 0.4297075 -0.7548875 -0.5840541 0.4823627 -0.6528453 -0.5294517 0.3944031 -0.7510841 0.5505415 0.5274077 -0.6471052 -0.613709 0.4350195 -0.6588773 0.4594052 0.4624571 -0.7583405 -0.5611925 0.3567119 -0.7468731 0.5133021 0.5697626 -0.6417878 -0.6392312 0.3854919 -0.6654169 0.4214754 0.4926165 -0.761372 -0.5904635 0.3167023 -0.7423292 0.4726247 0.609169 -0.6368196 -0.6606831 0.3343396 -0.6720974 0.3818906 0.5201135 -0.7639644 -0.6171676 0.2746155 -0.7373539 0.428795 0.6453899 -0.6321446 -0.67787 0.2817261 -0.6790602 0.3406007 0.5446559 -0.766382 -0.6411103 0.2306617 -0.731965 -0.6906798 0.2278867 -0.6863155 0.3818592 0.6777757 -0.6283342 -0.6619709 0.1851321 -0.7263063 0.2979907 0.5662556 -0.7684766 -0.6993517 0.1730144 -0.6935225 0.3323565 0.7064942 -0.6248242 -0.6795622 0.1384943 -0.7204268 0.2541675 0.5849393 -0.770224 -0.7042964 0.1169503 -0.7002066 0.2805671 0.7311896 -0.6218072 -0.6936471 0.09140586 -0.7144919 0.2093641 0.6005942 -0.7716563 -0.706399 0.05923789 -0.7053307 0.2268214 0.7515748 -0.6194251 -0.7034264 0.04492354 -0.709347 0.163706 0.6131957 -0.7727817 0.1715208 0.7677848 -0.6173226 0.1174098 0.6227572 -0.7735557 0.1150253 0.7793582 -0.6159301 0.0706216 0.6289722 -0.7742136 0.05771082 0.7862833 -0.6151651 0.02356046 0.6321659 -0.7744749 -0.997334 0.07297116 3.66229e-4 -0.9973296 0.0730319 3.96747e-4 0.0365917 0.9993303 -3.05186e-5 -0.03668427 0.999327 -1.52597e-4 -0.03656125 0.9993315 -9.15558e-5 -0.1095337 0.9939829 -4.57789e-4 0.9973319 0.07300156 1.22076e-4 0.9973341 0.07297116 6.10382e-5 0.9893563 0.1455133 2.74669e-4 -0.108924 0.99405 -4.88312e-4 0.989311 0.1458208 3.05192e-4 -0.1814684 0.9833968 -3.35714e-4 -0.181769 0.9833412 -3.05186e-4 -0.2530915 0.9674423 -5.18818e-4 0.9760857 0.217386 2.44151e-4 -0.2525473 0.9675844 -5.49348e-4 0.9760714 0.2174507 2.74675e-4 0.9575807 0.2881654 0 0.9576195 0.2880366 -3.05188e-5 0.9340509 0.3571398 9.15587e-5 -0.3227118 0.9464973 -2.44155e-4 -0.3230132 0.9463945 -2.13633e-4 -0.3912841 0.9202699 -3.66228e-4 0.9339374 0.3574366 1.22075e-4 0.905492 0.4243635 0 -0.3906164 0.9205535 -4.57789e-4 -0.4574142 0.8892537 -3.35706e-4 0.9054169 0.4245238 0 0.8720893 0.4893467 -9.15579e-5 -0.4568438 0.8895468 -4.5779e-4 -0.5211093 0.85349 -3.66226e-4 0.8719689 0.4895612 -6.10387e-5 0.8340498 0.5516893 -3.05189e-5 -0.5207116 0.8537326 -3.96744e-4 0.8339283 0.5518729 0 0.7915836 0.6110609 -3.35714e-4 -0.5817202 0.813389 -1.52594e-4 -0.5819337 0.8132362 -1.83113e-4 0.7913823 0.6113215 -3.66225e-4 -0.6395238 0.7687714 -2.44151e-4 -0.6397551 0.768579 -2.74678e-4 0.7446009 0.6675101 -2.7467e-4 0.7448613 0.6672195 -2.44157e-4 0.694157 0.7198236 -3.96748e-4 -0.6940017 0.7199735 -2.74671e-4 -0.6941571 0.7198237 -2.44153e-4 -0.7449036 0.6671721 -3.66226e-4 0.6938616 0.7201084 -4.27273e-4 -0.7444856 0.6676383 -4.57788e-4 -0.7915836 0.6110609 -3.05195e-4 0.6395057 0.7687864 -3.05195e-4 0.6398062 0.7685364 -2.13635e-4 -0.79133 0.6113893 -3.66229e-4 -0.8340731 0.551654 -2.1363e-4 0.5814518 0.8135808 -4.57788e-4 0.5820376 0.8131618 -3.66234e-4 -0.8339283 0.5518729 -2.44151e-4 0.5210512 0.8535254 -2.44152e-4 0.457208 0.8893598 0 -0.8720227 0.4894656 3.05191e-5 -0.8720893 0.4893467 0 0.391096 0.9203498 0 -0.9054169 0.4245238 -3.05193e-5 -0.9054865 0.4243752 -6.10392e-5 -0.9340509 0.3571398 6.10391e-5 0.3228945 0.9464349 -3.96751e-4 -0.9339578 0.3573834 6.1039e-5 0.3226751 0.9465098 -3.96744e-4 0.2530058 0.9674648 -3.05194e-4 -0.9575639 0.2882214 1.52595e-4 -0.9576278 0.2880086 1.22076e-4 0.2527623 0.9675285 -2.74675e-4 0.18171 0.9833523 -2.74671e-4 -0.976044 0.2175735 2.74675e-4 -0.9760922 0.217357 2.44153e-4 0.18171 0.9833523 -2.44152e-4 0.1093193 0.9940067 0 -0.9893381 0.1456372 1.22076e-4 -0.9893513 0.1455476 9.15586e-5 0 -0.3275036 0.9448499 0.07712209 -0.5683904 0.8191365 0.118843 -0.5590869 0.8205475 0.02652126 -0.4435982 0.8958333 0.006470024 -0.414723 0.9099248 -0.0035097 -0.5792004 0.8151776 -0.02969515 -0.5289585 0.8481281 -0.06979697 -0.5168091 0.8532508 -0.0853942 -0.4862098 0.8696597 -0.0644868 -0.4433505 0.8940256 -0.1429531 -0.5144665 0.845511 -0.1473767 -0.4775934 0.866132 -0.1440497 -0.4578158 0.8772996 -0.1491186 -0.4387797 0.8861355 -0.1896138 -0.4157283 0.8895036 -0.2490697 -0.4186056 0.8733463 -0.2855046 -0.3532261 0.8909087 -0.2219338 -0.3570718 0.9073286 -0.1957179 -0.3298182 0.9235337 -0.2433596 -0.3259445 0.9135296 -0.1875431 -0.3114224 0.9315813 -0.1977018 -0.294691 0.9349178 -0.2015461 -0.2822379 0.9379344 0 -0.3275309 0.9448406 -1.35368e-5 -0.3275611 0.94483 -0.1797882 -0.2337765 0.9555234 -0.002899289 -0.3264351 0.9452152 -0.1678262 -0.2330157 0.9578821 -0.08255541 -0.2660391 0.9604207 -0.1597956 -0.1725218 0.9719577 -0.1386492 -0.1589446 0.9775034 -0.1244875 -0.125342 0.9842725 -0.1025146 -0.1213451 0.9873025 -0.06799572 -0.1645876 0.9840161 -0.05511742 -0.1736535 0.9832633 -0.04434484 -0.1381924 0.9894123 -0.01007115 -0.1411492 0.9899372 0.01019328 -0.2033781 0.9790472 0.02392655 -0.1255841 0.9917944 0.04272699 -0.1590055 0.9863528 0.06970632 -0.1483243 0.9864791 0.09293007 -0.143744 0.985242 0.1115172 -0.1243352 0.9859537 0.1145696 -0.1474084 0.9824178 0.1308958 -0.1575083 0.9788041 0.1122797 -0.200724 0.9731923 0.1699004 -0.1534506 0.9734408 0.1825631 -0.1575072 0.9704959 0.1804607 -0.1915697 0.9647461 0.1254958 -0.2438196 0.9616668 0.1510081 -0.2689336 0.9512473 0.1239394 -0.2698528 0.9548919 0.2242247 -0.2860872 0.9315994 0.2856273 -0.1613844 0.9446545 0.2221211 -0.3053784 0.9259624 0.2209315 -0.3290015 0.9181217 0.2158944 -0.2998839 0.9292251 0.03006106 -0.2934082 0.9555146 0.3691639 -0.3089184 0.8765202 0.1919351 -0.343585 0.9192988 0.3229486 -0.3622872 0.8743296 0.1721599 -0.3663243 0.914422 0.261151 -0.403339 0.8769937 0.2229715 -0.4284557 0.8756195 0.2239172 -0.4256472 0.8767471 0.1272338 -0.3987617 0.9081854 0.2216011 -0.4659392 0.8566176 0.2004207 -0.4703952 0.8593951 0.1969119 -0.4912422 0.8484733 0.09903335 -0.4674558 0.8784518 0.07870894 -0.4167089 0.9056261 0.1231145 -0.5131198 0.8494417 0.3170311 -0.2016083 0.9267392 0.388944 -0.2352464 0.8907198 0.4066078 -0.2984477 0.8634809 -0.157235 -0.2147943 0.9639194 -0.2497718 -0.3959909 0.8836319 -0.2802567 -0.3554862 0.8916759 -0.2327089 -0.2711631 0.9339792 0.3293645 -0.1759439 0.9276653 -0.3031204 -0.5255776 0.7949128 0.3897633 -0.1982845 0.8993152 0.3406221 -0.1512214 0.9279594 -0.2621918 -0.4193665 0.8691303 -0.1655958 -0.31404 0.9348566 -0.2120484 -0.4516557 0.8666272 0.5044503 -0.2057293 0.8385735 0.4681392 -0.1681016 0.867518 -0.1404482 -0.3230736 0.9358941 -0.1800608 -0.4700502 0.8640782 0.575598 -0.1890378 0.7955826 0.5357632 -0.1468885 0.8314936 -0.115819 -0.3321061 0.9361045 -0.1458218 -0.4832143 0.8632728 0.5185236 -0.1086488 0.8481325 0.5450766 -0.1388938 0.8268012 -0.08887231 -0.3333017 0.9386223 -0.10877 -0.4880001 0.8660399 0.3906732 -0.0511192 0.9191091 0.4051724 -0.07013303 0.9115463 -0.06381613 -0.3383079 0.9388692 -0.07352149 -0.5008251 0.8624203 0.3828948 -0.04321527 0.9227806 -0.03927809 -0.3486809 0.9364182 -0.03753876 -0.5133655 0.8573487 0.4221694 -0.01553416 0.9063839 0.9969828 -0.0730561 -0.02623552 0.2759764 -0.01958996 0.9609649 -0.01257371 -0.3560941 0.9343656 0.009094655 -0.631072 0.775671 0.02597218 -0.3345559 0.942018 0.02212655 -0.3102295 0.9504042 0.01263493 -0.7742734 0.6327251 0.06753766 -0.9229746 -0.3788886 -0.4373102 -0.02566665 0.8989443 -0.4409754 -0.03708112 0.8967528 0.0803579 -0.5944595 0.8001003 0.06601345 -0.3543912 0.9327642 0.1065407 -0.8045947 0.5841887 0.1137457 -0.5094906 0.8529252 -0.3781592 -0.04950153 0.9244162 -0.3902501 -0.06705087 0.9182642 0.09231901 -0.3439151 0.9344516 0.1414551 -0.4724326 0.8699413 -0.5463555 -0.1371535 0.8262474 0.1160962 -0.3396822 0.9333477 -0.5214563 -0.1086189 0.8463364 0.2747997 -0.6886473 0.6710069 -0.4979559 -0.1384974 0.8560715 -0.5375011 -0.1784447 0.8241663 0.2509312 -0.5669605 0.784595 -0.4710077 -0.1685898 0.8658691 -0.5088551 -0.2074729 0.8354769 0.249738 -0.5616741 0.7887669 0.4653078 -0.881915 -0.07559651 -0.3556388 -0.1579056 0.9211878 -0.4240295 -0.2211094 0.8782424 0.1905328 -0.3982481 0.8972713 0.3547245 -0.6382173 0.6832637 0.186686 -0.3923245 0.9006831 0.2951171 -0.4728282 0.8302648 0.3886644 -0.5861554 0.7108882 -0.4297468 -0.2299959 0.8731664 -0.4804637 -0.2848967 0.8294508 0.3272036 -0.4460175 0.8330705 -0.3132766 -0.1986171 0.9286599 -0.3745625 -0.2661887 0.8881704 0.4234583 -0.5470929 0.7220613 0.3543286 -0.4138717 0.8385472 -0.292438 -0.2175431 0.9312117 -0.358536 -0.2968878 0.8850479 0.4290996 -0.4804634 0.7648716 0.3510608 -0.3543568 0.8667109 0.3804815 -0.3736147 0.8459586 -0.2831911 -0.2451027 0.927215 -0.3797767 -0.373032 0.8465322 0.2821539 -0.2440959 0.9277966 0.27678 -0.2371658 0.9312064 -0.3473055 -0.3501132 0.8699423 -0.3992255 -0.4351165 0.8070271 0.585424 -0.43243 -0.6857719 0.2758008 -0.2182418 0.9361114 0.2778807 -0.2195883 0.9351809 -0.6743901 0.6808908 0.2856323 -0.6478832 0.7395617 0.1824718 -0.7070384 0.6780451 0.2008773 -0.176185 -0.8582495 0.4820443 -0.04611474 -0.4788793 0.8766688 -0.9081678 -0.3278096 0.260331 -0.9459586 -0.2919727 0.1411186 -0.9563039 -0.2910264 -0.02804666 -0.2324352 -0.05951267 0.9707894 -0.2187283 -0.01394706 0.9756862 -0.7151721 0.5978842 0.3620267 -0.3006728 -0.01586985 0.9535953 -0.7571496 0.6377892 0.1412426 -0.1550359 -0.3814554 0.9112935 -0.2797366 -0.3994014 0.8730555 -0.75548 0.5992811 0.2647873 -0.3662359 -0.4262681 0.8271438 -0.4221087 0.07696896 0.9032718 -0.2466883 -0.1040101 0.9634972 -0.7885521 0.5057012 0.3499318 -0.8039671 0.5661921 0.1818333 -0.3076619 -0.08517849 0.9476755 -0.4464297 0.03634792 0.8940802 -0.3827696 -0.4782636 0.7904122 -0.3638843 -0.4742735 0.8016564 0.1408761 0.8995804 0.4134114 0.2038699 0.8157241 0.5413237 -0.8359277 0.462095 0.2961302 0.04779303 -0.05935978 0.997092 0.05996936 0.006714105 0.9981777 -0.2154974 -0.1846423 0.9588891 -0.7614353 0.3280255 0.5591204 -0.7398508 -0.59525 0.3135256 -0.8563616 -0.5160754 0.01763993 -0.7840672 -0.5941771 0.179422 -0.1749382 -0.4311499 0.885159 -0.8438923 0.3789595 0.3797836 -0.8773955 0.4331585 0.206279 -0.7232767 -0.6425533 0.2529745 -0.7655639 -0.6305487 0.1277517 0.03335756 0.8745303 0.4838223 6.10377e-5 -0.1349239 0.990856 -0.322529 -0.1381308 0.9364268 -0.118752 -0.398831 0.909303 -0.8807726 0.3177007 0.3511493 -0.9052563 0.3855473 0.178506 -0.3407499 -0.1517733 0.9278224 -0.7029137 -0.6860978 0.1875697 -0.7247434 -0.6812534 0.1031554 -0.1294915 -0.2755547 0.9525238 -0.9091529 0.2586854 0.3263782 -0.09622639 0.8996999 0.4257705 -0.05661296 0.9569579 0.2846519 -0.2797098 -0.5343327 0.7976535 -0.7845531 0.1136529 0.6095569 -0.6982472 -0.7152769 0.02881002 -0.03228896 -0.01989829 0.9992806 -0.1323305 0.953628 0.2703375 -0.9093254 0.1970332 0.3664768 -0.9467367 0.2245604 0.2307863 -0.1694749 0.8997768 0.4020948 -0.2780919 -0.5513312 0.7865741 -0.03088515 -0.123968 0.9918054 -0.2077714 0.9558767 0.2076798 -0.216867 -0.2623408 0.9402904 -0.641625 -0.04574751 0.765653 -0.6521918 -0.03830134 0.7570858 -0.2994807 -0.2415866 0.9230099 -0.9681485 0.2362157 0.08301115 -0.2147623 0.8973204 0.3856079 -0.2830913 -0.5836699 0.7610446 -0.07644903 0.03576773 0.9964318 -0.2833399 0.9343748 0.2160146 0.001197993 0.3272626 -0.9449327 -0.281113 0.8678156 0.4097213 -0.361249 -0.2532131 0.8974309 -0.1791484 -0.5331117 0.8268603 -0.07678598 -0.05868816 0.995319 -0.3473981 0.8919491 0.2893814 -0.3171907 -0.2846567 0.9046329 -0.3774937 0.8138594 0.4417369 -0.1279964 -0.001312255 0.9917739 -0.4168576 0.8871842 0.1978234 -0.4436311 -0.8095597 0.3844538 -0.7314236 -0.2143059 0.6473736 -0.994213 0.07751786 0.07437443 -0.9890447 -0.00939995 0.1473175 -0.4087694 0.8340494 0.3704987 -0.2962254 -0.7558236 0.5839359 -0.4701806 -0.8731272 0.128761 -0.3824669 -0.8905816 0.2461375 -0.4751545 0.8575304 0.1971545 -0.1732258 0.05700945 0.9832307 -0.9306073 -0.1547909 0.3316774 -0.4786937 0.7942623 0.3741654 -0.182167 0.01794505 0.9831039 -0.5345503 0.7950653 0.2865787 -0.2343866 -0.7916653 0.5642066 -0.3561283 -0.9299499 0.09146595 -0.6804702 -0.301658 0.6678045 -0.9699227 -0.1800312 0.1638257 -0.5617104 0.7238296 0.4006897 -0.1862255 -0.04483205 0.9814836 -0.2743063 -0.9294309 0.2468085 -0.5966467 0.7826906 0.1772239 -0.8865864 -0.2953355 0.3560079 -0.5987288 0.7279172 0.3341564 0.63997 0.002044796 0.7683973 0.3700379 -0.6581945 0.655631 0.7966938 0.508413 0.3267955 0.1666946 -0.09268611 0.9816426 0.3930246 -0.132117 0.9099872 0.8682986 0.3201147 0.378925 0.8704788 0.3226222 0.3717282 0.337783 -0.5769594 0.7436536 0.203653 -0.2211099 0.9537484 0.2645389 -0.1934295 0.9447774 0.8515539 0.3944945 0.3452972 0.381925 -0.5675161 0.7294237 0.8412008 0.380544 0.3841452 0.204051 -0.202525 0.9577822 0.2030725 -0.4895531 0.8479972 0.822719 0.4570627 0.3379749 0.801153 0.4962992 0.3344267 0.3173043 -0.09036624 0.9440085 0.3456894 -0.07498532 0.9353481 0.3857004 -0.5405543 0.7476874 0.6259856 -0.5809998 0.5201745 0.7573918 0.5605438 0.3348857 0.2774531 -0.09226053 0.956299 -0.03329688 -0.6292233 0.7765111 -0.0744062 -0.7939612 0.6033983 0.7166638 0.5990714 0.3570806 0.4521442 -0.49832 0.7397586 0.2574597 -0.08771204 0.9622999 0.5670555 -0.4902681 0.6618802 0.6456398 0.6375216 0.4203754 0.2172939 -0.06802642 0.973733 0.02279782 -0.7922478 0.6097735 0.4681745 -0.4375021 0.7677269 0.5729979 0.7258991 0.3804523 0.01986783 -0.4510397 0.8922827 0.09900325 -0.9631762 0.2499802 0.2112495 -0.01629692 0.9772963 0.2653948 -0.3594244 0.8946394 0.3962326 -0.3874736 0.8323845 0.5404025 0.7585228 0.3641544 0.1139897 -0.8166056 0.5658283 0.6058147 -0.3535699 0.712725 0.2132349 0.03372323 0.9764189 0.4857454 0.7667978 0.41961 0.2117129 0.07495558 0.9744534 0.4866544 -0.3506926 0.800114 0.4166848 0.813135 0.4064301 0.4731017 -0.3084226 0.8252577 0.6961477 -0.2584078 0.669779 0.6849824 -0.252031 0.6835785 0.1436223 0.01037645 0.9895781 0.1088607 -0.5776916 0.8089635 0.5897011 0.04288011 0.8064824 0.3541499 0.816553 0.4558719 0.1191456 0.02233976 0.9926255 0.2133891 -0.7328538 0.6460577 0.7871248 0.01931875 0.6164911 0.2818436 0.8647884 0.4155782 0.08084362 -0.05035555 0.9954541 0.06054955 -0.105565 0.9925673 0.289595 -0.1687701 0.9421526 0.190837 -0.6347397 0.7487902 0.6045618 -0.6496696 0.4609062 0.2069807 -0.5937191 0.7775968 0.3816404 -0.2277024 0.8958249 0.6072722 -0.08273768 0.7901741 0.2558109 -0.6359565 0.7280936 0.4013936 -0.1035529 0.9100331 0.8699527 0.3734342 0.3220702 0 0 1 0.6172146 0.6730337 0.4075191 0.2195214 -0.2626749 0.939581 0.1850055 -0.1162159 0.9758416 0.4429582 -0.7498611 0.491423 0.7105507 0.008697986 0.7035923 0.161201 -0.5773574 0.8004203 0.4859198 0.8251044 -0.2882443 -0.06198388 0.8444885 -0.5319749 -0.02664321 0.7149424 -0.6986757 0 0.8469319 -0.5317015 -0.07983756 0.7112748 -0.6983653 -0.1236327 0.837199 -0.5327408 0.7794904 0.05029559 -0.6243919 -0.132698 0.7041115 -0.6975803 0.7815777 0.06479316 -0.6204339 -0.1844259 0.8251711 -0.5339288 -0.1849794 0.6931008 -0.6967021 0.7680783 0.1020259 -0.6321761 0.7762284 0.1281811 -0.6172838 0.7537297 0.1539992 -0.6388863 -0.243967 0.8080759 -0.5361842 -0.2364289 0.6786155 -0.6954009 0.7705412 0.1902548 -0.6083332 -0.3021392 0.787088 -0.5377774 0.7361364 0.2067407 -0.6444856 -0.286758 0.6598851 -0.6944938 0.7579091 0.2493098 -0.602842 -0.3593976 0.7634148 -0.5366854 -0.3382127 0.6421221 -0.6879618 0.714389 0.2577635 -0.6505432 -0.4121049 0.7310643 -0.5437965 0.741623 0.3073615 -0.5962587 -0.3842024 0.6148947 -0.6886894 0.6880807 0.3066546 -0.6576533 -0.463896 0.6979194 -0.5456272 0.7231038 0.3654737 -0.5861313 -0.4296868 0.5856119 -0.6873341 0.6601549 0.354752 -0.6620776 -0.5111992 0.6584551 -0.5523698 0.6969364 0.4196695 -0.5815128 -0.4745681 0.5552294 -0.6830119 0.6275032 0.3992509 -0.6684598 -0.5568879 0.6178352 -0.5551178 0.6698264 0.4743841 -0.5712202 -0.5159658 0.5198113 -0.6808637 0.5933845 0.4426502 -0.6722766 -0.5970272 0.5715126 -0.5629671 0.6371177 0.5259059 -0.563475 -0.5566449 0.4835507 -0.6755183 0.5565848 0.4834905 -0.675611 -0.6373271 0.5261465 -0.5630134 -0.5933567 0.4426828 -0.6722796 0.5970272 0.5715126 -0.5629671 -0.6707949 0.4751935 -0.5694081 0.5158339 0.5196793 -0.6810644 -0.6277827 0.3996201 -0.6679766 0.5572479 0.6182556 -0.5542876 -0.699066 0.4211913 -0.5778449 0.4740907 0.5547539 -0.6837295 -0.6592792 0.3540245 -0.6633383 0.5129664 0.6609538 -0.5477277 -0.7212671 0.3642501 -0.5891483 0.4304125 0.586915 -0.6857668 -0.6889154 0.3071782 -0.6565343 0.4630677 0.6966311 -0.5479722 -0.742022 0.3076371 -0.5956197 0.3839947 0.6142939 -0.6893411 -0.7140219 0.2575496 -0.6510307 -0.7572433 0.2488843 -0.6038537 0.4120041 0.7309258 -0.5440591 -0.7360934 0.2067372 -0.6445357 0.3361734 0.6391414 -0.6917267 -0.768687 0.1891274 -0.6110248 0.3582682 0.7611864 -0.5405916 -0.7543131 0.1546714 -0.6380349 0.2869103 0.6605254 -0.6938218 -0.7759124 0.1277826 -0.6177635 0.3025376 0.7881605 -0.5359796 -0.7683519 0.1022394 -0.6318089 0.2364635 0.6788102 -0.695199 -0.7796828 0.06509816 -0.6227818 0.243971 0.8080891 -0.5361624 -0.7772319 0.04992938 -0.6272302 0.1849794 0.6931008 -0.6967021 0.1844289 0.8251845 -0.533907 0.1326981 0.7040507 -0.6976417 0.1236948 0.8376333 -0.5320433 0.07980734 0.711216 -0.6984286 0.06201457 0.8448883 -0.531336 0.02664321 0.7149122 -0.6987065 0.8263272 0.00519222 0.5631664 0.0664094 -0.9069342 0.416005 -2.13636e-4 2.13636e-4 1 2.1363e-4 -2.1363e-4 1 -0.004486262 0.004486262 0.99998 -0.8990194 -0.004852414 0.4378821 0.004486262 -0.004486262 0.99998 -0.05844461 0.8112358 0.5817911 -0.01785367 0.8629886 0.504908 -0.1620559 0.7634942 0.6251515 -0.8617449 -0.4364575 0.2586516 -0.8460268 -0.4782992 0.2355177 -0.9198479 -0.3918662 0.01791471 -0.8910724 -0.4535475 0.01687717 -0.8475177 -0.5150418 0.1282416 -0.7520601 -0.5862479 0.3011963 -0.7672697 -0.6293253 0.1234785 -0.7134186 -0.683052 0.1564418 -0.6655027 -0.7283723 0.1630338 -0.6101035 -0.7711824 0.1818011 -0.6516423 -0.7582757 0.01950162 -0.06637823 0.997658 0.0165106 -0.6009005 -0.7990031 0.02264553 -0.5841974 -0.810863 0.03485286 -0.1354442 0.9637356 0.2299317 -0.20063 0.948827 0.243875 -0.258585 0.9657948 0.01934885 -0.320911 0.9468628 0.02160769 -0.3835014 0.9233515 0.01867759 -0.5129966 -0.8452586 0.1495743 -0.4433485 0.8961579 0.01852494 -0.4151177 -0.8833993 0.2174469 -0.5008431 0.8652064 0.02395719 -0.5603373 0.7564858 0.3372705 -0.6340082 0.7503784 0.1869918 -0.3729157 -0.9050812 0.2043574 -0.6689752 0.7428005 0.02682614 -0.7397566 0.6415761 0.202831 -0.7821776 0.5822461 0.2217833 -0.8230236 0.5279291 0.2095785 -0.3347041 -0.9417616 0.03253346 -0.8527396 0.4439642 0.2751925 -0.8811945 0.4721088 0.02469038 -0.9120723 0.3905873 0.1247633 -0.2429033 -0.9451531 0.2183657 -0.9376472 0.3458152 0.03506678 -0.9610813 0.2687219 0.06412088 -0.9652277 0.1952612 0.1738063 -0.1945885 -0.980542 0.02594107 -0.2178729 -0.9700976 0.106968 -0.9803479 0.1684372 0.1026982 -0.9743799 0.1910794 0.1186273 -0.9787223 0.1184452 0.1675508 -0.9726707 0.207407 0.1043749 -0.1751183 -0.9729167 0.1508861 -0.9805328 0.0421462 0.191779 -0.1303783 -0.9909908 0.03064131 -0.998297 0.00967437 0.05752772 -0.9674087 -0.103858 0.2309414 -0.9949558 -0.09842431 0.01937967 -0.9918963 -0.01800614 -0.1257686 -0.06113046 -0.9978342 0.02429348 -0.0641207 -0.9977015 0.02191275 -0.9681566 -0.2062784 0.1418526 -0.987688 -0.0835306 -0.1322695 -0.9709593 -0.2383221 -0.02099698 -0.9080228 -0.3660718 0.2036817 -0.9973341 0.07297116 0 -0.9893469 0.1455774 0 0.03650027 0.9993336 0 -0.03650027 0.9993336 0 0.1093797 0.9940001 0 0.9973341 0.07297116 0 -0.1093797 0.9940001 0 -0.1816214 0.9833685 0 -0.2529126 0.9674892 0 0.9893469 0.1455774 0 0.9760778 0.2174216 0 -0.3228579 0.9464475 0 0.9576001 0.288101 0 0.9340204 0.3572197 0 -0.391096 0.9203498 0 -0.4572446 0.8893411 0 0.9054458 0.4244621 0 0.872056 0.4894061 0 0.8340031 0.5517598 0 -0.5209352 0.8535963 0 0.7914904 0.6111817 0 -0.5818588 0.8132899 0 0.7447886 0.6673006 0 -0.639674 0.7686464 0 0.6940791 0.7198988 0 -0.6940791 0.7198988 0 0.6940639 0.7199135 0 -0.7447886 0.6673006 0 0.639674 0.7686464 0 -0.7914904 0.6111817 0 0.5818588 0.8132899 0 -0.8340031 0.5517598 0 0.5209352 0.8535963 0 0.4572446 0.8893411 0 -0.872056 0.4894061 0 -0.9054458 0.4244621 0 0.3228579 0.9464475 0 -0.9340204 0.3572197 0 -0.9576001 0.288101 0 0.2529126 0.9674892 0 0.1816214 0.9833685 0 -0.9760778 0.2174216 0 0.6960181 0.5325986 -0.4815576 0.1794239 0.7585612 0.626412 -0.6819527 0.6984987 -0.216887 -0.731561 -0.4833838 0.4807897 -0.7465302 -0.505276 0.4328844 -0.6868355 0.5469353 0.4786637 -0.7458428 0.5104435 0.4279791 -0.166451 -0.3516407 0.9212182 -0.392936 -0.4267819 0.8145297 -0.1434713 -0.393333 0.9081328 -0.311814 -0.01651084 0.9499998 -0.3130072 -0.01434409 0.9496425 -0.4108544 0.05148649 0.910246 -0.1456988 -0.2246215 0.9634922 -0.127691 -0.3832561 0.9147731 -0.7901455 0.458736 0.4064869 -0.5731267 -0.5574397 0.6006554 -0.2228162 0.47061 -0.8537443 -0.255626 -0.1424925 0.9562171 -0.7764766 0.329885 0.5368987 -0.2167747 -0.4638544 0.8589807 -0.08215677 -0.2956362 0.9517614 -0.6753611 -0.6669072 0.3148369 -0.6927318 0.1275715 0.7098227 -0.3773119 -0.5533785 0.7425686 -0.6776658 0.1308943 0.7236269 -0.1322385 -0.4235724 0.8961581 -0.0702545 -0.2998483 0.9513966 0.1113337 0.4114281 0.904617 0.09821259 0.5212776 0.847717 -0.5175434 -0.6751748 0.525631 0.1617204 0.686648 0.7087743 -0.5706198 8.54541e-4 0.821214 0.1255863 0.5517866 0.8244754 0.02050876 -0.1735921 0.9846041 0.1035814 0.06143468 0.9927219 0.004699885 -0.2968586 0.9549099 -0.009613394 -0.3242313 0.9459291 -0.9135184 0.3258184 0.2435703 0.07178056 0.8594135 0.5062177 -0.5138545 -0.7019147 0.4932234 0.03680616 0.01919656 0.9991381 0.01644968 -0.1526558 0.9881426 -0.2218753 -0.532989 0.8165133 -0.6075443 0.02291983 0.7939551 -0.02481228 0.8480732 0.529298 -0.5195277 -0.7712193 0.3678475 -0.005920708 -0.1266239 0.9919332 -0.001556396 -0.1415151 0.9899349 -0.8298763 0.08896327 0.5508093 -0.05588001 0.8613647 0.5049042 -0.2158951 -0.5293306 0.8204868 -0.8461272 0.09464144 0.524511 -0.04699969 0.02060049 0.9986825 -0.02060014 -0.1099592 0.9937227 -0.08893191 0.8444565 0.5281897 -0.4367271 -0.1575391 0.8856923 -0.2116813 -0.2655478 0.940572 -0.1668161 0.8521171 0.4960535 -0.555717 -0.5433647 -0.6292326 -0.5350998 -0.8125228 0.2312468 -0.5527384 -0.8153585 0.1722519 -0.5381422 -0.8132101 0.2215682 -0.5527954 -0.8153525 0.1720981 3.05195e-4 -0.32723 0.9449447 -0.2369221 -0.5641207 0.7909714 2.74676e-4 -0.3271389 0.9449763 -0.9151728 0.06116002 0.3983948 -0.04138416 -0.09805858 0.9943198 -0.434809 -0.8358637 0.3350718 -0.4696927 -0.1840927 0.8634227 -0.1975789 -0.2826044 0.9386679 -0.2278584 0.8180746 0.528048 -0.06839233 0.05352973 0.9962214 -0.07907569 -0.005340874 0.9968543 -0.2672287 0.8859506 0.3790522 -0.2489781 -0.6441752 0.7232207 -0.3004262 0.808044 0.5067632 -0.6734564 -0.1631225 0.7210046 -0.9273588 0.002319455 0.3741663 -0.1500938 0.1407549 0.978601 -0.1254326 0.05545276 0.9905512 -0.1598281 -0.3200529 0.9338209 -0.3374555 0.876432 0.3434983 -0.279797 -0.7924225 0.5420151 -0.05264526 -0.4496062 0.8916742 -0.9077099 -0.07782459 0.4123179 -0.5886011 -0.202406 0.7826753 -0.1100825 -0.03985798 0.9931231 -0.360889 0.780863 0.5099139 -0.4051803 0.8350206 0.3722495 -0.1857677 0.1282398 0.9741894 -0.3537901 -0.9239628 -0.145346 -0.8886933 -0.1814376 0.4210756 -0.02960371 -0.4034047 0.9145427 -0.3924187 -0.3005858 0.8692846 -0.2216945 -0.8801552 0.4197363 -0.4236328 0.7565328 0.4981904 -0.4625471 0.8010341 0.379993 -0.02551358 -0.3992758 0.9164758 -0.2100006 0.1532354 0.9656183 -0.1226871 -0.1024529 0.987143 -0.1442638 -0.3315901 0.9323283 -0.9215272 -0.1941021 0.3363215 -0.4792751 0.7144263 0.5097944 -0.5251829 0.7590553 0.3847312 -0.1209183 -0.1177138 0.9856584 0.01605796 -0.1323835 -0.9910686 -0.4804617 0.5196482 0.706486 -0.9336208 -0.2279193 0.2764148 -0.1619039 -0.9483819 0.2726884 -0.2935981 -0.3296111 0.8973053 -0.2105528 0.04056018 0.9767407 -0.2196797 0.05371439 0.9740923 -0.02728402 -0.4635241 0.8856642 -0.02859658 -0.5099478 0.85973 -0.4903485 0.3499917 0.7981631 -0.003723263 -0.3210557 0.9470531 -0.1341921 -0.1291869 0.9824985 -0.05282807 -0.2591354 0.9643952 -0.8434267 -0.3184366 0.4327002 -0.9693694 -0.1989521 0.1440183 -0.6841728 -0.3254536 0.6526773 -0.6961134 -0.3220696 0.6416366 -0.4722554 -0.3667807 0.8015279 -0.300068 0.06946241 0.9513855 -0.5549885 0.5002373 0.6646432 -0.9688623 -0.234479 -0.07953298 -0.4661208 -0.3675436 0.8047627 2.4415e-4 -0.3277404 0.9447678 -0.6425642 0.5953195 0.4823962 -0.6787735 0.5963111 0.4285786 -0.5796291 0.479433 0.658919 -0.8650177 -0.3860732 0.320456 -0.7910324 -0.4239755 0.4410359 -0.5805734 0.4812628 0.6567502 -0.349816 -0.3641297 0.8631561 -0.004791438 -0.3286577 0.944437 -0.2995194 -0.005707144 0.9540732 -0.3579612 -0.337849 0.8704723 0.8808307 0.2405872 0.4077439 0.8885492 0.3489311 0.2978718 0.593395 -0.7163285 0.3670912 0.4566657 -0.6902022 0.5613176 0.6283349 -0.7060985 0.3265278 0.5802925 -0.06225919 0.812025 0.2046937 -0.2357625 0.9500087 0.3937851 -0.1319941 0.9096763 0.8771576 0.3998344 0.2659459 0.6458571 -0.6532123 0.3951991 0.3044949 -0.4947241 0.81396 0.3247564 -0.4945965 0.8061686 0.1543671 -0.4142094 0.8969958 0.2865443 -0.1506119 0.9461545 0.3079119 -0.1539711 0.9388734 0.8151001 0.399493 0.419544 0.7030349 -0.6068696 0.3707444 0.4070631 -0.5353653 0.7400566 0.2450976 -0.1730424 0.9539306 0.1370907 -0.3984237 0.9068984 0.2176605 -0.1598883 0.9628395 0.740856 -0.5299066 0.4127125 0.6992456 0.2769576 0.6590524 0.1535407 -0.4124938 0.8979278 0.1408449 0.2014861 0.9693123 0.06537258 -0.2679606 0.9612094 -0.1294904 -0.9908477 0.0381177 0.7386506 0.5690566 0.3613446 0.6405073 0.2965555 0.7083821 -0.05536174 -0.7997244 0.5978096 0.3661435 0.01416105 0.9304507 0.003234982 -0.3248742 0.9457517 0.4101198 -0.4681677 0.7827008 0.702486 0.527185 0.4781104 0.8295436 -0.4587958 0.3183767 0.1811597 -0.385758 0.9046391 0.7003251 0.6256445 0.3436772 -0.04898393 -0.7473333 0.6626414 -0.01104795 -0.4971896 0.8675716 0.493983 0.2450993 0.8342105 0.3654707 -0.4389005 0.8208518 0.643332 0.6660379 0.377515 0.6500783 0.5634052 0.5098755 0.9154512 -0.3494438 0.1995949 0.9140333 -0.3575683 0.1915413 0.4289186 0.1544888 0.890035 0.3015317 0.03235054 0.9529073 0.6017736 0.6143475 0.5103387 0.4118628 0.2901199 0.8638284 0.0644561 -0.9134639 0.4017825 0.4795454 -0.4230547 0.7688048 0.5958003 0.6818956 0.4243116 0.1289148 -0.9354565 0.3290929 0.2970984 -0.3740357 0.8785384 0.7212933 -0.3716031 0.5845058 0.727949 -0.3712076 0.5764507 0.04547315 -0.6470618 0.7610804 -0.002105772 -0.3808104 0.9246508 0.4176903 0.3051953 0.8557983 0.004242122 -0.3224349 0.9465822 0.06274777 -0.2216315 0.9731096 0.1434695 -0.1501837 0.9781929 0.5447711 0.6408767 0.5408341 0.5180988 0.5952216 0.6142352 0.888885 -0.07007229 -0.4527398 0.02044796 -0.4035574 0.914726 0.2894121 -0.3716915 0.8820919 0.25325 0.1425256 0.9568443 0.1015375 -0.1461872 0.9840322 0.4311529 0.7436144 0.5110232 0.5786088 -0.3607035 0.7315087 0.7397615 -0.348685 0.5754752 0.3899769 -0.3535673 0.8502401 0.9071493 -0.2106732 0.3642758 0.9485329 -0.06361943 -0.3102223 0.1156981 -0.679143 0.7248302 0.5616424 -0.3057705 0.7688058 0.3911997 -0.3546983 0.8492067 3.05194e-4 -0.3288165 0.9443938 0.2371025 0.1510388 0.9596717 0.1119747 -0.1055657 0.9880879 0.4166757 0.7817132 0.4640106 0.1902281 -0.8249119 0.5322909 0.3670879 0.7509308 0.5489532 0.1229017 -0.0744369 0.9896234 0.1664181 0.09134227 0.9818155 0.3465157 0.8112936 0.4708818 0.6523798 -0.2807162 0.7039879 0.4172576 -0.2917934 0.86067 0.183418 -0.6838046 0.706236 0.916855 -0.1407541 0.3735844 0.30611 0.7895074 0.5319538 0.1153938 -0.03497517 0.9927039 0.2721073 0.8292924 0.4880901 0.5371734 -0.2485506 0.8060195 0.3583769 -0.7217044 -0.5922068 0.3467934 -0.8543027 0.3871707 0.8992838 -0.09439641 0.4270576 0.04565632 -0.1297666 0.9904929 0.1843662 -0.6696512 0.7194279 0.2921044 -0.2845049 0.9130893 0.4224553 -0.7170366 -0.5544277 0.8955596 -3.05193e-4 0.4449415 0.5159307 -0.1986814 0.8332715 0.3739251 -0.2323451 0.8978841 0.3610444 -0.7400343 0.5674471 0.1149064 -0.4973783 0.8598904 0.0564289 -0.4127642 0.9090883 0.1380096 -0.5070524 0.8507946 0.8882298 0.08514845 0.4514394 0.5627717 -0.1237243 0.8173007 0.9108855 0.1830561 0.3698355 0.5722349 -0.08603358 0.8155645 0.5704658 -0.1027888 0.8148639 0.00149542 -0.3273209 0.9449121 0.04538202 -0.3182541 0.9469187 0.06656175 -0.3022289 0.9509087 3.99106e-5 0 1 -1.54488e-5 0 1 -1.02507e-5 0 1 1.51433e-5 0 1 -2.3363e-5 0 1 1.03274e-5 0 1 -3.92829e-6 0 1 1.03949e-5 0 1 -1.49914e-5 0 1 7.9335e-6 0 1 -5.22619e-6 0 1 1.60332e-5 0 1 -3.8101e-5 0 1 5.2532e-6 0 1 2.02677e-5 0 1 -3.76074e-5 0 1 -7.42501e-6 0 1 -8.20738e-6 0 1 3.70717e-5 0 1 7.93869e-6 0 1 2.07773e-6 0 1 6.6306e-6 0 1 -1.0535e-5 0 1 1.32747e-6 0 1 2.03138e-5 0 1 3.42127e-5 0 1 1.46235e-5 0 1 9.24994e-7 0 1 -3.37113e-5 0 1 -5.30448e-6 0 1 2.0556e-5 0 1 9.26489e-7 0 1 -1.05849e-5 0 1 1.62195e-5 0 1 5.56876e-6 0 1 -3.28285e-5 0 1 -9.4014e-6 0 1 3.24283e-5 0 1 1.42875e-5 0 1 3.73329e-6 0 1 1.12435e-5 0 1 -2.07896e-5 0 1 -9.76245e-6 0 1 7.53131e-6 0 1 4.93622e-6 0 1 -3.11507e-5 0 1 -7.57163e-6 0 1 2.05015e-5 0 1 2.99327e-5 0 1 7.61649e-6 0 1 -4.06653e-5 0 1 4.03023e-5 0 1 1.53341e-5 0 1 1.54488e-5 0 1 -3.99106e-5 0 1 1.02499e-5 0 1 2.3363e-5 0 1 -1.03259e-5 0 1 3.92829e-6 0 1 -1.49927e-5 0 1 -7.9335e-6 0 1 3.85869e-5 0 1 -5.22728e-6 0 1 1.49341e-5 0 1 -1.60332e-5 0 1 3.81009e-5 0 1 -2.02677e-5 0 1 3.76074e-5 0 1 8.20738e-6 0 1 -3.70718e-5 0 1 7.93802e-6 0 1 -2.07773e-6 0 1 6.63116e-6 0 1 1.0535e-5 0 1 -2.03138e-5 0 1 -3.42127e-5 0 1 9.25076e-7 0 1 3.37113e-5 0 1 -1.46235e-5 0 1 -5.30493e-6 0 1 9.26434e-7 0 1 -2.0556e-5 0 1 -1.0584e-5 0 1 3.28284e-5 0 1 -1.62195e-5 0 1 -3.24283e-5 0 1 9.4014e-6 0 1 -1.12005e-5 0 1 -1.42875e-5 0 1 1.12445e-5 0 1 9.76245e-6 0 1 -7.5706e-6 0 1 3.11507e-5 0 1 -4.93622e-6 0 1 -2.99327e-5 0 1 -4.03023e-5 0 1 0.6153707 0.3362671 0.712912 0.7099421 0.2624973 0.6535117 0.5412554 0.4513157 0.7094764 0.1356868 0.7300416 0.6697973 -0.5198939 0.5267913 0.6724592 0 1 0 0 1 -3.75173e-6 0 1 -4.82863e-6 0 1 3.92387e-6 0 1 -4.30024e-6 0 1 4.08623e-6 0 1 -3.8718e-6 0 1 -3.04158e-6 0 1 1.50294e-6 0 1 -2.63644e-6 0 1 2.55779e-6 0 1 -2.70597e-6 0 1 -2.37042e-6 0 1 2.58683e-6 0 1 -5.63794e-7 0 1 1.38424e-6 0 1 -1.46946e-6 0 1 -1.86737e-6 0 1 1.92889e-6 0 1 -3.22825e-6 0 1 -2.00801e-6 0 1 3.84275e-6 0 1 2.81565e-6 0 1 -2.70983e-6 0 1 2.50198e-6 0 1 -2.44192e-6 0 1 2.6112e-6 0 1 2.31211e-6 0 1 -2.53615e-6 0.1799112 -0.156686 0.9711239 8.85045e-4 -0.3266733 0.945137 6.10392e-4 -0.3269564 0.9450392 8.75978e-4 -0.3266984 0.9451283 0.001434385 -0.3257336 0.9454605 0.1866268 -0.5741026 0.7972307 0.1717615 -0.5973197 0.7833946 -7.93499e-4 -0.3272572 0.9449351 -2.13634e-4 -0.3274397 0.9448721 0.1800017 -0.223522 0.9579339 0.002655148 -0.3223727 0.9466092 -9.15578e-5 -0.327075 0.9449985 0.4201896 -0.1197882 0.8994953 0.1229918 -0.5865153 0.8005455 8.85065e-4 -0.3272299 0.9449444 1.22076e-4 -0.3275309 0.9448406 0.06527966 -0.5985347 0.7984328 2.44155e-4 -0.3275948 0.9448183 -9.15565e-5 -0.3274669 0.9448627 -9.15582e-5 -0.3275036 0.9448499 0.04757881 -0.04785346 0.9977207 6.87564e-5 -0.3276013 0.9448161 0.001081228 -0.3273113 0.944916 -0.3635198 -0.2833446 0.8874509 0.001089215 -0.3273218 0.9449123 4.61811e-4 -0.3274974 0.944852 -0.3455383 -0.3175522 0.8830425 0.00115621 -0.3275281 0.9448409 -0.03009152 -0.61471 0.788179 0.001137733 -0.3275178 0.9448444 -0.4208309 -0.3530474 0.8356189 9.66885e-4 -0.3275435 0.9448356 0.001927971 -0.3276939 0.944782 -0.07892185 -0.6221583 0.7789034 -0.3231062 -0.3911638 0.8617385 0.001862108 -0.327636 0.9448022 -8.01849e-5 -0.3275952 0.9448182 -0.001151025 -0.3272755 0.9449284 -0.1035212 -0.5663453 0.8176407 -0.2930147 -0.4477774 0.844771 -0.001159727 -0.3272298 0.9449441 -0.001159727 -0.3271659 0.9449663 0.3056809 -0.4151535 0.8568587 -0.146948 -0.535666 0.8315458 -0.3100813 -0.4807174 0.8202198 -0.2499848 0.1257401 0.9600505 -0.2780625 -0.4957576 0.8227428 -0.2393598 -0.5121074 0.8248957 -1.52594e-4 -0.3273757 0.9448943 -3.35708e-4 -0.3272845 0.9449259 0.001068115 -0.3285335 0.9444918 4.88307e-4 -0.3279594 0.9446917 0.001069724 -0.328523 0.9444954 0.2778485 -0.5239874 0.805132 0.001026928 -0.328527 0.9444941 -7.01938e-4 -0.3268285 0.9450835 3.05191e-5 -0.3274397 0.9448721 -0.001159727 -0.3267099 0.945124 0.2479372 -0.5704326 0.7830286 -0.001037597 -0.3268284 0.9450832 -0.07471024 0.9972053 0 0.07471024 0.9972053 0 0.9972053 0.07471024 0 0.9888339 0.1490224 0 -0.1490224 0.9888339 0 -0.222514 0.9749296 0 0.9749296 0.222514 0 -0.2947506 0.9555743 0 0.9555743 0.2947506 0 0.9308713 0.3653473 0 -0.3653473 0.9308713 0 -0.4338826 0.9009695 0 -0.4999893 0.8660317 0 0.9009695 0.4338826 0 -0.5633188 0.8262397 0 0.866024 0.5000025 0 0.8262397 0.5633188 0 -0.6234773 0.7818415 0 -0.6801831 0.7330424 0 0.7818266 0.6234959 0 0.7818415 0.6234773 0 0.7330424 0.6801831 0 -0.7330424 0.6801831 0 0.6801831 0.7330424 0 -0.7818415 0.6234773 0 -0.7818266 0.6234959 0 0.6234773 0.7818415 0 -0.8262397 0.5633188 0 -0.866024 0.5000025 0 0.5633188 0.8262397 0 0.4999893 0.8660317 0 -0.8660317 0.4999893 0 -0.9009695 0.4338826 0 0.4338826 0.9009695 0 -0.9308713 0.3653473 0 0.3653473 0.9308713 0 0.2947506 0.9555743 0 -0.9555743 0.2947506 0 0.222514 0.9749296 0 -0.9749296 0.222514 0 -0.9888339 0.1490224 0 -0.9972053 0.07471024 0 0.1490224 0.9888339 0 0.9321866 0.331655 -0.1450285 0.9232289 0.349503 -0.1596751 0.9308035 0.3064122 -0.19929 0.9036415 0.4211032 -0.07812899 -0.9514845 0.3042724 -0.04577815 -0.9693785 0.2433288 -0.03311318 -0.9578085 0.2635316 -0.1146904 -0.4116429 0.5577079 -0.7207719 -0.3817375 0.2148571 -0.898951 -0.3257936 0.197643 -0.9245517 -0.007202446 0.1091356 -0.9940008 -0.05411088 0.00137335 -0.998534 0.03302186 0.003936946 -0.9994469 0.9894797 0.005493342 -0.1445674 0.9840575 0.005249261 -0.1777737 0.9838005 2.13631e-4 -0.1792668 0.9889342 -9.15567e-4 -0.1483523 0.9425108 0.2795213 -0.183143 0.980099 0.06125235 -0.1888233 0.9745175 0.1102364 -0.1953552 0.9691439 0.1017822 -0.2245008 0.984997 0.02414041 -0.1708752 0.9803107 0.02661281 -0.1956593 0.9774927 0.005920648 -0.2108862 0.9513803 0.2569426 -0.1698708 0.9439028 0.2454062 -0.2209601 0.9262885 0.3724869 -0.05694884 -0.9315099 0.2777866 -0.2347849 -0.9373437 0.2838003 -0.2020998 -0.9394193 0.2203504 -0.2625588 -0.9301651 0.2458323 -0.2726893 0.9744109 0.1396549 -0.1761252 0.9665673 0.1423101 -0.2132973 0.9791296 0.09882241 -0.1775935 0.9913954 0.02551347 -0.1283915 -0.8542633 0.1388929 -0.5009423 -0.8890523 0.1719751 -0.4242767 -0.8695327 0.1132581 -0.4807136 -0.8329203 0.06430315 -0.5496444 0.966602 0.1722805 -0.1897374 -0.8984245 0.2479686 -0.3624156 -0.9124426 0.1978276 -0.3582078 0.9611677 0.1691367 -0.2180588 0.9604488 0.2296897 -0.1574196 0.9573137 0.2072837 -0.2014546 0.984804 0.07202607 -0.1580301 -0.9184146 0.2551406 -0.3023538 -0.9281729 0.2032867 -0.3117206 0.9935976 0.003601193 -0.1129192 0.993569 9.15563e-4 -0.1132246 0.9960321 3.66233e-4 -0.08899462 0.9872644 0.04977673 -0.1511003 -0.9462608 0.2801701 -0.1615403 -0.9609652 0.2317035 -0.1511932 0.9695917 0.1835724 -0.1618428 0.9508174 0.2893805 -0.1104785 0.9962276 0.002807736 -0.08673459 0.9979107 -1.22076e-4 -0.06460875 0.985489 0.09344917 -0.1416997 0.9604679 0.2492803 -0.1239382 -0.9356944 0.3310745 -0.121925 0.9778745 0.1546727 -0.1408473 -0.4713947 0.6739485 -0.5688415 -0.4027268 0.8879767 -0.2220552 -0.472076 0.852685 -0.2237691 0.9806854 0.1217082 -0.1531119 -0.5367485 0.6348996 -0.5557011 -0.4331331 0.4421668 -0.7854199 0.9943144 0.01348942 -0.1056268 0.9680361 0.2106429 -0.1361458 0.9927411 0.04718178 -0.11063 0.9979616 0.002075254 -0.06378406 -0.9314214 0.3257945 -0.1622105 0.9890015 0.07141464 -0.129523 -0.8784331 0.2248651 -0.4216528 0.9968174 0.009369373 -0.07916671 -0.2193446 0.212142 -0.9523044 -0.2621878 0.1430421 -0.9543566 -0.1929716 0.01989835 -0.9810026 0.9465034 0.3197126 -0.04376363 0.9990805 0.001434385 -0.04284912 -0.8742311 0.1920287 -0.4459204 0.9742823 0.1877211 -0.1246387 0.9990887 0.002044737 -0.04263448 -0.3215178 0.4750284 -0.8191303 0.9859971 0.1125528 -0.1230512 0.9963361 0.02606356 -0.08145624 -0.1301627 0.1225025 -0.9838957 -0.9077273 0.3453227 -0.2382925 0.999768 0.001953184 -0.02145475 0.9996466 0.003845334 -0.02630728 0.9998825 0.001190185 -0.01528984 0.525935 -0.02237045 -0.8502305 0.4682226 -0.03097677 -0.8830675 0.4711587 -0.02862721 -0.8815838 -0.7950562 0.06427347 -0.6031208 -0.815057 0.008148729 -0.5793235 -0.7913402 0.02310323 -0.6109395 0.9844358 0.1307454 -0.1174389 0.9670112 0.2398529 -0.08579033 0.9986393 0.01333671 -0.05041718 0.9933019 0.06424248 -0.09604328 0.9765854 0.1922407 -0.09656286 0.9999169 -0.001037597 -0.01284837 0.9999981 -0.001861631 6.10388e-4 0.9894712 0.09265702 -0.1111823 0.4989868 -0.02261459 -0.8663145 0.9799276 0.1655949 -0.1109968 0.9959237 0.04367256 -0.07892191 0.5567331 -0.02621603 -0.8302776 -0.9160894 0.3633289 -0.1696247 0.9852259 0.1432578 -0.09384697 -0.3482533 0.89485 -0.279219 0.9981247 0.02743655 -0.05472058 -0.8999763 0.312393 -0.3040613 0.9649 0.2606964 -0.03170961 -0.2423801 0.5618202 -0.7909551 -0.7736338 0.0518828 -0.6315054 0.9936149 0.08032661 -0.07922792 0.9995771 0.01297038 -0.02603238 0.98957 0.1115154 -0.09118998 -0.9307706 0.362414 -0.04818958 -0.9898583 0.003875911 -0.1420056 -0.9947896 0.02554416 -0.09869766 -0.9980433 0.01007133 -0.06171005 -0.9924593 -0.00149542 -0.1225658 0.9820664 0.1722179 -0.07672441 -0.9816528 0.04553472 -0.1851605 -0.9750638 0.009460985 -0.2217233 -0.9634144 0.09195286 -0.2517488 -0.9572791 0.007324457 -0.2890728 0.999993 -0.002044737 0.003143429 0.9967049 0.05136424 -0.06277853 -0.8584734 0.2393007 -0.4536062 -0.9998962 -5.18824e-4 -0.01440495 -0.9999995 0 0.001068174 -0.9999845 0.004669368 -0.003051877 -0.9997305 -0.001617431 -0.02316361 -0.3339716 0.7341393 -0.5911874 -0.8184379 0.1554656 -0.5531635 -0.9472138 0.004577815 -0.3205701 -0.9380649 0.005127191 -0.346422 -0.9993634 -3.66224e-4 -0.03567624 -0.8788936 0.262252 -0.3984594 -0.9077428 0.4144554 -0.06503713 -0.7537358 0.01126158 -0.6570811 -0.7391151 -0.01989853 -0.6732851 -0.7274637 0.043002 -0.6847975 -0.9986164 -4.57787e-4 -0.0525844 0.9990359 0.02792477 -0.03387594 -0.9975247 -2.13635e-4 -0.07031655 0.988298 0.1332766 -0.07419174 -0.9956009 -0.002411007 -0.09366375 -0.923165 0.04702955 -0.3815163 -0.9093084 0.007049798 -0.4160631 0.9789544 0.2020961 -0.02838253 0.9931346 0.09717106 -0.06512653 -0.9877161 -7.32455e-4 -0.1562571 -0.8930209 0.06259477 -0.4456407 -0.8776084 0.003662288 -0.4793642 0.9959532 0.06793498 -0.05884039 0.5644113 0.07922631 -0.8216832 0.5927507 -0.01434415 -0.8052583 -0.9816443 -0.001739561 -0.1907134 0.990247 0.1312007 -0.04687702 -0.8610365 0.05310326 -0.5057632 -0.8437659 9.15578e-5 -0.5367116 -0.9737205 -0.002227902 -0.2277355 0.9983819 0.04526031 -0.03442591 -0.7165319 0.002807736 -0.6975489 0.9998802 0.00967437 -0.01208531 -0.1572344 0.4472575 -0.8804761 -0.2284999 -0.03180134 -0.9730244 -0.2830058 -0.02920699 -0.9586734 -0.2274615 0.07629865 -0.9707935 -0.8243031 0.07809954 -0.5607361 -0.804799 -0.002288937 -0.5935431 0.9996147 -0.01467955 0.02356052 0.9994099 -0.002716183 0.03424245 -0.9659882 -0.003875911 -0.2585572 -0.7508401 0.06692916 -0.6570842 0.9966827 0.07406908 -0.03372323 0.5943276 -0.03015279 -0.8036576 -0.9558863 -0.003601193 -0.2937151 -0.8983023 0.3601877 -0.2516309 -0.2493708 0.7961495 -0.5513259 -0.8057692 -0.01220768 -0.5921039 0.999978 -0.004913508 0.004486203 -0.8613787 0.3074522 -0.4043511 0.9897612 0.1416386 -0.01763993 -0.2693912 0.9377269 -0.2193095 -0.8341903 0.2514504 -0.4908151 -0.8756756 0.342726 -0.3401929 0.09143549 -3.05192e-4 -0.995811 0.06998008 -0.01141411 -0.9974831 -0.9999632 0.002411007 -0.008240163 -0.9998745 -0.008697986 0.01324534 -0.9999966 -0.002441525 -0.00100708 -0.04913556 0.152534 -0.9870761 0.9996309 -0.01626676 0.02176028 0.9996785 0.02099698 -0.01422178 -0.7751947 2.44156e-4 -0.6317226 -0.9992846 0.01034593 -0.03637868 -0.9998731 0.002655148 -0.01571738 -0.9989222 0.001617491 -0.0463891 -0.9455116 -0.005188226 -0.325547 -0.7702711 -0.06729453 -0.6341561 -0.7707434 -0.01178056 -0.6370368 -0.8801342 0.4678851 -0.0802952 0.6210709 0.07196485 -0.7804436 0.6519217 -0.01388621 -0.7581592 -0.934247 -0.002990841 -0.3566142 -0.07526081 0.4767434 -0.8758149 -0.9969011 0.01016277 -0.07800638 0.9944218 0.09878909 -0.03695815 -0.1953194 -0.05926722 -0.9789472 -0.7779386 0.1660863 -0.6059925 -0.1702041 0.7826884 -0.5986899 -0.6969277 0.003814816 -0.7171312 -0.6927043 -0.007843494 -0.7211791 -0.6621679 -0.02011191 -0.7490857 0.9997393 0.01712107 0.01510685 -0.892038 0.4060544 -0.1984646 -0.9992836 -0.02319443 0.02990859 -0.9998454 0.01443564 0.01004087 -0.9999651 0.00827068 0.001220762 -0.9999739 0.004974484 -0.005249202 -0.9973859 0.01959306 -0.06955242 -0.9993539 0.02020376 -0.02972573 -0.7420662 -0.0411089 -0.6690651 -0.7204378 -0.009186267 -0.6934587 0.9994706 0.03122103 -0.00915569 0.9999613 0.00802654 0.00363177 -0.9974758 0.02905386 -0.06479132 -0.993607 0.01617503 -0.11173 -0.9929749 0.006042778 -0.1181706 -0.1943767 0.9594625 -0.2040818 -0.8402497 0.3163297 -0.4403589 -0.9999013 -0.009735584 0.01013237 0.9961768 0.08636927 -0.01312321 0.6135287 -0.03576856 -0.7888619 0.9998865 0.01217699 0.008880913 -0.9207925 -0.003814876 -0.3900344 -0.9894457 0.01947087 -0.1435903 -0.9881166 0.004730403 -0.1536331 -0.6653172 -0.001556456 -0.7465592 -0.9999491 0.008362114 0.00564593 -0.7201359 -0.07828223 -0.6894028 -0.9071574 -0.004242181 -0.4207704 0.003173947 0.229198 -0.9733747 -0.871379 0.4063913 -0.2748543 -0.7031322 0.06830197 -0.7077711 0.9981889 0.05990898 -0.005462884 -0.9944831 0.03250241 -0.09973514 -0.07193326 0.8090585 -0.5833095 0.6563183 -0.03289979 -0.7537665 -0.9976949 0.03540194 -0.0578944 0.9999473 0.01016271 -0.001464843 0.004608333 0.7250695 -0.6886604 -0.6926687 -0.02710121 -0.7207466 -0.6716583 -0.0107426 -0.7407833 -0.8438765 0.3728487 -0.3858191 -0.7975539 0.27577 -0.5365247 -0.989741 0.03604328 -0.1382524 0.08121246 0.401759 -0.9121372 0.04431384 0.1004998 -0.9939498 0.9996076 0.0278635 -0.002899229 0.9990939 0.04239058 -0.003814816 0.9998176 0.01907408 -0.00100708 -0.6574522 -0.02246236 -0.7531615 -0.9977067 0.04468023 -0.05084514 -0.9999189 0.01269578 -0.001037597 -0.9844662 0.02462917 -0.1738388 -0.9823199 0.006927788 -0.1870824 -0.9747822 0.006470024 -0.2230646 0.6799397 -0.01934921 -0.7330128 -0.9912217 0.05401831 -0.1206715 0.3185282 -0.01638877 -0.9477717 0.3658966 0.03192323 -0.9301079 0.2978042 0.04022401 -0.9537792 -0.1355051 0.96935 -0.2049362 0.3810882 -0.02716171 -0.9241397 0.3452028 -0.02514779 -0.9381912 0.4047763 -0.02890169 -0.913959 0.1341009 0.240704 -0.9612901 0.4242132 0.02591055 -0.9051916 0.4382253 -0.02505624 -0.8985159 0.477599 -0.01150578 -0.8785027 0.4841883 0.03119069 -0.8744077 -0.9999247 0.005432367 -0.01101732 0.4972787 -0.02600228 -0.8672012 -0.8893697 -0.009461045 -0.457091 -0.9995042 0.02548354 -0.0184946 -0.9952072 0.0468778 -0.08582055 0.6984564 -0.01229912 -0.7155469 -0.9999597 0.001831114 -0.008789479 0.1143861 -0.003021359 -0.9934318 0.1556184 -0.01062071 -0.9877602 -0.6468544 -3.05192e-5 -0.7626137 -0.6347128 -0.006409049 -0.7727217 -0.03982681 -0.01892155 -0.9990274 -0.9775092 0.02752858 -0.2090891 0.1551912 -0.007965564 -0.9878523 0.1788116 -0.02356076 -0.9836012 0.2145467 -0.05914533 -0.9749214 0.2508041 -0.03378438 -0.9674482 -0.7311536 0.1749373 -0.6594024 -0.8637271 0.4552871 -0.2160767 0.211069 0.01785355 -0.9773082 0.1887609 0.003997981 -0.982015 0.2378063 -0.003235042 -0.9713072 -0.8402822 0.4534527 -0.2971644 0.2710113 -0.05078405 -0.9612356 0.3020439 -0.04132235 -0.9523981 -0.6310096 0.1713028 -0.7566256 -0.6045923 -0.01867794 -0.7963161 -0.8741868 -0.009521901 -0.4854965 0.5606359 -0.02896261 -0.8275559 0.5713829 -0.02194339 -0.8203902 0.5081818 -0.0267046 -0.8608358 0.526112 -0.03003036 -0.8498849 0.5237735 -0.03747779 -0.8510328 0.5456492 -0.02652096 -0.837594 0.5611227 -0.02447623 -0.8273707 0.7057896 -0.01641935 -0.7082312 -0.9862514 0.0506615 -0.1572948 0.5367045 -0.01138353 -0.8436936 0.5971592 0.04138332 -0.8010545 0.5395778 0.05075323 -0.8404046 -0.9702061 0.02914583 -0.240522 -0.9992291 -0.004211604 -0.039034 -0.9972143 -3.05192e-4 -0.07458889 -0.105046 -0.004272639 -0.9944582 -0.1591256 -0.04660236 -0.9861579 -0.1154841 -0.003296017 -0.993304 -0.9999926 8.54538e-4 -0.00375384 -0.9994706 0.003173947 -0.03238075 -0.9961901 -0.001770079 -0.08719176 -0.8067765 0.3637285 -0.4656323 -0.9973621 0.003875911 -0.0724827 -0.9918301 0.07089513 -0.1060528 0.7315104 -0.02511715 -0.6813676 0.7474811 -0.008850574 -0.6642242 -0.9977723 0.05582004 -0.03653174 0.001892149 6.40907e-4 -0.9999981 -0.9786821 0.04977637 -0.1992583 0.07526075 0.01141422 -0.9970986 0.09976571 -0.04757869 -0.9938728 0.04425191 -0.01312297 -0.9989342 0.1214969 -0.009613513 -0.9925453 -0.07327544 0.9789491 -0.1904978 0.2707673 -0.01739597 -0.9624877 0.3163914 -0.002746701 -0.9486249 0.2922226 -0.03491407 -0.9557129 0.2271254 -0.01269602 -0.9737828 0.2243769 -0.01693809 -0.9743552 -0.9952024 0.06128263 -0.0762676 0.4835401 -0.05630719 -0.8735094 0.4339557 -0.04062134 -0.900018 0.4896836 -0.02893233 -0.8714202 -0.6106484 -0.007293939 -0.7918683 -0.6081192 -0.01922684 -0.793613 0.5351179 -0.02325534 -0.8444572 0.1357474 0.6454105 -0.7516768 0.5868502 -0.02420157 -0.8093339 -0.6257068 -0.02627712 -0.7796157 -0.8506907 0.5179099 -0.0899704 -0.6522702 0.0715385 -0.7546032 0.05478149 0.8122621 -0.5807147 0.6383468 -0.01748764 -0.7695503 0.6564887 -0.02050858 -0.7540571 0.613762 -0.01001012 -0.7894278 0.6418257 -0.01751816 -0.7666505 0.5858381 -0.03378427 -0.8097237 0.6148297 -0.02389609 -0.7882979 0.64075 -0.02197378 -0.7674351 0.6648711 -0.020967 -0.7466638 0.6165565 -0.02585005 -0.7868862 0.5893664 -0.007965624 -0.8078266 -0.819957 0.5598427 -0.1193602 0.6069159 -0.02093654 -0.7944903 0.642347 0.04889243 -0.7648528 -0.9898568 -8.24016e-4 -0.1420664 -0.7545015 0.2973203 -0.5850881 -0.8585169 -0.00940001 -0.5126991 -0.9869369 -0.002868771 -0.1610814 -0.981426 0.07089531 -0.1782608 -0.9896593 4.5778e-4 -0.1434377 0.04300147 2.74672e-4 -0.9990751 -0.9999426 0.01071202 -4.57779e-4 -0.03735476 -0.001190185 -0.9993014 -0.06460952 -0.0184642 -0.9977398 -0.1060548 -0.03421217 -0.9937716 -0.9879608 0.07492411 -0.1353517 0.2583732 0.1110888 -0.9596367 0.2127517 0.2772395 -0.9369499 0.1822276 0.03292971 -0.9827048 0.002319455 -3.05194e-4 -0.9999973 0.01535123 -0.03335762 -0.9993256 0.1114261 -0.001739561 -0.9937712 0.4405164 -0.03274726 -0.8971471 0.1709973 -0.03100717 -0.9847835 0.1397153 -0.01165819 -0.9901231 0.7492811 -0.01730448 -0.6620261 0.4497031 -0.01944082 -0.8929666 -0.9616219 0.03378444 -0.2722902 -0.9526458 0.007141351 -0.3039982 0.6861283 -0.01995939 -0.7272068 0.7207413 -0.01919656 -0.6929383 -0.8077531 0.4378291 -0.3947665 0.6858326 -0.01974606 -0.7274916 0.6906396 -0.02728372 -0.7226843 0.7185136 -0.01767057 -0.6952885 0.7306866 -0.006714165 -0.6826801 -0.7078375 0.2195264 -0.6713973 -0.003784298 0.9799336 -0.1992889 -0.8382388 -0.01480174 -0.5451024 0.6500507 -0.01937943 -0.7596437 0.6683495 0.04403978 -0.7425427 0.7047303 0.04236131 -0.7082096 -0.5926213 -0.005554437 -0.8054621 -0.5794005 -0.01757884 -0.8148534 -0.5555458 -0.02612465 -0.8310754 -0.9753348 -0.004516839 -0.2206848 -0.9764682 -0.002258419 -0.21565 -0.9919591 0.08588045 -0.09296083 -0.9829467 0.09073209 -0.1599485 -0.1153306 -0.03814852 -0.9925944 -0.07217752 -0.01095628 -0.9973316 -0.9952124 0.07229858 -0.06576764 -0.1594924 -2.74671e-4 -0.9871991 -0.1202136 0.0220651 -0.9925029 -0.1921187 -0.04153668 -0.9804924 -0.1372377 0.8133148 0.5654068 0.773322 -0.009552419 -0.6339415 0.05587947 0.006775081 -0.9984146 -0.9735813 0.06177008 -0.2198271 0.09802597 0.03897231 -0.9944205 -0.5467775 -0.02337747 -0.8369517 -0.564795 -0.03360199 -0.824547 0.4133514 -0.01306217 -0.9104779 0.3624154 -0.02349972 -0.9317204 0.7585111 -0.0152288 -0.6514822 0.6967861 -0.01925766 -0.7170204 0.7353671 -0.01635843 -0.6774716 -0.9997196 0.02356082 -0.00238049 -0.9456779 0.008179187 -0.3250024 0.2902717 0.4053611 -0.8668476 0.7598641 -0.01385563 -0.6499344 0.2223055 0.7202039 -0.6571807 -0.5786092 -0.02688717 -0.8151617 0.7627004 -0.0137335 -0.6466062 0.7453047 -0.01586985 -0.6665351 0.7892025 -0.0132454 -0.6139903 0.7900452 -0.01428288 -0.6128824 0.8029245 -0.02371323 -0.5956089 -0.9924178 0.09152656 -0.08203512 0.6913618 -0.01062083 -0.7224308 -0.7628614 0.4116753 -0.4985639 -0.9602309 -0.005096733 -0.2791607 -0.9548377 -0.003387629 -0.2971088 -0.9619102 -0.001403868 -0.2733622 -0.9691213 0.01083409 -0.2463469 -0.9620436 0.008056938 -0.2727769 -0.9657278 0.06436556 -0.25145 -0.5578887 -0.008270621 -0.8298746 -0.5267611 -0.0225231 -0.849715 0.1380399 0.9486391 -0.2846558 -0.6655638 0.1715487 -0.7263581 -0.9989826 0.04208582 -0.0162056 -0.2547399 -0.006744623 -0.9669861 -0.1988651 0.03140455 -0.9795237 -0.2233663 -0.01071202 -0.9746758 -0.2611206 -0.01828086 -0.9651331 -0.5963764 0.0676611 -0.7998482 0.7938942 -0.005340814 -0.6080327 -0.9885768 0.09570807 -0.1164306 -0.02215695 0.002838253 -0.9997505 0.3430632 -0.02145481 -0.9390673 0.4033975 -0.1119735 -0.9081478 0.01532065 0.04007178 -0.9990795 0.3618013 -0.02905392 -0.9318025 0.7920986 -0.0139473 -0.6102339 0.769333 -0.01467978 -0.6386795 0.8078048 -0.01239067 -0.58932 0.825088 -0.0131843 -0.5648504 0.8163376 -0.009827256 -0.5774914 0.8461807 -0.01385575 -0.532716 0.8321395 -1.83116e-4 -0.5545664 0.8518586 0.03479212 -0.522615 0.8590492 -0.01471012 -0.5116817 0.8755885 -0.00576806 -0.4830234 -0.5089758 -0.0207228 -0.8605314 -0.8005464 0.508143 -0.3176733 0.3477365 0.2765046 -0.8958932 0.8546569 -0.008118033 -0.5191299 0.7096022 -0.02380496 -0.7042003 0.8231918 -0.004333674 -0.5677469 0.731501 -0.02117991 -0.6815115 -0.9972465 0.07043838 -0.02319461 0.8462645 -0.004791498 -0.5327417 0.8301577 -0.01013243 -0.5574365 0.07468122 0.9823621 -0.1714281 0.8472841 -0.01028507 -0.5310404 0.8581695 -0.001770079 -0.5133635 -0.9951825 0.08337694 -0.05157649 0.8661354 -0.007568717 -0.4997522 0.7394792 -0.01516801 -0.6730085 0.7356957 0.0401327 -0.6761222 0.7287698 -0.008972644 -0.6847 0.7644712 0.04165846 -0.6433106 -0.963987 0.1030031 -0.2451931 -0.8225814 -0.01382511 -0.5684793 -0.9773452 7.93498e-4 -0.2116503 -0.9384568 -0.009705007 -0.3452606 -0.9495786 0.001617491 -0.3135251 -0.9365938 0.009216666 -0.3502957 -0.9362035 -5.79863e-4 -0.3514578 -0.9472169 0.03714156 -0.3184348 -0.2936245 -0.05124157 -0.9545466 -0.3251241 -0.02612465 -0.9453105 -0.9836812 0.1124922 -0.1404169 -0.3419949 -0.02710074 -0.939311 -0.3132839 -0.011689 -0.9495875 -0.3233759 -0.02969467 -0.9458045 -0.3539024 -0.01611417 -0.9351435 -0.2913342 -0.03161764 -0.9560987 -0.2702789 -0.1256174 -0.9545521 -0.4989584 -0.03494447 -0.8659211 -0.100073 0.005981743 -0.9949622 -0.5386897 -0.00701934 -0.8424751 -0.9771553 0.09958326 -0.1877524 -0.7148587 0.388666 -0.5813053 -0.06631803 0.03238075 -0.997273 -0.630101 0.1974596 -0.7509877 0.2873407 -0.0174266 -0.95767 -0.9994191 0.03393757 -0.003143489 -0.9920187 0.107091 -0.06656181 0.8779945 -0.006561517 -0.4786258 0.8979658 -0.005920708 -0.4400255 0.8990108 -0.005432426 -0.4378927 0.9134246 0.05166983 -0.4037148 0.7809443 -0.0125432 -0.6244747 -0.7585352 0.5369321 -0.3692266 -0.678627 0.2941449 -0.6730114 0.3053473 0.6843686 -0.6621199 0.8767313 -0.007324635 -0.4809249 -0.8019585 -0.01318436 -0.5972343 0.8778312 -0.005218803 -0.4789419 0.8817991 -0.006042838 -0.4715868 0.8975236 -0.007477045 -0.4409032 0.8961774 -0.0104987 -0.4435718 0.7606324 -0.006988883 -0.6491454 0.7834954 0.04022461 -0.6200942 0.9017413 -0.002410948 -0.4322694 0.9102365 -0.03061079 -0.4129559 0.7772287 -0.01113945 -0.6291197 0.7934852 0.007599115 -0.6085422 0.4124057 0.2528819 -0.8751985 -0.7798677 0.6110332 -0.1358122 -0.9698106 0.1136232 -0.2157711 -0.9256712 -0.0102238 -0.3781912 -0.910793 -0.006866872 -0.4128066 -0.9031268 -0.00714147 -0.4293148 -0.9141333 6.10378e-4 -0.4054134 -0.8951666 0.01147532 -0.4455842 -0.9083833 0.009705185 -0.418026 -0.923661 0.01016283 -0.3830759 -0.998668 0.05127161 -0.005798578 -0.3769701 -0.05429321 -0.9246328 -0.3457534 -0.02758944 -0.9379197 -0.4318772 -0.02771139 -0.9015067 -0.4079203 -0.03814905 -0.9122202 -0.4714872 0.05023413 -0.8804411 -0.4348435 -0.0220046 -0.9002372 -0.4080105 -0.01812833 -0.9127972 -0.4209194 -0.02270621 -0.9068139 -0.4435645 -0.02169907 -0.8959797 0.8710081 -0.006042718 -0.4912315 -0.3812777 4.88309e-4 -0.9244605 -0.7333137 0.4959054 -0.4651116 -0.1400843 -0.01321488 -0.9900514 0.2057272 0.954048 -0.2178736 -0.1652321 -0.01260447 -0.9861742 -0.5032584 -0.01190239 -0.864054 -0.5004463 -0.02227866 -0.8654809 -0.4740546 -0.02795553 -0.8800516 -0.1411502 0.01446598 -0.9898825 0.9212965 -0.008087396 -0.3887768 0.9287343 0.00939995 -0.370627 0.8839424 -0.006836175 -0.4675459 0.374199 0.6113047 -0.6973391 0.9112337 -0.00552392 -0.4118527 -0.9883304 0.1196959 -0.09421241 0.9337967 -0.004425287 -0.3577768 -0.953121 0.07431471 -0.293322 0.9403507 0.02746707 -0.3390963 0.9451529 -0.008179128 -0.3265262 0.9540707 0.004425287 -0.2995491 0.9373667 -0.007904469 -0.3482546 0.9414919 -0.002655148 -0.3370254 0.9211878 2.44153e-4 -0.3891182 -0.5422918 0.06903392 -0.8373494 0.2765972 0.9209226 -0.2745829 0.9371744 -1.22076e-4 -0.3488615 -0.9925683 0.1155447 -0.03817921 0.9242338 -0.01941001 -0.3813334 0.8173637 0.009186208 -0.5760489 0.8056401 -0.01049852 -0.5923124 0.9330039 -0.003173947 -0.3598524 0.9352077 -0.01083439 -0.3539341 0.9446665 0.002563595 -0.3280223 0.9505115 -0.002105772 -0.3106825 -0.9832589 0.1341917 -0.1232659 -0.8955692 -0.01245158 -0.444748 -0.878729 -0.01107829 -0.4771926 -0.8825725 0.002014219 -0.4701722 -0.8771107 0.008484184 -0.4802135 -0.8610922 0.008087515 -0.5083847 -0.9967441 0.08014237 -0.008850395 0.4556893 0.1557724 -0.8764031 0.4467409 0.5656135 -0.693184 -0.4659907 -0.0474261 -0.8835177 -0.5182062 -0.02508628 -0.8548878 -0.4671267 -0.005798637 -0.8841715 -0.7266604 0.4938605 -0.4775633 -0.2114051 -0.01065111 -0.9773406 -0.9775879 0.1337957 -0.1625447 0.9592523 0.001403868 -0.2825477 0.9648334 -4.57788e-4 -0.2628617 0.9706922 0.009064137 -0.2401552 0.9559374 -0.002594053 -0.2935594 0.4900799 0.3394364 -0.8028728 0.8197776 -0.01400828 -0.5725107 -0.9569589 0.105108 -0.2705219 0.9721435 -0.003967404 -0.2343533 0.9773358 0.01742625 -0.2109768 0.8386624 -0.01327574 -0.5444897 0.9699379 -0.003235042 -0.2433314 0.8481155 -0.01165813 -0.5296831 -0.9921402 0.1243036 -0.01437437 0.8372054 -0.006927847 -0.5468448 0.8324771 0.008911609 -0.5539878 -0.4828739 0.002685666 -0.8756858 0.8548945 0.01306205 -0.5186377 0.8680608 0.009552538 -0.4963661 0.9555443 -0.001739561 -0.2948423 0.8977022 0.002471983 -0.4405958 0.9576455 -0.001892149 -0.2879437 0.9641489 -0.004089474 -0.2653301 0.9692017 -0.001983702 -0.2462603 0.9718478 0.001220703 -0.2356068 0.9763135 -0.001983702 -0.2163515 0.9697678 -7.01934e-4 -0.2440289 0.3691005 0.7860857 -0.4958167 -0.7850131 -0.00149542 -0.6194773 -0.9994522 -0.01309245 -0.03039646 -0.930661 0.0428493 -0.3633649 -0.8501971 -0.01318418 -0.5262994 -0.5860337 0.2292618 -0.7771767 -0.8409364 -0.007538318 -0.5410816 -0.6653589 0.3755443 -0.6451854 -0.9704633 0.1402028 -0.1963266 -0.8662573 -0.005401849 -0.499569 -0.5475773 -0.03729456 -0.8359236 0.5247686 0.19947 -0.8275443 -0.9847785 0.1594909 -0.06909441 -0.6000422 -0.02398824 -0.7996087 -0.5502209 -0.009949028 -0.8349599 -0.2609344 0.006439387 -0.9653351 -0.3282661 0.008911609 -0.9445433 0.9800575 -0.001434385 -0.1987094 0.9842332 0.01120036 -0.1765211 0.3336709 0.9196168 -0.2072892 -0.9598672 0.1369102 -0.2447659 0.8609005 -0.009491264 -0.5086848 -0.7750301 0.01748734 -0.6316823 0.9600301 -0.002197325 -0.279888 0.9742555 -0.001281738 -0.2254433 0.8732935 -0.01312303 -0.4870179 0.9804829 -8.54531e-4 -0.1966032 0.9862342 -5.49349e-4 -0.165354 -0.9177376 0.04648047 -0.3944584 0.9864932 0.0810582 -0.1423402 0.9907529 -0.002288937 -0.1356589 -0.73811 0.6583324 -0.1476219 -0.9814332 0.1612934 -0.1037954 0.509392 0.5300838 -0.6778872 0.9851861 -9.15572e-4 -0.1714866 0.9927399 -0.003021299 -0.1202433 0.9943743 0.006073296 -0.1057488 0.87321 0.004028499 -0.4873276 0.8861222 0.01013231 -0.463341 0.9001647 0.00939989 -0.4354484 0.4442009 0.7500907 -0.4899485 0.5618926 0.4122865 -0.7171448 0.9812027 -0.001434385 -0.1929749 0.9806475 -3.96751e-4 -0.1957815 0.9886022 -4.88304e-4 -0.1505503 0.9898177 -6.40896e-4 -0.1423399 0.9928115 0.002410948 -0.1196647 -0.5248012 0.1565614 -0.8367033 -0.9707572 0.1640106 -0.1753027 -0.6745986 -0.02273684 -0.7378346 0.5904882 0.2321606 -0.7729329 0.5130298 0.6915983 -0.5084214 -0.6321212 -0.03656238 -0.7740066 -0.6910768 -0.01974588 -0.7225116 -0.7435284 0.01153606 -0.6686049 -0.8086625 -0.01419132 -0.5881016 -0.74883 -0.04837363 -0.6609944 0.3971424 0.8937153 -0.2086881 0.4616616 0.8617357 -0.2104288 -0.8263176 -0.01153635 -0.5630862 -0.8473588 0.01580882 -0.5307857 -0.7143 0.5889885 -0.3779792 -0.6936644 -0.0195626 -0.7200328 -0.8024466 -0.006866812 -0.5966847 -0.9381082 0.08380651 -0.3360501 0.9964564 -2.44155e-4 -0.08411127 0.9933023 3.0519e-5 -0.1155449 0.9963129 -0.002563536 -0.08575749 -0.977943 0.1625733 -0.131139 0.8971981 -0.004028499 -0.4416102 0.9174037 0.01751792 -0.3975721 0.934959 0.03717249 -0.3528032 0.9393136 0.01107835 -0.3428809 0.9990842 3.35714e-4 -0.0427882 0.9999447 0.009979605 -0.003326535 0.9832124 0.1813766 -0.01989859 0.9980541 0.05294996 -0.03292965 0.9981392 -2.74675e-4 -0.06097787 0.5717772 0.5831303 -0.5770875 0.9961042 0.003662228 -0.08810782 0.9982579 0.001037657 -0.05899393 0.9999157 -0.01199382 -0.004974544 -0.676187 0.5244447 -0.5174253 0.9999579 0.00915569 6.10382e-4 -0.9703975 0.1791799 -0.1619364 0.6348862 0.1700212 -0.753666 0.6274447 0.4601994 -0.6281161 -0.9261881 0.09085482 -0.3659523 0.5664426 0.6901077 -0.4504378 0.5183415 0.8315621 -0.1995663 0.6637275 0.3195035 -0.6763013 -0.3780388 -0.0210886 -0.9255495 -0.410822 -0.003967523 -0.9117069 -0.3243919 -0.01867789 -0.9457384 -0.458607 0.09265524 -0.8837957 -0.943226 0.1177124 -0.3105937 -0.6157295 0.4192149 -0.6671853 0.6788994 0.2118337 -0.7030095 0.6417959 0.4154323 -0.6446037 -0.2861216 0.05615609 -0.9565464 -0.251717 -0.02758872 -0.9674076 0.6277147 0.6309803 -0.4558928 0.5742525 0.7958536 -0.1919668 -0.9615327 0.1722189 -0.2139995 -0.9038535 0.04889154 -0.4250394 0.713707 0.1996914 -0.6713759 -0.6956309 0.7014906 -0.1549475 -0.9313539 0.1279364 -0.3408992 0.681599 0.4982665 -0.5358669 -0.9772707 0.1834538 -0.106239 0.7248152 0.3641319 -0.5848513 0.7330908 0.1434991 -0.6648203 -0.9418657 0.1629999 -0.2938027 -0.9840535 0.1760333 -0.02551382 -0.1846705 -0.007355034 -0.982773 -0.9707565 0.198955 -0.1343457 -0.3974779 0.001159667 -0.9176111 0.6262581 0.7589259 -0.1784164 0.7549133 0.2510783 -0.6058595 0.6779432 0.6112903 -0.4083102 -0.6618781 0.6259871 -0.4123805 0.7133253 0.5028343 -0.4881851 -0.8889846 0.0517295 -0.455006 0.2906384 -0.01410001 -0.9567291 0.399958 -0.03091621 -0.9160121 -0.5700697 0.405998 -0.7142733 -0.6958722 0.58997 -0.4095089 -0.6437411 0.7464076 -0.1687405 0.7766487 0.140998 -0.6139516 0.7718709 0.3804267 -0.5094025 -0.6217457 0.5591499 -0.5484376 0.749953 0.4734473 -0.461972 0.7827827 0.2387503 -0.5746734 -0.5104891 0.2839475 -0.8116494 0.6762557 0.7182813 -0.1635551 -0.9792911 0.1892478 -0.07193309 -0.3622942 0.003173947 -0.9320584 0.7258046 0.5807474 -0.3687009 -0.9147834 0.101629 -0.3909513 0.8107461 0.2526391 -0.5280761 -0.4167684 0.1692892 -0.8931099 0.7981774 0.3290616 -0.50461 0.8114934 0.1501217 -0.5647494 0.8198601 0.06161767 -0.5692387 0.9982634 0.007141351 -0.05847358 -0.9716734 0.2220584 -0.08087611 0.7718329 0.5208728 -0.3646445 0.8440939 0.1470099 -0.5156487 -0.9685223 0.2219346 -0.1127374 0.7370212 0.5643405 -0.3719136 0.7238264 0.6735611 -0.1496361 0.8281387 0.2378972 -0.5075345 -0.9608429 0.205182 -0.1862295 0.803174 0.4246442 -0.4178385 0.8206059 0.3541778 -0.4485133 0.8336873 0.208994 -0.5111626 -0.8996245 0.1048651 -0.4238859 0.8376928 0.3015914 -0.4553169 0.8551198 0.05179119 -0.515837 -0.9222611 0.1527183 -0.355122 -0.9494417 0.1975486 -0.2439982 0.8206394 0.4263577 -0.3804869 0.7964167 0.5133752 -0.3196349 0.7695953 0.6232571 -0.1388305 0.858972 0.2415314 -0.4514753 -0.5136642 0.4667261 -0.7199417 -0.5914909 0.7842496 -0.1873263 0.8705722 0.1164628 -0.4780593 -0.8737936 0.05362212 -0.4833316 0.8402297 0.370414 -0.3959892 0.8517625 0.3176141 -0.4166797 0.8812846 0.1847047 -0.4349961 -0.6123756 0.6595284 -0.4359113 0.8165381 0.4875118 -0.3091889 0.8381881 0.4570909 -0.2975043 0.8797342 0.05227863 -0.4725832 -0.5807456 0.6210306 -0.5263608 0.8742247 0.3147148 -0.3697105 0.8958001 0.05020409 -0.441613 -0.9045056 0.1480805 -0.3999272 0.8506032 0.3872899 -0.3556413 0.8925631 0.1130734 -0.4365154 0.8880265 0.2482457 -0.387018 0.8107739 0.5718693 -0.1249457 0.902246 0.171916 -0.3954708 0.8783742 0.2914892 -0.3788048 0.9071474 0.1059926 -0.4072458 -0.8575602 0.05606383 -0.5113194 0.8628985 0.4122833 -0.2922819 0.9097431 0.05261486 -0.4118244 0.9161618 0.1597995 -0.3675756 0.9239406 0.04657238 -0.3796903 -0.5382637 0.81501 -0.2145485 0.9211555 0.1185053 -0.3707145 0.8945159 0.3166055 -0.3155984 0.8460127 0.4766737 -0.2388404 0.9090634 0.2267851 -0.3495318 -0.9530431 0.2267856 -0.200692 -0.8871691 0.1082218 -0.4485744 0.9335147 0.09546333 -0.345597 0.8420472 0.5303881 -0.09820979 0.9214887 0.2112826 -0.3259117 0.8662632 0.4509244 -0.2150703 0.9350326 0.04226839 -0.3520334 0.8903172 0.3735596 -0.2603625 0.9284577 0.1742651 -0.3280213 0.9117946 0.2809607 -0.2994859 0.9464185 0.04007112 -0.3204473 0.9407477 0.159187 -0.2994217 0.9366372 0.07553523 -0.3420603 0.8845756 0.419093 -0.2046633 0.9088065 0.3473107 -0.2311844 0.9242399 0.2613959 -0.2783035 0.9515644 0.007385671 -0.3073609 0.9322487 0.2256612 -0.2828242 0.9499948 0.09470039 -0.29756 0.9613472 0.006988823 -0.2752505 0.9225662 0.3158131 -0.2216612 0.8733712 0.4802214 -0.08130341 0.9564243 0.03448718 -0.2899365 0.943673 0.2056664 -0.2591965 0.9503301 0.1470708 -0.2743045 0.9590394 0.1103884 -0.2608792 0.965523 0.03076291 -0.2584941 0.9336733 0.2659441 -0.2398502 0.9588744 0.1378845 -0.2480884 0.9699366 0.006561636 -0.24327 0.9656928 0.06512826 -0.2513877 0.9510596 0.1805496 -0.2507736 0.9728826 0.02996957 -0.2293498 0.9335234 0.08688837 0.3478281 0.937854 0.1251286 0.3236863 0.9450844 0.0880475 0.3147433 -0.812058 0.3879005 0.4359989 -0.8082677 0.4428015 0.3881112 -0.7638459 0.4606941 0.451996 0.8508949 0.5139977 0.1085554 0.8835375 0.4591038 0.09265697 0.881338 0.4211977 0.2140933 0.9023303 0.3177962 0.2912141 0.899116 0.3712618 0.2318517 0.914626 0.3051602 0.2652107 -0.5834949 0.04049885 0.8111064 -0.6799 0.1860126 0.7093203 -0.626838 0.191204 0.7553247 0.9154241 0.2530348 0.3130051 0.9256574 0.260118 0.2747673 0.9486323 0.03579914 0.3143491 0.9549678 0.07712131 0.2865117 -0.7441512 0.2498917 0.6195105 -0.7571207 0.357441 0.5468128 -0.7034471 0.3183808 0.6354494 0.9304597 0.1448121 0.3365623 0.9337096 0.1958733 0.2997002 0.9425318 0.1626991 0.2918269 -0.7990584 0.5027152 0.3298229 -0.8300905 0.4951428 0.256483 -0.7727666 0.6175174 0.1466428 0.9413274 0.005615472 0.3374484 0.9592732 0.03439491 0.2803781 0.9571626 0.01660221 0.2890745 0.9108511 0.3676975 0.1874806 0.9351494 0.2356109 0.2645434 0.9211199 0.3186473 0.2236115 0.9526129 0.1438044 0.2680463 0.2317035 -0.04172003 0.9718915 0.2071648 -0.01413041 0.978204 0.2808065 -0.02291983 0.9594907 0.8835377 0.4126839 0.2214795 0.9586625 0.1106313 0.2621581 0.9453014 0.2094837 0.2500437 0.9354414 0.2692091 0.2290765 -0.8073805 0.5775724 0.1206111 0.9641324 0.07358181 0.2550185 0.9679567 0.03128248 0.2491612 0.9720845 0.009338736 0.2344453 0.9684869 0.03137338 0.247081 -0.04898303 -0.06534123 0.9966601 -0.03888076 -0.001495361 0.9992428 -0.05990791 -0.002533018 0.9982007 0.9547609 0.1864414 0.2316707 0.9078322 0.4129894 0.07266682 0.9614984 0.1430727 0.2346294 0.9755156 0.02768087 0.2181817 -0.7143646 0.4115218 0.5659799 0.9724313 0.06509733 0.2239189 0.9266306 0.3353168 0.1700541 0.9699429 -0.006958246 0.2432334 0.9799056 0.005676507 0.1993809 0.9793719 -0.001068115 0.2020635 0.9451123 0.2661867 0.1894924 -0.5200834 -0.006714284 0.8540892 -0.5339416 0.05185288 0.8439299 0.9680166 0.1177441 0.2215408 0.9626618 0.1722799 0.2088112 0.9558987 0.2176035 0.197247 0.9856184 -7.32461e-4 0.1689849 0.9859806 0.004944086 0.1667869 0.9785037 0.0546292 0.1988625 -0.7561391 0.5507767 0.3534102 0.9821135 0.02404922 0.186748 0.975959 0.1028786 0.192146 0.9803407 0.07718324 0.1815897 -0.6572358 0.3706279 0.6562592 0.9702094 0.155618 0.1856796 0.9619217 0.2023395 0.1837536 0.9320496 0.3571941 0.06079375 0.9875409 0.02032577 0.1560447 0.9535294 0.2542216 0.1617191 0.9396273 0.3093739 0.1462484 0.9907221 -4.27272e-4 0.135903 0.9909717 0.004150509 0.1340075 -0.7182616 0.4991056 0.4847617 0.984387 0.04873865 0.1691357 0.9768276 0.1344675 0.1665127 0.9944812 0.003357052 0.1048621 0.9890701 0.03946143 0.1420674 0.9817066 0.09955316 0.1623003 0.9619457 0.2265394 0.152776 0.9919949 0.01654136 0.1251896 -0.5686675 0.1957512 0.7989361 0.9695114 0.2029849 0.1372765 0.9491087 0.2930124 0.1154837 0.9861949 0.07880038 0.1456372 0.9900899 0.05627655 0.1286671 0.9962508 0.03985732 0.0767849 0.9972454 0.00250256 0.07413113 0.2951807 -0.02334707 0.9551562 0.3478518 -0.02453696 0.9372285 0.9822093 0.1196669 0.1447235 0.9951075 0.01303166 0.09793591 0.9774345 0.1564407 0.1419442 0.959767 0.2503793 0.1271123 0.992981 0.03357136 0.1134102 0.9509276 0.3067518 0.04049938 0.9991392 0.002411007 0.04141461 0.9978561 0.05328649 0.03799641 0.9626389 0.2532184 0.09595257 0.9975017 0.01010161 0.06991815 0.9871178 0.1034911 0.1220163 -0.9978119 0.0127263 0.06488293 -0.9992354 0.004120111 0.03888165 0.9957885 0.02658241 0.0877434 -0.9906731 0.01248216 0.1356869 -0.993343 0.05127245 0.1031553 0.9817982 0.1405403 0.1277528 -0.9982618 8.54519e-4 0.05893129 -0.9996744 4.88301e-4 0.0255137 0.9909006 0.07403981 0.1124026 0.9763619 0.1824727 0.1158499 -0.9971897 -0.002868771 0.07486397 -0.9777017 0.007110834 0.209878 -0.9838784 0.05002099 0.1717011 -0.7292209 0.6680303 0.1482307 0.9997718 0.001098632 0.02133274 0.9914736 0.1279055 0.02490353 -0.999895 0 0.0144965 0.9937883 0.05124127 0.09878981 0.9992356 0.01037657 0.03769141 -0.9995927 -6.40901e-4 0.02853536 -0.9984742 -0.001068174 0.05520963 0.9977867 0.0198068 0.06347942 -0.9980345 -0.002288877 0.06262511 0.9786963 0.204541 0.01779276 0.9999499 1.52594e-4 0.01001018 0.9818629 0.1569906 0.1062983 -0.9627268 0.007599234 0.2703692 -0.9715437 0.00665313 0.2367668 0.9964897 0.03854602 0.07431477 -0.1679793 -0.01190257 0.9857187 -0.183448 -0.006561517 0.9830076 -0.222298 -0.02667331 0.9746139 0.9866107 0.1346793 0.09198355 0.9913464 0.08984768 0.09570735 0.9999313 0.002929806 0.01135313 0.9767917 0.2019138 0.0714755 -0.9998827 -9.1556e-4 0.01528984 -0.9416391 0.006286919 0.3365656 -0.9529089 0.02295058 0.3023868 0.9658375 0.2459533 0.08163851 0.9687339 0.2466545 0.0267651 0.9940276 0.06558489 0.08722269 -0.7080601 0.5923594 0.3843973 0.9983999 0.02584958 0.05029535 -0.5994929 0.3549713 0.7173588 0.982051 0.1680699 0.08560669 0.991359 0.1018429 0.08267682 -0.9858111 -7.01949e-4 0.1678574 -0.9949465 1.22076e-4 0.1004071 -0.6702688 0.522463 0.5270409 0.9965513 0.05343949 0.06348043 0.9942048 0.07922631 0.07266479 -0.9120864 -0.002533018 0.4099903 -0.9304376 0.01080375 0.3662909 -0.9908929 -8.54534e-4 0.1346501 0.9907068 0.1188713 0.06610405 -0.4863486 0.08792483 0.8693298 0.9980217 0.04095602 0.04770064 0.9998206 0.01147496 0.01507616 0.9873477 0.1485355 0.05551385 -0.9793151 -0.00100708 0.2023391 0.9962471 0.06381618 0.05847525 -0.9718106 -0.00149542 0.2357578 0.982614 0.1844862 0.02084434 -0.6591916 0.6282143 0.4132956 0.9991279 0.02963382 0.02942019 0.9940926 0.09170931 0.05804693 0.9997634 -0.005584895 0.02102738 0.9981448 0.05081361 0.03354001 -0.9789977 -9.76617e-4 0.2038688 -0.6820123 0.7128673 0.1633387 -0.5306095 0.2829449 0.7989967 0.9963105 0.07214599 0.04647982 -0.9633175 -0.001892209 0.2683579 0.2380504 -0.0371114 0.9705436 -0.6162123 0.5039323 0.6052559 -0.9312848 -0.003662228 0.3642738 -0.9537322 -0.002533078 0.3006469 -0.6338791 0.7533608 0.1750568 0.3541161 -0.02713167 0.9348078 -0.8762615 0.001434385 0.4818339 -0.8917672 0.01001024 0.4523841 0.999953 -0.006592035 -0.007110893 0.9999014 -0.006561577 -0.0124213 0.9942114 0.1028789 0.03097659 0.4091688 -0.02691775 0.9120616 0.9919564 0.1260431 0.01165819 -0.9629284 -0.00100708 0.2697554 0.9993332 -0.02215653 -0.02902323 -0.9436126 -0.003051877 0.3310381 0.9972502 0.06943148 0.02591091 -0.9172154 -0.004760921 0.3983633 -0.9044849 -0.005554378 0.4264694 0.9995852 0.02401834 0.01590031 -0.9718773 -0.001281797 0.2354845 -0.43035 0.03396773 0.9020228 -0.4660592 0.2897801 0.8359524 0.9999993 2.13636e-4 -0.00125128 -0.8469133 -0.00939995 0.5316479 -0.8419066 -0.02017325 0.5392463 -0.5541392 0.4593767 0.6941923 0.05585002 0.1399914 0.9885764 0.05221879 -0.004059076 0.9986274 0.08658206 -0.00576806 0.996228 -0.9414443 -0.004303097 0.3371413 -0.88894 -0.006683707 0.4579749 -0.930574 -0.003906488 0.366083 -0.6097176 0.6411831 0.4659709 -0.8670399 -0.008942008 0.4981588 0.9988781 0.04428267 0.01678526 0.9998454 0.01443564 -0.01004087 -0.9546396 -0.003357052 0.297745 0.9999119 0.01217693 -0.005279719 0.9968776 0.07849526 0.008575856 -0.8098609 -0.02264529 0.5861847 -0.4114274 0.1346501 0.9014416 -0.5820551 0.7902546 0.1915972 -0.7728943 -0.01312315 0.634399 0.999983 0.005798578 -6.71416e-4 0.9997652 0.02151572 0.002624571 -0.999998 3.66233e-4 -0.002014279 -0.9999431 -0.006378352 -0.00854516 -0.9104452 -0.005401849 0.4135946 0.03875964 -0.00476098 0.9992373 0.9985899 0.0528593 0.004913568 -0.9999743 0.001495361 0.007019281 -0.3497521 0.009674608 0.9367924 -0.3464875 0.1775012 0.921108 -0.9977427 0.003112971 0.0670818 -0.999078 0.003875911 0.04275745 -0.9979248 0.01348954 0.06296157 -0.9997372 0.01748728 -0.01483213 -0.9995498 -0.01818913 -0.02386564 0.9994333 0.03347921 0.00350964 -0.9998478 0.003540217 0.01709085 -0.007690787 -0.004425227 0.9999607 -0.9318703 -0.005493402 0.3627501 -0.8450232 -0.005523979 0.5347012 -0.8716917 -0.00814861 0.489987 -0.8615524 -0.008789479 0.5075926 -0.8868656 -0.008850634 0.4619429 -0.8793344 0.007080316 0.4761523 -0.8470498 0.005493342 0.5314852 -0.9993067 0.01388603 0.0345472 -0.8928164 -0.006378531 0.4503757 -0.8254428 -0.01068156 0.5643847 -0.8101006 0.004577815 0.5862731 -0.8076043 -0.01031553 0.5896348 -0.7862166 -0.01324552 0.6178091 -0.7671043 -0.01278758 0.641395 -0.7695438 0.003296077 0.6385856 -0.7429913 -0.01623624 0.6691042 -0.7251924 0.003173947 0.6885392 -0.7221841 -0.01617532 0.6915119 -0.5526786 0.6137787 0.5637573 -0.6816731 -0.02295029 0.731297 -0.6955701 -0.01867789 0.7182156 -0.6893953 0.004699885 0.7243701 -0.6421248 -0.02426278 0.766216 -0.6638223 0.005249261 0.7478721 -0.6574322 -0.02377396 0.7531386 0.4266217 0.02700906 0.9040268 0.4879375 0.03024429 0.8723545 0.4425594 -0.02743679 0.8963194 0.4123108 -0.02862673 0.9105935 0.3807547 -0.02749758 0.9242672 0.4594063 -0.03192311 0.8876524 0.4998964 -0.02703952 0.8656631 0.5163896 -0.03003114 0.8558272 0.4815288 -0.0323196 0.8758342 0.5466797 0.03720217 0.8365149 0.5557877 -0.02594143 0.8309195 0.5379952 -0.0305193 0.8423953 0.577421 6.7142e-4 0.8164464 0.5729368 -0.02758932 0.8191351 0.6126717 -0.02246206 0.7900183 -0.7694097 -0.01382499 0.6386061 0.5967673 -0.02829104 0.8019156 0.6076772 0.00137335 0.794183 -0.9955925 0.0119329 0.09302204 -0.9999843 3.0519e-4 0.005615472 -0.9495272 -0.002990782 0.3136708 -0.9394379 -0.005920648 0.3426681 -0.9233834 -0.002929806 0.3838682 -0.9430144 -6.40904e-4 0.3327514 -0.9589516 0.00500518 0.2835257 -0.9436715 0.006317377 0.3308237 -0.6309243 -0.03262501 0.7751582 -0.606051 -0.0446496 0.7941716 -0.6000647 -0.00878942 0.7999032 -0.5771442 -0.02954232 0.8161078 -0.6450834 0.06146556 0.7616361 -0.4883306 -0.05218702 0.8710969 -0.6046404 -0.02368265 0.7961465 0.3609218 -0.03811866 0.9318168 0.3408668 -0.02288925 0.9398329 0.3860629 0.0532248 0.9209357 0.3391558 -0.02990841 0.9402547 0.3072389 0.0350973 0.950985 0.6416984 -0.04229968 0.7657898 0.6221928 -0.03286904 0.7821738 0.6736369 -0.03540152 0.7382141 0.6580293 -0.01599216 0.7528226 0.6503284 -0.01394712 0.7595252 0.6555259 0.04751867 0.7536762 -0.5053144 0.5597615 0.656753 0.6813322 0.007843315 0.7319324 0.6704193 -0.01135319 0.7418957 -0.9996487 8.8505e-4 0.02649044 -0.9999793 6.71414e-4 0.006408929 -0.998892 0.001770079 0.04702997 -0.9633018 -0.001770079 0.2684146 -0.9625818 -0.002319455 0.2709817 -0.9093107 -0.00439471 0.4160947 -0.9259863 -0.006958365 0.3774933 -0.9610551 -0.001464903 0.276353 -0.9758812 0.005127191 0.2182426 -0.9336474 0.01107847 0.3580223 -0.9106393 0.004974663 0.4131723 -0.926162 -0.001190185 0.377124 -0.533444 0.8120837 0.2365538 -0.5558479 -0.05374443 0.8295449 -0.5607903 -0.01437449 0.8278332 0.2906308 -0.01916581 0.9566434 -0.8134226 0.003692746 0.5816615 0.2664917 0.1550974 0.9512766 0.6980282 -0.01178026 0.7159734 0.7112501 0.05914622 0.7004464 0.7059009 -0.006439447 0.7082815 0.7291865 0.005462825 0.6842933 -0.9998783 0.01278722 -0.008941888 -0.8710264 -0.007385671 0.4911807 -0.9949797 -0.007873892 0.09976655 -0.9769508 -0.00238049 0.2134514 -0.9952806 0.001983702 0.09701937 -0.9960166 0.02557486 0.08542251 -0.9954375 0.001678526 0.0954023 -0.9910785 0.004120051 0.133216 -0.9801819 -1.52595e-4 0.1980994 -0.976773 -4.27271e-4 0.2142766 -0.9861196 0.005035579 0.1659606 -0.9121368 -0.007568836 0.4098161 -0.9129828 -0.003723323 0.4079811 -0.5244057 -0.02572739 0.8510798 -0.5334447 -0.03723335 0.8450151 -0.9968436 0.03204494 0.07263529 -0.7162635 -0.006561696 0.6977992 -0.7388015 0.002533018 0.6739183 -0.5086597 -0.03616493 0.8602078 -0.4963957 -0.03106856 0.8675404 -0.4895527 -0.02792471 0.8715265 -0.429342 0.4891288 0.7592224 0.2060328 -0.02133268 0.9783126 0.2464081 0.02099686 0.9689387 0.2109199 0.2348167 0.9488804 0.7151353 -0.02249294 0.6986242 0.6942816 -0.02887117 0.7191243 0.7272496 -0.01538181 0.6862008 0.7395393 -0.02169907 0.6727635 0.749632 -0.01535087 0.661677 0.7526689 -0.01809799 0.6581504 0.7382612 -0.01162779 0.6744148 -0.9999015 -0.01028496 -0.009552478 0.7591449 0.008881151 0.6508612 0.7764265 0.008514702 0.6301503 -0.9874066 -0.002746641 0.1581791 -0.9901484 -8.24005e-4 0.1400197 -0.9889382 -0.001403868 0.1483224 -0.8929939 -0.0105291 0.4499456 -0.9922252 0.01556456 0.1234788 -0.7214165 -0.01788431 0.6922705 -0.4734123 -0.04208582 0.8798351 -0.447986 -0.04654121 0.8928285 -0.4631012 0.02295053 0.8860083 0.1682533 -0.04840368 0.9845547 0.1291859 0.003021299 0.9916158 0.1673337 0.02887064 0.9854776 0.7696642 -0.02029526 0.6381264 0.7860465 -0.02313333 0.6177344 0.7922142 -0.01290953 0.6101068 -0.9996791 0.01797562 0.01785355 0.7867861 0.00238049 0.6172212 0.8023785 0.009460926 0.5967406 0.8180146 0.01309287 0.5750486 0.1610796 -0.005615472 0.9869254 -0.9875573 0.01953202 0.1560425 -0.408074 -0.03933942 0.9121009 -0.3772788 -0.03170943 0.9255568 -0.4159756 -0.01272642 0.9092867 0.1202754 -0.04443562 0.9917456 0.08630812 0.001892149 0.9962668 0.06842404 0.3139206 0.9469805 0.1277236 0.1656898 0.977872 -0.998543 0.03039664 0.04458785 0.8084844 -0.02282834 0.5880747 0.808914 -0.01904404 0.5876186 -0.3747774 -0.06421267 0.9248885 -0.3337893 -0.08160853 0.9391086 -0.2916685 -0.01861643 0.9563383 -0.3123627 -0.04983758 0.9486548 0.03274673 -0.001800596 0.9994621 -0.049196 0.2013924 0.9782745 0.04474073 0.05221784 0.997633 0.8265691 -0.01687681 0.5625821 0.8330062 -0.009582817 0.5531808 0.8526731 -0.00915569 0.5223648 0.8421691 -0.02002036 0.5388417 0.8328763 0.007416188 0.5534095 0.8398301 0.00601226 0.5428163 0.8447732 0.008057057 0.5350638 0.8505222 0.007599353 0.5258845 -0.9999621 0.007965505 -0.003540217 -0.2979596 -0.03958344 0.9537575 -0.991261 0.03460866 0.1272952 -0.2701564 0.0070194 0.9627909 -0.2786072 0.1949548 0.9404099 0.8634833 -0.01797586 0.5040572 0.8641152 -0.002288877 0.503289 -0.6602622 -0.010163 0.7509666 -0.6899802 0.0430932 0.7225443 0.8856835 0.05105775 0.4614735 0.8886858 0.007355093 0.458458 -0.2519372 -0.04870885 0.9665171 -0.2158893 0.02569669 0.9760797 -0.1704162 0.06823974 0.9830064 -0.1627262 0.03634786 0.9860016 0.8851616 -0.00625652 0.4652417 0.8771465 -0.009705007 0.4801248 0.9007527 -0.00589025 0.4342926 0.8932094 -0.009064197 0.4495498 0.9006494 -0.00814855 0.4344701 0.8925278 -0.003479123 0.4509791 0.9051114 0.01281809 0.4249815 0.916913 0.009460866 0.398975 -0.9994528 -0.01275676 0.03051853 -0.1116386 0.009857594 0.9937 -0.1764929 -0.04013282 0.9834835 0.9287493 -0.004455745 0.3706818 0.9159471 -0.004974603 0.4012686 0.9116985 -0.005859673 0.4108182 0.9217011 -0.009949147 0.387773 0.9213023 0.007263422 0.3887793 0.9307183 0.01135319 0.3655608 -0.9994861 -0.01800596 0.02652066 0.9419374 -0.003570675 0.3357697 0.9406239 -0.007385551 0.3393705 0.9616633 -0.01449662 0.2738497 0.9524654 -0.02557486 0.3035718 -0.9968245 0.04480242 0.06583034 0.942215 -9.15572e-4 0.3350077 -0.3822271 0.4574273 0.8029089 -0.9918581 0.05294996 0.1158185 -0.9999874 0.004211604 0.002746701 -0.476837 0.7130581 0.5139793 -0.9986892 0.0371716 0.0351879 -0.978894 0.02624624 0.2026761 -0.9946557 0.04904454 0.09085601 -0.9995287 0.02694827 0.01471012 -0.9999551 0.009430348 9.76608e-4 -0.6153642 -0.0190441 0.7880129 -0.6319034 -0.02203494 0.774734 -0.9969422 0.05539244 0.05511778 -0.3264613 0.575374 0.7499119 -0.4648666 0.8724185 0.1509473 -0.4262312 0.8205087 0.3809101 -0.3975746 0.6967551 0.5970486 -0.9813835 0.05078434 0.1852225 -0.6587768 -0.02111893 0.7520421 -0.992151 0.06955337 0.1039181 -0.2246822 0.2958834 0.928424 -0.9984032 0.04565662 0.03326582 -0.9831123 0.07477182 0.1670311 -0.9639043 0.03320437 0.2641703 -0.6853415 -0.02002066 0.7279465 0.5314589 -0.0237438 0.8467513 0.4688962 -0.02642953 0.8828579 0.5134933 -0.02618587 0.857694 -0.5534365 -0.01541221 0.8327489 -0.5820942 0.0397666 0.8121485 -0.9927754 0.08539146 0.08429282 -0.99686 0.06680524 0.04251241 -0.1444153 0.3288714 0.9332673 -0.996769 0.0758388 0.02645963 -0.9998571 0.01681584 0.001770079 -0.9680473 0.06277853 0.2427824 -0.9843311 0.09976679 0.1453927 -0.9942553 0.08273756 0.06790524 -0.2520244 0.7073042 0.6604579 -0.7234055 -0.01754873 0.6902003 -0.4978548 -0.01947104 0.867042 -0.5262362 0.08920651 0.8456464 -0.2725985 0.9192723 0.2839517 -0.9996584 0.02581924 0.004089534 -0.9705242 0.08719182 0.2246784 -0.3258222 0.9189614 0.2221487 -0.9446884 0.03799623 0.325761 -0.9990167 0.04406976 0.004852533 0.5968319 -0.01330631 0.8022561 -0.9914664 0.1093796 0.07092583 0.5992181 -0.02423238 0.800219 -0.972452 0.1120346 0.2044151 -0.9847776 0.1270188 0.1186567 -0.9981759 0.06027448 0.003448605 -0.4031009 0.5195594 0.753371 -0.9737155 0.1533895 0.1683745 -0.9510927 0.08005118 0.2983533 -0.1634616 0.7716208 0.6147208 0.6464057 0.01818966 0.762777 0.6329692 -0.01159727 0.7740902 -0.007800817 -0.6722905 0.7402464 -0.9916453 0.1211898 0.04419112 -0.955261 0.1156079 0.2722342 -0.9957033 0.09198337 0.0106815 -0.08685785 0.6798169 0.7282204 -0.9735093 0.1335531 0.1855887 -0.6980308 0.07233005 -0.7124053 -0.7784902 0.3261614 -0.5362573 -0.7768901 0.3171833 -0.5439085 -0.0234695 0.5779183 0.8157572 -0.9209442 0.04538184 0.3870431 -0.984365 0.1459731 0.09857684 -0.3836925 -0.02118057 0.9232181 -0.4309619 -0.02704 0.901965 -0.1974615 0.9539192 0.2259362 -0.3159341 -0.01333683 0.9486874 -0.34688 0.0310989 0.9373939 -0.9739616 0.1702365 0.1497275 -0.9895157 0.1435 0.01632761 -0.9572229 0.1508851 0.2468972 -0.6940754 0.06360268 -0.7170872 -0.929436 0.08926904 0.3580224 -0.9836389 0.1622104 0.07837367 -0.1200025 0.9734488 0.1949278 0.7068731 -0.01651054 0.7071478 0.6836313 -0.01965439 0.7295628 0.7028593 -0.01922714 0.711069 -0.2545911 -0.009644031 0.9670008 -0.2851364 0.04684621 0.9573415 -0.95958 0.1787806 0.2173566 -0.9341672 0.1229625 0.3349806 -0.9733389 0.1915365 0.1261956 -0.8938519 0.05145567 0.4453999 -0.9820894 0.1806702 0.05346858 0.1363905 0.565491 0.8133988 0.06238085 0.7907784 0.6089156 -0.2212617 0.06192272 0.9732466 -0.9322786 0.1083136 0.3451448 -0.0611608 0.9684412 0.241622 -0.9150475 0.1733777 0.3641818 0.7282325 -0.01620596 0.6851385 0.007660388 0.9546803 0.2975346 -0.904071 0.1015069 0.415153 -0.9710105 0.2179707 0.09812039 0.7458034 -0.01028501 0.6660866 -0.9783222 0.2059128 0.02203482 -0.9584073 0.2115008 0.1916326 -0.9195911 0.2016441 0.3371824 0.7523301 -0.01544272 0.6586055 0.6599335 -0.02096623 0.7510315 0.6797525 -0.02136337 0.7331305 -0.9106023 0.1409378 0.3885098 -0.9409934 0.2473567 0.230968 -0.9688664 0.2368299 0.07217818 0.2247114 0.6830459 0.6949483 -0.9604055 0.2302055 0.1569291 -0.1170403 -0.004120051 0.9931187 -0.8618829 0.05533063 0.5040799 -0.9408281 0.2811161 0.1892521 0.7973739 6.10383e-5 0.6034858 0.7678662 0.1294935 0.6273857 -0.9548455 0.2630731 0.1380676 0.1463075 0.9412328 0.3044258 -0.4655059 -0.02829104 0.8845925 0.3052281 0.5012871 0.8096587 -0.9625692 0.2693912 0.02981704 0.07458853 0.9839032 0.1623918 -0.9421791 0.2473556 0.2260839 -0.9189535 0.2692375 0.2881591 0.3580232 0.2872792 0.88842 -0.8752468 0.1127362 0.4703547 0.8378723 0.00112915 0.5458652 0.8112308 0.120795 0.5721129 -0.9550936 0.2768383 0.1056264 -0.8836823 0.1580282 0.4406051 0.8366898 -0.01034605 0.5475794 -0.9145339 0.2353934 0.3289648 -0.8275561 0.0591765 0.5582555 0.7745068 -0.01397758 0.6324111 0.4035913 0.1956917 0.8937668 -0.05322444 0.005127072 0.9985694 -0.02725327 0.005584895 0.9996131 -0.009674608 0.04596203 0.9988964 0.2975624 0.7742727 0.5585324 0.02356082 0.003234982 0.9997172 -0.9453428 0.323202 0.04321563 0.2078677 0.9507406 0.2299637 -0.8892071 0.1954448 0.4136571 0.3815202 0.6097731 0.694708 0.8171517 -0.0121771 0.5762941 0.8532862 0.06213706 0.5177276 0.8721404 0.002471983 0.4892495 0.4398147 0.4308725 0.7879797 0.02420133 -0.002960264 0.9997028 0.4692589 0.1856771 0.8633192 -0.9271937 0.2823911 0.2461041 -0.8430883 0.1225949 0.5236151 -0.8869876 0.1844292 0.4233662 -0.9379345 0.3136113 0.1480772 0.2818764 0.9359664 0.21098 0.3734002 0.7818056 0.4993521 0.9025415 0.005829095 0.4305635 0.8857243 0.05975633 0.4603496 0.5009142 0.3838725 0.7757106 -0.9112293 0.3071408 0.2744553 0.4481117 0.6442274 0.6198121 0.8712452 -0.007751941 0.4907869 0.5302756 0.191357 0.8259481 -0.8531274 0.1740497 0.4918133 0.3484724 0.914366 0.2061596 -0.8673905 0.2866683 0.4067619 0.0879265 0.008118152 0.9960939 0.5183629 0.5794007 0.6289633 -0.7886803 0.06171005 0.6116988 -0.9261957 0.3720897 0.06091624 -0.9162586 0.3334873 0.2219383 0.4203428 0.8674511 0.2661591 0.5623812 0.3801192 0.7343275 0.9305948 0.008301198 0.365957 0.914236 0.07577937 0.3980327 0.5881112 0.1913574 0.7858166 -0.8601004 0.2168715 0.4617294 -0.8666877 0.3366891 0.3680935 0.5786149 0.5559697 0.5967434 0.6124976 0.06674629 0.7876495 0.5104666 0.7197684 0.4704863 0.6202678 0.3588117 0.6975113 -0.8077818 0.1363902 0.573486 0.9539523 0.008209466 0.2998458 0.9418486 0.04602271 0.3328713 -0.8889555 0.237773 0.3914359 -0.8650627 0.2556582 0.431631 0.6450498 0.1844264 0.741551 -0.9146414 0.3657467 0.172222 0.470242 0.8592721 0.2013056 -0.8694008 0.3682143 0.3294854 0.6365771 0.5239599 0.5658938 0.5678085 0.6889084 0.4505539 0.6736871 0.3277487 0.6623644 -0.8245186 0.2135459 0.5239917 0.9723585 0.008270561 0.2333465 0.9550495 0.1346219 0.2641165 0.6902961 0.1508287 0.7076314 0.533412 0.8231903 0.1944979 -0.7465063 0.06607466 0.6620895 0.6869361 0.4666758 0.557075 0.7134425 0.2601439 0.6506344 -0.9014108 0.4266563 0.07364243 -0.8708111 0.4115546 0.2689067 0.628526 0.6501951 0.4268507 -0.8799644 0.4536701 0.1408773 0.7453096 0.1745393 0.643467 0.9856293 0.009583115 0.1686511 0.9784016 0.04763978 0.2011492 0.118597 0.05673491 0.9913203 0.7309675 0.369436 0.5737627 0.5837766 0.7904545 0.1854363 0.6858557 0.5916128 0.4237882 0.6390712 0.7481467 0.1785066 0.7617627 0.06427371 0.64466 -0.8220085 0.2129335 0.528168 -0.7725328 0.1639797 0.6134361 0.7660921 0.2918243 0.5726616 0.7362335 0.4690421 0.4878112 0.9943824 0.01022398 0.1053529 0.9755108 0.1742027 0.1342838 0.778179 0.1266242 0.6151454 0.7317598 0.5651857 0.3809106 0.7751247 0.3709599 0.5114395 0.7963618 0.1995635 0.5709487 -0.8740651 0.4772261 0.09091615 -0.8053105 0.340808 0.4851029 0.8052926 0.06302314 0.5895184 0.809766 0.2778773 0.5167816 0.7779627 0.4480206 0.4405128 0.9952478 -0.08175975 0.05288898 -0.6965435 0.06515884 0.7145499 0.8256407 0.1472254 0.5446488 0.7748909 0.530399 0.343833 0.6861873 0.7092902 0.1614145 0.8147659 0.340348 0.4693824 -0.7928695 0.272052 0.5452942 0.8124817 0.2055162 0.5455609 0.7347034 0.6617 0.1495471 0.8429307 0.05755853 0.5349345 0.8174518 0.4107859 0.4037665 0.8430981 0.2441245 0.4791544 0.8482307 0.1280296 0.5139192 -0.7199495 0.1504294 0.6775277 0.8116976 0.4871528 0.3222252 -0.8590773 0.4624533 0.21937 0.8613838 0.1942874 0.4693297 0.8500828 0.3100449 0.4257128 0.774316 0.6224194 0.1141437 0.8690205 0.1221969 0.479449 0.8682342 0.05636852 0.4929423 -0.8396195 0.5342729 0.09793674 0.8531848 0.3719027 0.3657378 0.8795834 0.1874161 0.4372739 0.8806095 0.1221695 0.4578229 0.8676332 0.251479 0.4289183 0.8072607 0.528896 0.2619149 0.8746476 0.3065341 0.3755378 0.9016543 0.1295838 0.4125868 0.8876122 0.2488818 0.3875597 0.8989847 0.04999107 0.4351177 0.8611735 0.4092382 0.3015038 0.8398022 0.4579435 0.2915823 0.8192612 0.5607624 0.1198189 0.8963204 0.1999927 0.3957433 0.912586 0.04721319 0.4061499 0.8749344 0.3604053 0.3234156 0.9116237 0.1095315 0.3961631 0.8531637 0.4625182 0.2412236 0.9092307 0.1939195 0.3683678 0.890035 0.3075737 0.336506 0.9003634 0.2605167 0.3485356 0.922701 0.09751027 0.3729808 0.9257394 0.04446649 0.375539 0.8887708 0.3608857 0.2825742 0.8717576 0.4165905 0.2578588 0.9211753 0.177527 0.3462952 0.9370725 0.04010266 0.3468242 0.923813 0.2244679 0.3101351 0.8570953 0.5013354 0.1185357 0.8629006 0.5053679 0.002411007 0.8354973 0.5321656 0.1369097 0.3301209 -0.1073953 0.9378094 0.2953343 -0.5413191 0.7872429 0.4635253 -0.5713801 0.6772511 0.5725393 -0.6339133 0.5199548 0.341324 -0.6037564 0.7203999 0.4901053 -0.03119045 0.8711051 0.5970666 0.03851449 0.8012667 0.3571961 -0.6194776 0.6990411 0.656652 0.01803684 0.753978 0.7508519 0.05288892 0.6583497 0.2478202 -0.5962336 0.7636038 0.4465911 -0.171092 0.8782278 0.006622672 0.646888 0.7625563 0.00689727 0.6460282 0.7632825 0.01052916 0.6339171 0.7733294 0.9458777 0.3239856 0.01868063 0.02157706 0.1531454 0.9879682 0.4385638 -0.1757918 0.8813394 0.3411735 -0.7051755 0.621553 0.7767528 -0.04431432 0.6282448 0.6492282 -0.08844369 0.755434 0.6421796 -0.09067171 0.7611729 0.05243247 0.1246416 0.9908155 0.2943258 -0.7156719 0.6333925 0.6676403 -0.1252817 0.7338672 0.8589027 -0.06598258 0.5078705 0.09094768 0.1500027 0.9844937 0.169688 -0.6243054 0.7625278 0.7453368 -0.158455 0.6475841 0.5894596 -0.1862319 0.7860376 0.1909924 0.4722317 0.8605342 0.6541181 -0.2143669 0.7253804 0.6492418 -0.2169327 0.7289893 0.1394731 0.08646106 0.9864441 0.2248952 -0.7793965 0.5847763 0.9775311 -0.07840734 0.1956659 0.4665473 -0.2759959 0.8403333 0.08533191 -0.5693679 0.8176423 0.3637228 0.5682597 0.7380967 0.3030821 0.4552488 0.8371916 0.9961441 -0.08604705 0.01711815 0.4273204 -0.3086816 0.8497723 0.1722496 -0.006225883 0.9850337 0.3669047 0.5742538 0.7318562 0.3666273 0.5734255 0.7326443 0.108923 -0.6592453 0.7439971 0.9900001 -0.1339594 -0.04421246 0.4618436 -0.334549 0.8214486 0.4533559 -0.3339971 0.8263864 0.7409458 -0.3338499 0.5827037 0.4888806 0.8723493 0.001556456 0.43355 0.9011284 0.001464843 0.4540975 0.8889067 -0.0603367 0.6014198 -0.3772225 0.7042709 0.06872797 -0.6759573 0.7337291 0.4738984 0.53045 0.7028821 0.471135 0.5254299 0.7084881 0.577753 0.7530229 0.3148938 0.5788255 0.75663 0.3040925 0.7253519 -0.3810333 0.5733048 0.2869325 0.1441652 0.9470407 0.0418415 -0.7793328 0.6252119 0.5334788 -0.4229071 0.7324957 0.6322066 0.7631926 0.1336119 0.3116613 0.1131039 0.9434378 0.3047024 0.1047414 0.946671 0.5386338 0.403281 0.7397555 0.005707085 -0.5823109 0.8129463 0.005859553 -0.5568439 0.8305965 0.5587249 -0.4367985 0.7050061 0.5150677 0.2960029 0.8044176 0.5683037 0.3460606 0.7465071 0.4610866 -0.4600185 0.7588031 0.5566152 0.2704641 0.7855117 0.5239866 -0.5160516 0.6775906 0.6422493 -0.5570392 0.5265199 0.331866 -0.03415095 0.9427082 0.7548341 0.634588 0.1659029 0.7675912 0.6329094 0.101141 0.7012798 0.7128773 0.003570795 0.502507 -0.5463944 0.6700297 0.3493579 -0.05084562 0.9356089 0.6078827 -0.5961328 0.5245041 0.7898388 0.6132848 0.006042778 0.5111438 0.09753662 0.8539431 0.02802574 0.3448354 -0.9382448 -0.5755049 0.2093633 0.7905448 -0.5868647 0.1505845 0.795559 -0.6721267 0.2976243 0.6779864 -0.398979 -0.4969155 0.7706432 -0.5091521 -0.5791937 0.6366308 -0.6596983 0.4039187 0.6337571 -0.5870665 0.3628423 0.7236703 -0.5390247 0.2189731 0.8133285 -0.4593753 -0.4707894 0.7532144 -0.5134482 0.2836722 0.8098772 -0.5538303 0.4194241 0.7192744 -0.5162073 -0.4794313 0.7097012 -0.4895841 0.3396753 0.8030742 -0.4588328 -0.419615 0.7831959 -0.2427216 0.03930908 0.9692993 -0.02386605 -0.7930127 0.6087374 -0.5465689 0.8198076 0.1708161 -0.5799818 0.7975512 0.1659314 -0.6056243 0.7957288 0.005920708 -0.5378468 -0.3829293 0.7510567 -0.05130285 -0.6371203 0.7690551 -0.5577597 -0.3776078 0.7391322 -0.5051519 0.7415222 0.4415501 -0.4283677 0.8336325 0.3486515 -0.09015274 -0.7614157 0.6419647 -0.6399042 -0.3020828 0.7065895 -0.4603542 0.8607683 0.2171453 -0.1325156 -0.7554185 0.6417029 -0.06509673 -0.1879351 0.9800218 -0.05401933 -0.2133002 0.9754922 -0.1943472 -0.834765 0.5151665 -0.3803607 0.8910689 0.2476327 -0.3777371 0.7943863 0.4756734 -0.1268097 -0.6157062 0.7777052 -0.3394622 -0.3102556 0.8879792 -0.6929761 -0.2545938 0.6745117 -0.2056983 -0.717411 0.6655897 -0.0873143 -0.02139365 0.9959511 -0.2025542 0.3250573 0.9237476 -0.9971018 0.07596296 0.004211664 -0.9695816 -0.0491352 0.239786 -0.2983592 0.9544078 0.009369492 -0.6418893 -0.2043595 0.7390639 -0.1458224 -0.5792611 0.8019928 -0.7884264 -0.1233881 0.602627 -0.6628249 -0.1548259 0.7325928 -0.2460482 -0.690931 0.6797608 -0.7789779 -0.07562714 0.6224743 -0.06842279 0.001892149 0.9976546 -0.6517643 -0.1154839 0.7495778 -0.7748165 -0.02282822 0.6317739 -0.6025111 -0.0842331 0.7936531 -0.03140425 0.002258419 0.9995042 -0.1677043 -0.5310382 0.8305865 -0.6444733 -0.01525956 0.7644746 -0.03244149 0.1223804 0.991953 -0.2050896 -0.5653698 0.7989339 -0.6394064 -0.04385596 0.7676173 0.01083415 0.6329596 0.774109 -0.5584948 -0.04834181 0.8280982 -0.7441511 0.1407243 0.6530206 -0.3717223 -0.6381538 0.6742273 -0.3159652 -0.5891425 0.7436916 -0.5080786 -0.003296017 0.8613044 -0.3351343 -0.5556069 0.7609114 -0.8776159 0.3619026 0.3143516 -0.4981728 -0.00875914 0.8670336 -0.6987989 0.2442149 0.6723387 -0.6123753 0.1129224 0.7824609 -0.6072958 0.1061142 0.7873573 -0.507968 -0.6064851 0.6116735 3.84549e-5 0 -1 -2.62537e-5 0 -1 4.93486e-6 0 -1 -1.47145e-5 0 -1 3.78039e-6 0 -1 -9.93967e-6 0 -1 1.46413e-5 0 -1 -3.76573e-5 0 -1 7.62494e-6 0 -1 5.0022e-6 0 -1 -1.45732e-5 0 -1 -1.53975e-5 0 -1 1.00567e-5 0 -1 -7.77629e-6 0 -1 5.05398e-6 0 -1 7.86291e-6 0 -1 5.07346e-6 0 -1 7.21865e-6 0 -1 3.97791e-6 0 -1 -5.08813e-6 0 -1 -2.01354e-6 0 -1 -3.48094e-5 0 -1 1.8376e-5 0 -1 -3.19079e-6 0 -1 1.96659e-5 0 -1 -1.79768e-6 0 -1 3.31215e-5 0 -1 5.10527e-6 0 -1 -2.06681e-5 0 -1 1.34933e-6 0 -1 3.26684e-5 0 -1 1.54612e-5 0 -1 1.11934e-5 0 -1 -3.60933e-6 0 -1 -1.01469e-5 0 -1 -4.53912e-6 0 -1 -3.61931e-6 0 -1 -9.19489e-6 0 -1 3.62966e-6 0 -1 3.11066e-5 0 -1 -2.01134e-5 0 -1 -1.86138e-5 0 -1 7.28661e-6 0 -1 -9.41544e-6 0 -1 -1.0981e-5 0 -1 -9.51561e-6 0 -1 7.35721e-6 0 -1 -4.80765e-6 0 -1 3.00077e-5 0 -1 3.70074e-6 0 -1 -1.95798e-5 0 -1 4.85244e-6 0 -1 1.958e-5 0 -1 2.62537e-5 0 -1 -3.84549e-5 0 -1 -1.47158e-5 0 -1 -3.78039e-6 0 -1 -7.62494e-6 0 -1 3.76573e-5 0 -1 1.53975e-5 0 -1 1.45195e-5 0 -1 7.77629e-6 0 -1 -5.05357e-6 0 -1 7.23757e-6 0 -1 -7.86291e-6 0 -1 -5.07285e-6 0 -1 -3.97791e-6 0 -1 2.01354e-6 0 -1 -2.55045e-6 0 -1 3.59852e-6 0 -1 -1.8376e-5 0 -1 3.48094e-5 0 -1 -1.96659e-5 0 -1 -3.31215e-5 0 -1 -2.24908e-6 0 -1 -3.26684e-5 0 -1 2.06681e-5 0 -1 -1.80139e-6 0 -1 -1.54612e-5 0 -1 -1.11934e-5 0 -1 1.01457e-5 0 -1 -3.61878e-6 0 -1 4.53912e-6 0 -1 1.01071e-5 0 -1 -1.08896e-5 0 -1 9.19489e-6 0 -1 1.86138e-5 0 -1 3.66054e-6 0 -1 9.41544e-6 0 -1 1.47159e-5 0 -1 9.51561e-6 0 -1 -3.00075e-5 0 -1 4.80765e-6 0 -1 -4.85244e-6 0 -1 0.834063 0.5516676 -0.001403868 0.7915416 0.6111131 -0.001617491 0.8945119 0.4470423 0.001312315 -0.9998088 0.01955598 0 0.8720757 0.4893695 -0.001159667 0.9225596 0.385852 0.00137335 0.9055033 0.4243383 -9.15575e-4 0.9470154 0.3211856 0.00137335 0.934065 0.357103 -5.49342e-4 0.9668395 0.2553824 0.00112915 0.9576194 0.2880366 -2.74669e-4 0.9820808 0.1884567 0.001190245 0.9760649 0.2174797 -2.74673e-4 -0.9890069 0.1478658 0.001037597 -0.997325 0.07309257 -7.01933e-4 0.992475 0.1224418 0.00112915 0.9979341 0.06424206 8.54526e-4 -0.9764638 0.2156796 9.15578e-4 0.9893292 0.1456969 -5.49339e-4 -0.9893562 0.1455132 -6.40893e-4 -0.9591106 0.2830302 8.85042e-4 -0.9760854 0.217386 -8.2401e-4 -0.936796 0.3498755 7.93507e-4 -0.9576082 0.2880729 -9.15583e-4 -0.9094444 0.4158254 5.49347e-4 -0.9340403 0.3571662 -0.001068115 -0.8776797 0.4792476 3.66234e-4 -0.9054741 0.4244 -0.001220762 0.9973316 0.0730015 -7.62976e-4 -0.841838 0.5397297 7.93496e-4 -0.8720683 0.4893825 -0.001312315 -0.8016808 0.5977509 0.001312315 -0.8340024 0.5517594 -0.001342833 -0.4575339 0.8891881 -0.002716124 -0.3914952 0.9201723 -0.003814816 -0.3231658 0.9463334 -0.004150569 -0.2528817 0.9674876 -0.004303157 -0.1815267 0.9833815 -0.003021359 -0.1105412 0.9938643 -0.003814876 -0.03680562 0.9993067 -0.005615413 -0.7573933 0.6529568 0.001678526 0.03744667 0.9992809 -0.005951166 0.1812199 0.983434 -0.004119992 0.2531895 0.9674059 -0.004577875 0.3233208 0.9462794 -0.004394769 0.3914961 0.9201744 -0.003143429 0.6397209 0.7686051 -0.001831173 -0.7914636 0.6112151 -0.001281797 0.6525949 0.7577034 0.00238049 0.6012226 0.7990767 0.002838253 0.5456927 0.8379801 0.003021419 -0.7099075 0.7042921 0.002014219 0.3740389 0.9274066 0.003479123 0.3145595 0.9492331 0.003021359 0.2532762 0.9673874 0.003570675 0.1914473 0.9814997 0.002533078 0.1279958 0.9917694 0.0032655 0.06408983 0.9979397 0.002990841 -0.044649 0.9990028 -3.05188e-5 -0.0937547 0.9955948 0.001037597 -0.1614478 0.9868765 0.003082454 -0.3502096 0.9366695 -0.001892149 -0.7447881 0.6673001 -0.001190245 -0.6594855 0.7517139 0.00225836 -0.6940634 0.7199131 -0.00112915 -0.6395055 0.7687861 -7.93507e-4 -0.5820029 0.8131865 -6.71424e-4 -0.5214481 0.8532814 -0.001678526 -0.2298396 0.9732249 0.002655148 0.002960324 0.9999954 8.5454e-4 0.1100524 0.9939203 -0.003326594 0.4578748 0.889015 -0.001739561 0.5207248 0.8537238 -0.001220762 0.5817886 0.8133381 -0.001770079 0.6940779 0.7198976 -0.001861691 0.7448162 0.6672675 -0.001770079

681 19 3171 685 685 683 4326 681

608 0 699 1 0 2 0 2 1 3 608 0 4012 4 608 0 1 3 4012 4 1 3 3869 5 616 6 4012 4 3869 5 3869 5 3 7 616 6 4169 8 616 6 3 7 3 7 5 9 4169 8 676 10 4169 8 5 9 3205 11 3117 12 5 9 676 10 5 9 3117 12 926 13 3155 13 6 13 3117 12 3205 11 3154 14 3154 14 931 15 7 16 678 17 3117 12 7 16 3117 12 3154 14 7 16 931 15 3005 18 7 16 7 16 3005 18 8 19 678 17 7 16 8 19 4332 20 678 17 8 19 8 19 3005 18 3004 21 944 22 3006 22 9 22 4232 23 4332 20 3007 24 8 19 3004 21 4332 20 3004 21 3007 24 4332 20 3007 24 3376 25 4232 23 4232 23 3376 25 3072 26 3072 26 3376 25 3073 27 3376 25 10 28 3073 27 3073 27 10 28 3171 29 3171 29 10 28 13 30 3354 31 869 32 556 33 791 34 14 35 637 36 632 37 791 34 637 36 3171 29 13 30 16 38 13 30 15 39 16 38 649 40 637 36 14 35 14 35 17 41 649 40 16 38 15 39 665 42 15 39 18 43 665 42 3770 44 665 42 18 43 18 43 20 45 3770 44 649 40 17 41 21 46 4324 47 3770 44 20 45 20 45 979 48 4324 47 21 46 17 41 959 49 4324 47 979 48 22 50 21 46 959 49 656 51 979 48 23 52 22 50 22 50 23 52 24 53 959 49 27 54 656 51 22 50 24 53 696 55 696 55 24 53 26 56 25 57 696 55 26 56 26 56 24 53 25 57 24 53 28 58 25 57 3767 59 656 51 27 54 3961 60 4329 61 923 62 28 58 923 62 4329 61 25 57 28 58 4329 61 621 63 3961 60 29 64 3961 60 923 62 29 64 27 54 3062 65 3767 59 29 64 927 66 621 63 621 63 927 66 30 67 927 66 4133 68 30 67 4133 68 3701 69 624 70 30 67 4133 68 624 70 624 70 3701 69 633 71 3701 69 3560 72 633 71 641 73 633 71 3560 72 3560 72 3561 74 641 73 641 73 3561 74 33 75 3561 74 4217 76 33 75 33 75 4217 76 654 77 4217 76 957 78 654 77 654 77 957 78 659 79 957 78 4145 80 659 79 659 79 4145 80 661 81 3767 59 3062 65 4319 82 4145 80 4237 83 661 81 661 81 4237 83 667 84 4237 83 38 85 667 84 3767 59 4319 82 669 86 667 84 38 85 37 87 38 85 41 88 37 87 37 87 41 88 42 89 41 88 3354 31 42 89 4131 90 669 86 43 91 669 86 4319 82 43 91 42 89 3354 31 44 92 44 92 3354 31 45 93 3354 31 556 33 45 93 4131 90 43 91 47 94 48 95 4131 90 47 94 47 94 50 96 48 95 51 97 48 95 50 96 50 96 49 98 51 97 51 97 49 98 699 1 49 98 0 2 699 1 3554 99 3204 100 3654 101 4111 102 720 103 453 104 52 105 720 103 4111 102 4177 106 3204 100 3554 99 4177 106 3203 107 3204 100 4024 108 729 109 3203 107 3448 110 52 105 4111 102 4024 108 3203 107 4177 106 3448 110 726 111 52 105 3449 112 726 111 3448 110 4025 113 729 109 4024 108 4025 113 3721 114 729 109 3449 112 3008 115 726 111 58 116 56 117 458 118 3980 119 3008 115 3449 112 4027 120 458 118 4035 121 4025 113 3879 122 3721 114 3980 119 3009 123 3008 115 3964 124 3009 123 3980 119 63 125 3878 126 4026 127 63 125 4026 127 59 128 3890 129 3441 130 3877 131 3964 124 3079 132 3009 123 64 133 3079 132 3964 124 3892 134 3442 135 3441 130 3892 134 3441 130 3890 129 64 133 3127 136 3079 132 66 137 3127 136 64 133 67 138 3442 135 3892 134 67 138 65 139 3442 135 66 137 3128 140 3127 136 3360 141 3128 140 66 137 70 142 736 143 65 139 70 142 65 139 67 138 3360 141 3728 144 3128 140 3360 141 3359 145 3728 144 70 142 67 138 468 146 3303 147 4037 148 3304 149 70 142 69 150 736 143 433 151 69 150 70 142 429 152 433 151 70 142 433 151 73 153 69 150 432 154 73 153 433 151 75 155 71 156 3723 157 434 158 75 155 3723 157 432 154 72 159 73 153 3835 160 72 159 432 154 3844 161 3135 162 75 155 3844 161 75 155 434 158 3835 160 3801 163 72 159 4144 164 3801 163 3835 160 3080 165 3134 166 3135 162 3080 165 3135 162 3844 161 4144 164 77 167 3801 163 3045 168 77 167 4144 164 3081 169 3134 166 3080 165 3045 168 82 170 77 167 3081 169 3411 171 3134 166 3046 172 82 170 3045 168 3179 173 3168 174 3411 171 3179 173 3411 171 3081 169 3046 172 3499 175 82 170 3421 176 3499 175 3046 172 3084 177 92 178 3168 174 3084 177 3168 174 3179 173 4245 179 708 180 3499 175 4245 179 3499 175 3421 176 440 181 92 178 3084 177 91 182 708 180 4245 179 440 181 89 183 92 178 3257 184 91 182 4245 179 4101 185 95 186 89 183 4101 185 89 183 440 181 3257 184 93 187 91 182 3258 188 93 187 3257 184 3502 189 93 187 3258 188 444 190 95 186 4101 185 444 190 716 191 95 186 4312 192 3502 189 3258 188 443 193 716 191 444 190 4312 192 3454 194 3502 189 443 193 718 195 716 191 448 196 3454 194 4312 192 3555 197 3654 101 718 195 3555 197 718 195 443 193 3455 198 3454 194 448 196 453 104 3455 198 448 196 3554 99 3654 101 3555 197 720 103 3455 198 453 104 4165 199 131 200 2794 201 2896 202 126 203 3818 204 3663 205 3636 206 2742 207 4181 208 2799 209 4173 210 4181 208 4165 199 2799 209 2961 211 3763 212 3461 213 107 214 3461 213 3536 215 107 214 2961 211 3461 213 3806 216 3938 217 3906 218 3806 216 3897 219 3938 217 3601 220 2742 207 3672 221 3601 220 3663 205 2742 207 2900 222 107 214 3536 215 4331 223 2790 224 4313 225 2900 222 3536 215 3221 226 4331 223 2896 202 2790 224 3572 227 2900 222 3221 226 3572 227 3221 226 3220 228 3713 229 3906 218 3907 230 3722 231 3572 227 3220 228 3722 231 3220 228 3174 232 3713 229 3806 216 3906 218 3445 233 3601 220 3672 221 3445 233 3672 221 117 234 3406 235 3722 231 3174 232 3406 235 3174 232 2765 236 4328 237 4313 225 2800 238 4328 237 2800 238 3367 239 3345 240 3406 235 2765 236 4328 237 4331 223 4313 225 3345 240 2765 236 4327 241 3206 242 3907 230 3795 243 3344 244 3345 240 4327 241 3206 242 3713 229 3907 230 3344 244 4327 241 2770 245 3197 246 3344 244 2770 245 3197 246 2770 245 2709 247 3116 248 3367 239 3106 249 3198 250 3197 246 2709 247 3195 251 3795 243 2833 252 3198 250 2709 247 3931 253 3195 251 3206 242 3795 243 3195 251 2833 252 3924 254 3116 248 4328 237 3367 239 3435 255 3445 233 117 234 4243 256 3198 250 3931 253 4243 256 3931 253 3932 257 3435 255 117 234 3385 258 3196 259 3924 254 3420 260 4318 261 4243 256 3932 257 4318 261 3932 257 3941 262 3196 259 3195 251 3924 254 3039 263 3106 249 3105 264 3039 263 3116 248 3106 249 4280 265 4318 261 3941 262 4280 265 3941 262 3942 266 124 267 3420 260 3324 268 3230 269 3435 255 3385 258 124 267 3196 259 3420 260 3230 269 3385 258 3386 270 3230 269 3386 270 3392 271 4279 272 4280 265 3942 266 4279 272 3942 266 4271 273 3040 274 3105 264 4146 275 3410 276 3324 268 3156 277 3040 274 3039 263 3105 264 3410 276 124 267 3324 268 3611 278 4279 272 4271 273 3611 278 4271 273 4244 279 2867 280 3156 277 3157 281 126 203 3392 271 3818 204 2867 280 3410 276 3156 277 126 203 3230 269 3392 271 130 282 4146 275 4099 283 130 282 3040 274 4146 275 3398 284 2867 280 3157 281 3219 285 3611 278 4244 279 3398 284 3157 281 3298 286 3219 285 4244 279 4143 287 2861 288 3298 286 3299 289 2861 288 3398 284 3298 286 3897 219 4099 283 3938 217 129 290 3219 285 4143 287 3897 219 130 282 4099 283 129 290 4143 287 3636 206 131 200 3299 289 2794 201 131 200 2861 288 3299 289 4165 199 2794 201 2799 209 2896 202 3818 204 2790 224 3663 205 129 290 3636 206 2892 291 3021 292 1055 292 2898 293 3160 294 1009 293 2897 295 1053 291 3271 295 2897 295 2892 291 1053 291 3296 296 3271 295 3270 296 3172 297 4058 298 1051 297 3296 296 3270 296 1049 299 3296 296 2897 295 3271 295 3172 297 4082 298 4058 298 3212 299 3296 296 1049 299 3173 300 1051 297 4124 300 3173 300 3172 297 1051 297 3211 301 1049 299 1043 301 3211 301 3212 299 1049 299 3342 302 4124 300 1057 302 3342 302 3173 300 4124 300 3253 303 1043 301 3704 303 3253 303 3211 301 1043 301 2865 304 3342 302 1057 302 2865 304 1057 302 3423 304 139 305 3704 303 1040 305 3553 306 3423 304 3422 306 139 305 3253 303 3704 303 3553 306 2865 304 3423 304 3553 306 3422 306 3363 307 142 308 1040 305 3997 308 2871 307 3363 307 3272 309 142 308 139 305 1040 305 2871 307 3553 306 3363 307 3665 309 3272 309 3273 310 3631 311 3997 308 3998 311 3665 309 2871 307 3272 309 3631 311 3998 311 3967 312 3631 311 142 308 3997 308 3664 310 3273 310 3610 313 3575 312 3631 311 3967 312 3664 310 3665 309 3273 310 3575 312 3967 312 3966 314 149 315 3575 312 3966 314 2875 313 3664 310 3610 313 149 315 3966 314 1030 316 3615 316 149 315 1030 316 3639 317 3610 313 3873 317 3639 317 2875 313 3610 313 3616 318 3615 316 1030 316 3597 319 3873 317 3874 319 3597 319 3874 319 3855 320 3616 318 1030 316 4164 318 3597 319 3639 317 3873 317 3596 320 3855 320 3854 321 2915 322 3616 318 4164 318 3596 320 3597 319 3855 320 2915 322 4164 318 3545 322 158 321 3854 321 3888 323 158 321 3596 320 3854 321 3510 324 2915 322 3545 322 3510 324 3545 322 159 324 3510 324 159 324 3340 325 3498 326 158 321 3888 323 161 325 3510 324 3340 325 165 327 3888 323 160 327 165 327 3498 326 3888 323 2907 328 161 325 3340 325 2907 328 3340 325 3275 328 166 329 160 327 1061 329 166 329 1061 329 167 330 4061 331 3275 328 3274 331 166 329 165 327 160 327 4061 331 2907 328 3275 328 4061 331 3274 331 169 332 171 330 166 329 167 330 4079 332 169 332 3104 333 4079 332 4061 331 169 332 3506 334 167 330 172 334 3193 333 3104 333 3103 335 3506 334 171 330 167 330 3193 333 4079 332 3104 333 2903 335 3193 333 3103 335 3021 292 172 334 1055 292 3021 292 3506 334 172 334 3160 294 3103 335 3207 294 3160 294 3207 294 1009 293 3160 294 2903 335 3103 335 2892 291 1055 292 1053 291 4009 336 3805 337 2018 338 4011 339 4009 336 3820 340 4009 336 2018 338 3820 340 4011 339 3820 340 176 341 179 342 4011 339 176 341 179 342 176 341 177 343 1069 344 179 342 177 343 1069 344 177 343 1068 345 3600 346 3804 347 4010 348 178 349 174 349 175 349 179 342 3599 350 4011 339 3295 351 3632 351 3749 351 180 352 3717 353 3267 352 180 352 181 353 3717 353 183 354 182 354 703 293 183 354 3584 355 182 354 3384 356 3267 352 186 356 185 293 183 354 703 293 3384 356 186 356 3255 357 3384 356 180 352 3267 352 3035 358 188 298 3216 358 3035 358 187 298 188 298 3688 357 3384 356 3255 357 3784 359 3255 357 3287 359 3784 359 3287 359 3288 360 3036 361 3216 358 3215 361 3784 359 3688 357 3255 357 3036 361 3035 358 3216 358 1056 362 3215 361 4172 362 3819 360 3784 359 3288 360 1056 362 3036 361 3215 361 3756 363 3288 360 3330 363 3865 364 4172 362 234 364 3756 363 3819 360 3288 360 3865 364 1056 362 4172 362 3755 365 3330 363 196 366 192 367 234 364 235 367 3755 365 3756 363 3330 363 192 367 3865 364 234 364 195 368 235 367 239 368 4023 369 196 366 197 370 4023 369 197 370 3841 371 195 368 192 367 235 367 4023 369 3755 365 196 366 4022 371 4023 369 3841 371 4057 372 239 368 4140 372 4057 372 4140 372 4141 373 4057 372 195 368 239 368 4120 374 4022 371 3841 371 4120 374 3841 371 3842 374 4100 373 4057 372 4141 373 4120 374 3842 374 4287 375 4291 376 4120 374 4287 375 4084 377 4141 373 242 377 4291 376 4287 375 240 378 4084 377 4100 373 4141 373 3993 378 4291 376 240 378 4085 379 242 377 4002 379 3993 378 240 378 3962 380 4085 379 4084 377 242 377 205 380 3993 378 3962 380 3798 381 4002 379 207 381 3798 381 207 381 4277 382 3798 381 4085 379 4002 379 4005 383 205 380 3962 380 4005 383 3962 380 212 383 4005 383 212 383 214 384 3799 382 4277 382 4167 385 3799 382 3798 381 4277 382 1027 384 4005 383 214 384 1027 384 214 384 217 386 1066 385 4167 385 3075 387 1066 385 3799 382 4167 385 216 386 1027 384 217 386 3900 387 3075 387 3076 388 3900 387 1066 385 3075 387 4139 389 216 386 217 386 4139 389 217 386 4090 389 218 388 3900 387 3076 388 1019 390 4139 389 4090 389 1019 390 4090 389 4089 390 3812 391 3076 388 250 391 3812 391 218 388 3076 388 1020 392 4089 390 222 392 1020 392 1019 390 4089 390 220 393 250 391 3149 393 220 393 3812 391 250 391 1016 394 222 392 3407 394 1016 394 3407 394 3346 395 1016 394 1020 392 222 392 223 396 3149 393 3166 396 223 396 3166 396 3717 353 223 396 220 393 3149 393 1014 395 1016 394 3346 395 181 353 223 396 3717 353 3584 355 3346 395 3347 355 3584 355 3347 355 182 354 3584 355 1014 395 3346 395 499 397 255 398 4262 399 227 400 255 398 499 397 503 401 227 400 499 397 231 402 501 403 3727 404 229 405 227 400 503 401 231 402 3557 406 501 403 3355 407 229 405 503 401 228 408 229 405 3355 407 230 409 3727 404 3896 410 230 409 231 402 3727 404 191 411 230 409 3896 410 3732 412 228 408 3355 407 196 366 228 408 3732 412 4255 413 191 411 3896 410 3645 414 196 366 3732 412 3837 415 191 411 4255 413 197 370 196 366 3645 414 3266 416 3837 415 4255 413 3646 417 197 370 3645 414 236 418 197 370 3646 417 3838 419 3837 415 3266 416 4191 420 236 418 3646 417 3265 421 3838 419 3266 416 238 422 236 418 4191 420 3846 423 3838 419 3265 421 4209 424 238 422 4191 420 3289 425 3846 423 3265 421 201 426 238 422 4209 424 198 427 3846 423 3289 425 4151 428 201 426 4209 424 3037 429 198 427 3289 425 3963 430 201 426 4151 428 203 431 198 427 3037 429 4152 432 3963 430 4151 428 3038 433 203 431 3037 429 211 434 3963 430 4152 432 4003 435 203 431 3038 433 3667 436 211 434 4152 432 3356 437 4003 435 3038 433 4215 438 211 434 3667 436 206 439 4003 435 3356 437 3668 440 4215 438 3667 436 4201 441 4215 438 3668 440 525 442 206 439 3356 437 529 443 4201 441 3668 440 4263 444 206 439 525 442 4128 445 4201 441 529 443 3242 446 4263 444 525 442 480 447 4128 445 529 443 208 448 4263 444 3242 446 244 449 4128 445 480 447 3241 450 208 448 3242 446 483 451 244 449 480 447 215 452 208 448 3241 450 219 453 244 449 483 451 3524 454 215 452 3241 450 3034 455 219 453 483 451 247 456 215 452 3524 454 3408 457 219 453 3034 455 3071 458 3408 457 3034 455 482 459 247 456 3524 454 221 460 3408 457 3071 458 249 461 247 456 482 459 3826 462 221 460 3071 458 3551 463 249 461 482 459 225 464 221 460 3826 462 3148 465 249 461 3551 463 3180 466 225 464 3826 462 486 467 3148 465 3551 463 226 468 225 464 3180 466 251 469 3148 465 486 467 3181 470 226 468 3180 466 3122 471 251 469 486 467 3657 472 226 468 3181 470 252 473 251 469 3122 471 4155 474 3657 472 3181 470 3393 475 3657 472 4155 474 3125 476 252 473 3122 471 224 477 252 473 3125 476 497 478 224 477 3125 476 254 479 224 477 497 478 4189 480 254 479 497 478 3254 481 254 479 4189 480 4262 399 3254 481 4189 480 255 398 3254 481 4262 399 4129 482 3137 483 3381 484 4283 485 3281 486 260 487 4283 485 260 487 3969 488 4283 485 4051 489 3281 486 3724 490 4283 485 3969 488 4267 491 3381 484 265 492 4267 491 4129 482 3381 484 4268 493 265 492 266 357 4219 494 4028 495 3973 496 4268 493 4267 491 265 492 4219 494 68 497 4028 495 3306 498 266 357 298 359 3306 498 298 359 3097 499 3306 498 4268 493 266 357 3431 500 3973 496 3115 501 3431 500 3115 501 3077 502 3431 500 4219 494 3973 496 3621 503 3306 498 3097 499 3432 504 3431 500 3077 502 3622 505 3097 499 3096 506 3622 505 3621 503 3097 499 3944 507 3432 504 3077 502 3944 507 3077 502 3078 508 3692 509 3096 506 273 510 3692 509 273 510 277 511 3606 512 3078 508 3307 513 3692 509 3622 505 3096 506 3606 512 3944 507 3078 508 3607 514 3307 513 3353 515 3946 516 3692 509 277 511 3607 514 3353 515 280 517 3607 514 3606 512 3307 513 3995 518 3946 516 277 511 3995 518 277 511 3489 519 517 520 280 517 282 521 517 520 3607 514 280 517 3996 522 3995 518 3489 519 3996 522 3489 519 3490 523 4018 524 517 520 282 521 3996 522 3490 523 3984 525 3790 526 3996 522 3984 525 518 527 282 521 3247 528 3790 526 3984 525 3985 529 518 527 4018 524 282 521 3789 530 3790 526 3985 529 4150 531 3247 528 322 532 3789 530 3985 529 3768 533 4150 531 518 527 3247 528 3832 534 3789 530 3768 533 3832 534 3768 533 321 535 3760 536 322 532 323 537 3760 536 323 537 327 538 3760 536 4150 531 322 532 3829 539 3832 534 321 535 3829 539 321 535 3743 540 3453 541 327 538 3101 542 3453 541 3760 536 327 538 3917 543 3829 539 3743 540 3917 543 3743 540 3670 544 3389 545 3101 542 3102 546 3389 545 3453 541 3101 542 3415 547 3917 543 3670 544 3302 548 3102 546 4198 549 3302 548 3389 545 3102 546 288 550 3415 547 3670 544 288 550 3670 544 329 551 288 550 329 551 289 552 484 553 3302 548 4198 549 485 554 288 550 289 552 488 555 4198 549 3391 556 488 555 484 553 4198 549 293 557 289 552 290 558 293 557 290 558 294 559 293 557 485 554 289 552 3396 560 3391 556 292 561 3396 560 292 561 3446 562 3396 560 488 555 3391 556 3153 563 293 557 294 559 3136 564 3396 560 3446 562 3895 565 294 559 3282 565 3895 565 3282 565 3281 486 3895 565 3153 563 294 559 3137 483 3446 562 4162 566 3137 483 4162 566 3381 484 3137 483 3136 564 3446 562 4051 489 3895 565 3281 486 3240 567 3464 568 3239 569 3872 570 3464 568 3240 567 3397 571 3872 570 3240 567 300 572 3648 573 301 574 299 575 3872 570 3397 571 300 572 427 576 3648 573 3159 577 299 575 3397 571 270 578 299 575 3159 577 302 579 301 574 3735 580 302 579 300 572 301 574 269 581 302 579 3735 580 3158 582 270 578 3159 577 3619 583 270 578 3158 582 3593 584 269 581 3735 580 3706 585 3619 583 3158 582 304 586 269 581 3593 584 3620 587 3619 583 3706 585 305 588 304 586 3593 584 3261 589 3620 587 3706 585 279 590 3620 587 3261 589 278 591 304 586 305 588 3262 592 279 590 3261 589 306 593 278 591 305 588 309 594 279 590 3262 592 310 595 278 591 306 593 307 596 309 594 3262 592 308 597 310 595 306 593 311 598 309 594 307 596 3352 599 310 595 308 597 312 600 311 598 307 596 3535 601 3352 599 308 597 314 602 311 598 312 600 3246 603 3352 599 3535 601 313 604 314 602 312 600 315 605 3246 603 3535 601 317 606 314 602 313 604 316 607 3246 603 315 605 3514 608 317 606 313 604 3903 609 316 607 315 605 3769 610 317 606 3514 608 3785 611 316 607 3903 609 4163 612 3769 610 3514 608 320 613 3769 610 4163 612 3643 614 3785 611 3903 609 360 615 320 613 4163 612 3674 616 3785 611 3643 614 324 617 320 613 360 615 3644 618 3674 616 3643 614 4030 619 324 617 360 615 3675 620 3674 616 3644 618 3671 621 324 617 4030 619 3999 622 3675 620 3644 618 4036 623 3671 621 4030 619 286 624 3675 620 3999 622 4115 625 3671 621 4036 623 4042 626 286 624 3999 622 4330 627 4115 625 4036 623 333 628 286 624 4042 626 4095 629 4115 625 4330 627 4307 630 4095 629 4330 627 332 631 333 628 4042 626 4040 632 4095 629 4307 630 335 633 333 628 332 631 4272 634 4040 632 4307 630 334 635 335 633 332 631 296 636 4040 632 4272 634 337 637 335 633 334 635 4239 638 296 636 4272 634 3320 639 337 637 334 635 259 640 296 636 4239 638 3390 641 337 637 3320 639 4240 642 259 640 4239 638 3321 643 3390 641 3320 639 4159 644 259 640 4240 642 342 645 3390 641 3321 643 363 646 4159 644 4240 642 340 647 4159 644 363 646 341 648 342 645 3321 643 343 649 342 645 341 648 352 650 343 649 341 648 263 651 343 649 352 650 3372 652 263 651 352 650 3382 653 263 651 3372 652 3239 569 3382 653 3372 652 3464 568 3382 653 3239 569 4249 654 3677 654 345 654 4249 655 347 655 3677 655 4249 656 346 656 347 656 4249 657 3592 657 346 657 4249 658 3948 658 3592 658 4249 659 3534 659 3948 659 4249 660 348 660 3534 660 4249 661 3691 661 348 661 4249 662 318 662 3691 662 4249 663 325 663 318 663 4249 664 328 664 325 664 4249 665 349 665 328 665 4249 666 330 666 349 666 4249 667 4184 667 330 667 4249 668 4212 668 4184 668 4249 669 350 669 4212 669 4249 670 351 670 350 670 4249 671 4194 671 351 671 4249 672 4210 672 4194 672 4249 673 344 673 4210 673 4249 674 297 674 344 674 4249 675 353 675 297 675 4249 676 354 676 353 676 4249 677 356 677 354 677 4249 678 355 678 356 678 4249 679 303 679 355 679 4249 680 357 680 303 680 4249 681 358 681 357 681 4249 682 3773 682 358 682 4249 683 3925 683 3773 683 4249 684 3513 684 3925 684 4249 685 359 685 3513 685 4249 686 319 686 359 686 4249 687 4031 687 319 687 4249 688 326 688 4031 688 4249 660 361 660 326 660 4249 689 331 689 361 689 4249 690 362 690 331 690 4249 691 336 691 362 691 4249 692 338 692 336 692 4249 693 339 693 338 693 4249 694 3014 694 339 694 4249 695 3015 695 3014 695 4249 660 3366 660 3015 660 4249 696 3569 696 3568 696 4249 660 4186 660 3569 660 4249 697 4187 697 4186 697 4249 698 4213 698 4187 698 4249 699 405 699 4213 699 4249 700 4202 700 405 700 4249 701 4228 701 4202 701 4249 702 3245 702 4228 702 4249 703 3190 703 3245 703 4249 704 3142 704 3190 704 4249 705 3143 705 3142 705 4249 706 3433 706 3143 706 4249 707 3447 707 3433 707 4249 708 3512 708 3447 708 4249 709 3928 709 3512 709 4249 710 3926 710 3928 710 4249 711 3927 711 3926 711 4249 712 3184 712 3927 712 4249 713 3185 713 3184 713 4249 714 4297 714 3185 714 4249 715 3017 715 4297 715 4249 716 376 716 3017 716 4249 717 3118 717 376 717 4249 718 3474 718 3118 718 4249 719 3333 719 3474 719 4249 720 3283 720 3333 720 4249 721 3111 721 3283 721 4249 722 404 722 3111 722 4249 723 408 723 404 723 4249 724 406 724 408 724 4249 725 3189 725 406 725 4249 726 3188 726 3189 726 4249 660 3300 660 3188 660 4249 727 3301 727 3300 727 4249 660 4160 660 3301 660 4249 660 3994 660 4160 660 4249 660 3649 660 3994 660 4249 728 345 728 3649 728 375 729 3309 730 426 731 3308 732 3309 730 375 729 382 733 3308 732 375 729 3780 734 340 647 363 646 384 735 3308 732 382 733 3780 734 363 646 364 736 385 737 384 735 382 733 3808 738 384 735 385 737 3781 739 364 736 387 740 3781 739 3780 734 364 736 435 741 3781 739 387 740 390 742 3808 738 385 737 437 743 3808 738 390 742 389 744 435 741 387 740 3016 745 437 743 390 742 3833 746 435 741 389 744 3887 747 437 743 3016 745 395 748 3833 746 389 744 393 749 3887 747 3016 745 396 750 3887 747 393 749 394 751 3833 746 395 748 399 752 396 750 393 749 397 753 394 751 395 748 3067 754 396 750 399 752 3589 755 394 751 397 753 377 756 3067 754 399 752 365 757 3589 755 397 753 3068 758 3067 754 377 756 401 759 3589 755 365 757 378 760 3068 758 377 756 402 761 401 759 365 757 3057 762 3068 758 378 760 3124 763 401 759 402 761 379 764 3057 762 378 760 366 765 3124 763 402 761 3056 766 3057 762 379 764 3297 767 3124 763 366 765 3112 768 3056 766 379 764 4203 769 3297 767 366 765 3131 770 3056 766 3112 768 3213 771 3297 767 4203 769 3752 772 3131 770 3112 768 3022 773 3131 770 3752 772 367 774 3213 771 4203 769 3751 775 3022 773 3752 772 3214 776 3213 771 367 774 3023 777 3022 773 3751 775 410 778 3214 776 367 774 411 779 3023 777 3751 775 4298 780 3214 776 410 778 457 781 3023 777 411 779 368 782 4298 780 410 778 381 783 457 781 411 779 454 784 4298 780 368 782 460 785 457 781 381 783 369 786 454 784 368 782 380 787 460 785 381 783 414 788 454 784 369 786 462 789 460 785 380 787 415 790 462 789 380 787 370 791 414 788 369 786 3024 792 462 789 415 790 456 793 414 788 370 791 416 794 3024 792 415 790 371 795 456 793 370 791 3025 796 3024 792 416 794 3236 797 456 793 371 795 418 798 3025 796 416 794 372 799 3236 797 371 795 419 800 3025 796 418 798 3237 801 3236 797 372 799 420 802 419 800 418 798 373 803 3237 801 372 799 421 804 419 800 420 802 4276 805 3237 801 373 803 3648 573 421 804 420 802 427 576 421 804 3648 573 374 806 4276 805 373 803 3444 807 4276 805 374 806 423 808 3444 807 374 806 422 809 3444 807 423 808 424 810 422 809 423 808 430 811 422 809 424 810 426 731 430 811 424 810 3309 730 430 811 426 731 68 497 3965 812 4028 495 68 497 428 813 3965 812 3304 149 4037 148 4321 814 3949 815 4321 814 425 816 3949 815 425 816 383 817 3949 815 3304 149 4321 814 3726 818 3724 490 3969 488 3726 818 3969 488 388 819 3726 818 388 819 431 820 3836 821 3949 815 383 817 78 822 3726 818 431 820 76 823 383 817 4322 824 76 823 4322 824 386 825 78 822 431 820 3834 826 76 823 3836 821 383 817 79 827 76 823 386 825 80 828 78 822 3834 826 436 829 3834 826 392 830 436 829 392 830 3588 831 84 832 386 825 3809 833 84 832 3809 833 391 834 436 829 80 828 3834 826 84 832 79 827 386 825 85 835 3588 831 400 836 85 835 436 829 3588 831 438 837 391 834 3608 838 438 837 84 832 391 834 86 839 400 836 3123 840 86 839 85 835 400 836 87 841 3608 838 398 842 87 841 438 837 3608 838 3085 843 3123 840 439 844 3085 843 86 839 3123 840 90 845 87 841 398 842 441 846 439 844 445 847 441 846 3085 843 439 844 94 848 90 845 398 842 94 848 398 842 442 849 3684 850 441 846 445 847 446 851 94 848 442 849 446 851 442 849 447 852 3683 853 445 847 409 854 3683 853 409 854 450 855 3683 853 3684 850 445 847 98 856 446 851 447 852 98 856 447 852 403 857 98 856 403 857 449 858 452 859 3683 853 450 855 3827 860 98 856 449 858 3827 860 449 858 407 861 451 862 450 855 412 863 451 862 452 859 450 855 3828 864 3827 860 407 861 3828 864 407 861 455 865 53 866 412 863 4299 867 53 866 451 862 412 863 55 868 3828 864 455 865 56 117 4299 867 4288 869 56 117 4288 869 4289 870 56 117 53 866 4299 867 459 871 55 868 455 865 459 871 455 865 3493 872 458 118 4289 870 417 873 458 118 56 117 4289 870 461 874 459 871 3493 872 461 874 3493 872 3438 875 461 874 3438 875 3439 876 4035 121 417 873 464 877 4035 121 458 118 417 873 60 878 461 874 3439 876 3891 879 464 877 463 880 3891 879 4035 121 464 877 62 881 3439 876 413 882 62 881 60 878 3439 876 465 883 463 880 466 884 465 883 3891 879 463 880 3898 885 413 882 467 886 3898 885 62 881 413 882 4029 887 466 884 3443 888 4029 887 465 883 466 884 3417 889 467 886 3395 890 3417 889 3395 890 3965 812 3417 889 3898 885 467 886 4037 148 3443 888 4321 814 4037 148 4029 887 3443 888 428 813 3417 889 3965 812 476 891 663 892 668 893 476 891 655 894 663 892 476 891 647 895 655 894 476 891 631 896 647 895 476 891 533 897 631 896 476 891 600 898 533 897 476 891 594 899 600 898 476 891 586 900 594 899 476 891 583 901 586 900 476 891 573 902 583 901 476 891 469 903 573 902 476 891 552 904 469 903 476 891 551 905 552 904 476 891 542 906 551 905 476 891 535 907 542 906 476 891 602 908 535 907 476 891 598 909 602 908 476 891 590 910 598 909 476 891 470 911 590 910 476 891 579 912 470 911 476 891 3629 913 579 912 476 891 3628 914 3629 913 476 915 4086 915 3628 915 476 891 471 916 4086 917 476 891 549 918 471 916 476 891 543 919 549 918 476 891 537 920 543 919 476 891 472 921 537 920 476 891 593 922 472 921 476 891 473 923 593 922 476 891 584 924 473 923 476 891 580 925 584 924 476 891 568 926 580 925 476 891 560 927 568 926 476 891 555 928 560 927 476 891 550 929 555 928 476 891 674 930 550 929 476 891 666 931 674 930 476 891 660 932 666 931 476 891 657 933 660 932 476 891 651 934 657 933 476 891 642 935 651 934 476 891 640 936 642 935 476 891 628 937 640 936 476 891 629 938 628 937 476 891 623 939 629 938 476 891 4054 940 623 939 476 891 611 941 4054 940 476 891 4175 942 611 941 476 891 607 943 4175 942 476 891 691 944 607 943 476 891 686 945 691 944 476 891 688 946 686 945 476 891 475 947 688 946 476 891 671 948 475 947 476 891 683 949 671 948 476 891 679 950 683 949 476 891 652 951 679 950 476 891 638 952 652 951 476 891 630 953 638 952 476 891 626 954 630 953 476 891 622 955 626 954 476 891 618 956 622 955 476 891 614 957 618 956 476 891 609 958 614 957 476 891 612 959 609 958 476 891 700 960 612 959 476 891 477 961 700 960 476 891 672 962 477 961 476 891 668 893 672 962 481 963 3475 964 3936 965 479 966 246 967 245 968 479 966 245 968 478 969 3414 970 3475 964 481 963 479 966 3552 971 246 967 3414 970 3033 972 3475 964 3114 973 3033 972 3414 970 287 974 3552 971 479 966 3268 975 3552 971 287 974 3268 975 248 976 3552 971 3114 973 487 977 3033 972 3113 978 487 977 3114 973 3269 979 3121 980 248 976 3269 979 248 976 3268 975 489 981 487 977 3113 978 490 982 489 981 3113 978 291 983 492 984 3121 980 291 983 3121 980 3269 979 494 985 491 986 489 981 494 985 489 981 490 982 295 987 496 988 492 984 4050 989 491 986 494 985 295 987 492 984 291 983 4050 989 493 990 491 986 495 991 496 988 295 987 495 991 3256 992 496 988 4050 989 3182 993 493 990 257 994 253 995 3256 992 257 994 3256 992 495 991 3723 157 71 156 3183 996 3725 997 498 997 4052 997 74 998 261 998 258 998 262 999 256 1000 253 995 262 999 253 995 257 994 3305 1001 3322 1002 256 1000 3305 1001 256 1000 262 999 3305 1001 262 999 264 1003 500 1004 4268 493 3306 498 504 1005 232 1006 3728 144 3305 1001 3323 1007 3322 1002 504 1005 3728 144 3359 145 268 1008 3323 1007 3305 1001 268 1008 3305 1001 502 1009 268 1008 505 1010 3323 1007 267 1011 506 1012 232 1006 267 1011 232 1006 504 1005 271 1013 505 1010 268 1008 271 1013 507 1014 505 1010 267 1011 508 1015 506 1012 274 1016 507 1014 271 1013 272 1017 508 1015 267 1011 274 1016 233 1018 507 1014 509 1019 508 1015 272 1017 509 1019 510 1020 508 1015 233 1018 274 1016 3945 1021 275 1022 510 1020 509 1019 275 1022 237 1023 510 1020 3945 1021 3647 1024 233 1018 513 1025 512 1025 3947 1025 276 1026 514 1027 237 1023 276 1026 237 1023 275 1022 281 1028 4192 1029 511 1030 515 1031 4192 1029 281 1028 516 1032 4192 1029 515 1031 3956 1033 514 1027 276 1026 3956 1033 241 1034 514 1027 519 1035 516 1032 515 1031 283 1036 243 1037 241 1034 283 1036 241 1034 3956 1033 520 1038 516 1032 519 1035 284 1039 520 1038 519 1035 4032 1040 522 1041 243 1037 4032 1040 243 1037 283 1036 521 1042 520 1038 284 1039 524 1043 521 1042 284 1039 526 1044 521 1042 524 1043 523 1045 4130 1046 522 1041 523 1045 522 1041 4032 1040 3830 1047 526 1044 524 1043 3830 1047 530 1048 526 1044 527 1049 4130 1046 523 1045 527 1049 528 1050 4130 1046 478 969 245 968 528 1050 478 969 528 1050 527 1049 529 443 3668 440 3831 1051 3936 965 285 1052 531 1053 481 963 3936 965 531 1053 3850 1054 604 1055 539 1056 533 897 600 898 603 1057 533 897 603 1057 3194 1058 3845 1059 800 1060 3899 1061 536 1062 472 921 3849 1063 537 920 472 921 536 1062 4013 1064 3850 1054 539 1056 537 920 536 1062 538 1065 4013 1064 539 1056 771 1066 535 907 532 1067 801 1068 542 906 535 907 801 1068 4076 1069 4013 1064 771 1066 741 1070 542 906 801 1068 540 1071 537 920 538 1065 541 1072 537 920 540 1071 541 1072 543 919 537 920 3957 1073 4076 1069 771 1066 3957 1073 771 1066 774 1074 544 1075 540 1071 546 1076 544 1075 541 1072 540 1071 544 1075 543 919 541 1072 3696 1077 542 906 741 1070 551 905 542 906 3696 1077 744 1078 551 905 3696 1077 545 1079 4149 1080 42 89 545 1079 42 89 44 92 544 1075 549 918 543 919 3957 1073 774 1074 776 1081 674 930 673 1082 4063 1083 547 1084 544 1075 546 1076 3958 1085 3957 1073 776 1081 3144 1086 3145 1087 745 1088 550 929 674 930 4063 1083 551 905 744 1078 548 1089 545 1079 44 92 45 93 549 918 544 1075 547 1084 4072 1090 776 1081 553 1091 4072 1090 3958 1085 776 1081 552 904 551 905 548 1089 3968 1092 745 1088 746 1093 3968 1092 3144 1086 745 1088 559 1094 545 1079 45 93 4047 1095 550 929 4063 1083 4047 1095 555 928 550 929 554 1096 549 918 547 1084 559 1094 45 93 556 33 552 904 548 1089 558 1097 471 916 549 918 554 1096 4065 1098 553 1091 780 1099 4065 1098 4072 1090 553 1091 557 1100 471 916 554 1096 4117 1101 3968 1092 746 1093 4117 1101 746 1093 749 1102 4047 1095 560 927 555 928 4066 1103 471 916 557 1100 4066 1103 4086 917 471 916 4065 1098 780 1099 562 1104 4048 1105 556 33 563 1106 4048 1105 559 1094 556 33 561 1107 552 904 558 1097 4068 1108 4065 1098 562 1104 569 1109 4117 1101 749 1102 560 927 4047 1095 564 1110 4048 1105 563 1106 754 1111 4105 1112 562 1104 783 1113 4105 1112 4068 1108 562 1104 570 1114 4048 1105 754 1111 469 903 561 1107 4118 1115 469 903 552 904 561 1107 3666 1116 560 927 564 1110 570 1114 754 1111 756 1117 3523 1118 4067 1118 567 1118 565 1119 566 1120 3522 1121 574 1122 4105 1112 783 1113 568 926 560 927 3666 1116 571 1123 570 1114 756 1117 752 1124 469 903 4118 1115 568 926 3666 1116 3635 1125 571 1123 756 1117 759 1126 573 902 469 903 752 1124 576 1127 572 1127 4087 1127 3629 913 565 1119 3522 1121 577 1128 571 1123 759 1126 3629 913 3522 1121 3500 1129 753 1130 573 902 752 1124 3626 1131 568 926 3635 1125 3626 1131 580 925 568 926 577 1128 759 1126 758 1132 583 901 573 902 753 1130 3501 1133 3629 913 3500 1129 579 912 3629 913 3501 1133 3627 1134 577 1128 758 1132 584 924 580 925 3626 1131 584 924 3626 1131 582 1135 3627 1134 758 1132 761 1136 581 1137 583 901 753 1130 587 1138 578 1139 790 1140 3480 1141 3627 1134 761 1136 589 1142 581 1137 766 1143 470 911 3501 1133 587 1138 470 911 579 912 3501 1133 589 1142 583 901 581 1137 473 923 584 924 582 1135 589 1142 766 1143 768 1144 3480 1141 761 1136 762 1145 473 923 582 1135 585 1146 586 900 583 901 589 1142 587 1138 790 1140 591 1147 588 1148 3480 1141 762 1145 590 910 470 911 587 1138 3481 1149 473 923 585 1146 594 899 586 900 589 1142 593 922 473 923 3481 1149 588 1148 762 1145 592 1150 596 1151 589 1142 768 1144 596 1151 768 1144 773 1152 599 1153 587 1138 591 1147 599 1153 590 910 587 1138 594 899 589 1142 596 1151 599 1153 591 1147 597 1154 598 909 590 910 599 1153 4254 1155 588 1148 592 1150 595 1156 593 922 3481 1149 472 921 593 922 595 1156 600 898 594 899 596 1151 596 1151 773 1152 781 1157 4254 1155 592 1150 767 1158 599 1153 597 1154 796 1159 534 1160 4254 1155 767 1158 602 908 598 909 599 1153 603 1057 596 1151 781 1157 3849 1063 472 921 595 1156 603 1057 600 898 596 1151 534 1160 767 1158 604 1055 535 907 599 1153 796 1159 535 907 602 908 599 1153 3845 1059 796 1159 800 1060 532 1067 535 907 796 1159 3850 1054 534 1160 604 1055 605 1161 696 55 25 57 700 960 699 1 606 1162 4107 1163 605 1161 25 57 606 1162 608 0 612 959 4175 942 4106 1164 693 1165 606 1162 612 959 700 960 4175 942 607 943 4106 1164 4316 1166 25 57 4329 61 694 1167 4316 1166 4329 61 2 1168 612 959 608 0 612 959 2 1168 609 958 4317 1169 4175 942 610 1170 4317 1169 611 941 4175 942 3576 1171 4329 61 3961 60 609 958 2 1168 4088 1172 3577 1173 3576 1171 3961 60 3578 1174 611 941 697 1175 609 958 4088 1172 614 957 3578 1174 4054 940 611 941 617 1176 3961 60 621 63 620 1177 617 1176 621 63 3737 1178 4054 940 3880 1179 4 1180 614 957 4088 1172 614 957 4 1180 618 956 675 1181 4169 8 676 10 620 1177 621 63 30 67 623 939 4054 940 3737 1178 627 1182 620 1177 30 67 3491 1183 623 939 3737 1178 622 955 675 1181 676 10 622 955 618 956 675 1181 627 1182 30 67 624 70 625 1184 3194 1058 928 1185 629 938 623 939 3491 1183 3492 1186 627 1182 624 70 625 1184 533 897 3194 1058 626 954 676 10 677 1187 626 954 622 955 676 10 628 937 3491 1183 635 1188 634 1189 3117 12 678 17 628 937 629 938 3491 1183 626 954 677 1187 634 1189 3492 1186 624 70 633 71 631 896 533 897 625 1184 634 1189 678 17 630 953 626 954 634 1189 630 953 636 1190 3492 1186 633 71 4207 1191 628 937 635 1188 631 896 625 1184 632 37 636 1190 633 71 641 73 640 936 628 937 4207 1191 643 1192 631 896 632 37 643 1192 632 37 637 36 647 895 631 896 643 1192 630 953 639 1193 638 952 630 953 678 17 639 1193 642 935 640 936 4207 1191 4235 1194 636 1190 641 73 4125 1195 647 895 643 1192 4235 1194 641 73 33 75 646 1196 637 36 649 40 646 1196 643 1192 637 36 645 1197 642 935 4207 1191 638 952 644 1198 648 1199 651 934 642 935 645 1197 4104 1200 4235 1194 33 75 4104 1200 33 75 654 77 4125 1195 655 894 647 895 650 1201 646 1196 649 40 652 951 648 1199 11 1202 650 1201 649 40 21 46 651 934 645 1197 653 1203 652 951 638 952 648 1199 655 894 4125 1195 650 1201 3823 1204 4104 1200 654 77 3823 1204 654 77 659 79 656 51 650 1201 21 46 658 1205 651 934 653 1203 657 933 651 934 658 1205 3074 1206 652 951 11 1202 663 892 650 1201 656 51 663 892 655 894 650 1201 679 950 652 951 3074 1206 660 932 657 933 658 1205 3824 1207 3823 1204 659 79 3824 1207 659 79 661 81 682 1208 680 1209 12 1210 660 932 658 1205 662 1211 663 892 656 51 39 1212 683 949 679 950 682 1208 4305 1213 12 1210 671 948 666 931 660 932 662 1211 3935 1214 661 81 667 84 3935 1214 3824 1207 661 81 666 931 662 1211 664 1215 12 1210 671 948 683 949 668 893 663 892 39 1212 668 893 39 1212 3983 1216 4306 1217 16 38 665 42 670 1218 667 84 37 87 670 1218 3935 1214 667 84 4055 1219 666 931 664 1215 475 947 684 1220 687 1221 668 893 3983 1216 46 1222 674 930 666 931 4055 1219 475 947 671 948 684 1220 3770 44 4306 1217 665 42 670 1218 42 89 4149 1080 670 1218 37 87 42 89 4055 1219 673 1082 674 930 688 946 475 947 687 1221 3771 1223 3770 44 4324 47 672 962 668 893 46 1222 672 962 46 1222 4092 1224 4305 1213 16 38 4306 1217 686 945 688 946 3772 1225 3770 44 688 946 687 1221 4148 1226 4324 47 22 50 477 961 4092 1224 4093 1227 477 961 672 962 4092 1224 689 1228 686 945 3772 1225 4147 1229 4148 1226 22 50 691 944 4226 1230 4147 1229 4107 1163 25 57 474 1231 690 1232 691 944 686 945 690 1232 686 945 695 1233 701 1234 51 97 699 1 692 1235 22 50 696 55 697 1175 611 941 613 1236 698 1237 615 1237 619 1237 607 943 691 944 692 1235 700 960 701 1234 699 1 700 960 477 961 701 1234 606 1162 699 1 608 0 4033 1238 3462 1239 3970 1240 3452 1241 3462 1239 4033 1238 3825 1242 3452 1241 4033 1238 3394 1243 3393 475 4155 474 3403 1244 3452 1241 3825 1242 3394 1243 4155 474 3766 1245 705 1246 3403 1244 3825 1242 3341 1247 3403 1244 705 1246 4296 1248 3766 1245 707 1249 4296 1248 3394 1243 3766 1245 4231 1250 4296 1248 707 1249 3800 1251 3341 1247 705 1246 706 1252 3341 1247 3800 1251 81 1253 4231 1250 707 1249 4113 1254 706 1252 3800 1251 4211 1255 4231 1250 81 1253 3192 1256 706 1252 4113 1254 83 1257 4211 1255 81 1253 88 1258 3192 1256 4113 1254 3786 1259 3192 1256 88 1258 817 1260 4211 1255 83 1257 3365 1261 3786 1259 88 1258 711 1262 817 1260 83 1257 3810 1263 3786 1259 3365 1261 3259 1264 817 1260 711 1262 3364 1265 3810 1263 3365 1261 3167 1266 3259 1264 711 1262 3843 1267 3810 1263 3364 1265 3260 1268 3259 1264 3167 1266 4216 1269 3843 1267 3364 1265 4251 1270 3260 1268 3167 1266 823 1271 3843 1267 4216 1269 3718 1272 3260 1268 4251 1270 96 1273 823 1271 4216 1269 4252 1274 3718 1272 4251 1270 714 1275 823 1271 96 1273 715 1276 3718 1272 4252 1274 97 1277 714 1275 96 1273 3418 1278 715 1276 4252 1274 3146 1279 714 1275 97 1277 3337 1280 715 1276 3418 1278 99 1281 3146 1279 97 1277 3147 1282 3146 1279 99 1281 3419 1283 3337 1280 3418 1278 4081 1284 3147 1282 99 1281 3591 1285 3337 1280 3419 1283 3416 1286 3147 1282 4081 1284 100 1287 3591 1285 3419 1283 4080 1288 3416 1286 4081 1284 722 1289 3591 1285 100 1287 3010 1290 3416 1286 4080 1288 101 1291 722 1289 100 1287 4142 1292 3010 1290 4080 1288 3402 1293 722 1289 101 1291 3011 1294 3010 1290 4142 1292 54 1295 3402 1293 101 1291 725 1296 3011 1294 4142 1292 727 1297 3402 1293 54 1295 835 1298 3011 1294 725 1296 728 1299 835 1298 725 1296 3700 1300 727 1297 54 1295 3050 1301 835 1298 728 1299 4257 1302 727 1297 3700 1300 730 1303 3050 1301 728 1299 57 1304 4257 1302 3700 1300 3051 1305 3050 1301 730 1303 3716 1306 4257 1302 57 1304 732 1307 3051 1305 730 1303 61 1308 3716 1306 57 1304 3041 1309 3051 1305 732 1307 3169 1310 3716 1306 61 1308 733 1311 3041 1309 732 1307 734 1312 3169 1310 61 1308 3042 1313 3041 1309 733 1311 3170 1314 3169 1310 734 1312 501 403 3042 1313 733 1311 3557 406 3042 1313 501 403 735 1315 3170 1314 734 1312 3440 1316 3170 1314 735 1315 4043 1317 3440 1316 735 1315 3328 1318 3440 1316 4043 1317 3971 1319 3328 1318 4043 1317 738 1320 3328 1318 3971 1319 3970 1240 738 1320 3971 1319 3462 1239 738 1320 3970 1240 3290 1321 3293 1321 3750 1321 3599 350 179 342 3294 1322 3291 1323 3598 1324 740 1325 3792 1326 3804 347 3600 346 3793 1327 3598 1324 3291 1323 3791 1328 3292 1329 3794 1330 3782 1331 802 1332 3695 1333 742 1334 3695 1333 802 1332 742 1334 802 1332 743 1335 3145 1087 742 1334 743 1335 743 1335 854 1336 745 1088 3145 1087 743 1335 745 1088 745 1088 854 1336 747 1337 745 1088 747 1337 746 1093 746 1093 747 1337 748 1338 746 1093 748 1338 749 1102 749 1102 748 1338 868 1339 749 1102 868 1339 569 1109 868 1339 750 1340 3686 1341 569 1109 868 1339 3686 1341 869 32 3676 1342 563 1106 556 33 869 32 563 1106 581 1137 3687 1343 3550 1344 3686 1341 750 1340 3687 1343 3676 1342 755 1345 754 1111 750 1340 3550 1344 3687 1343 563 1106 3676 1342 754 1111 754 1111 755 1345 883 1346 756 1117 754 1111 883 1346 883 1346 4136 1347 756 1117 756 1117 4136 1347 759 1126 4136 1347 891 1348 759 1126 759 1126 891 1348 758 1132 891 1348 4269 1349 758 1132 3550 1344 760 1350 581 1137 758 1132 4269 1349 761 1136 4269 1349 763 1351 761 1136 768 1144 766 1143 882 1352 581 1137 760 1350 766 1143 761 1136 763 1351 762 1145 763 1351 4247 1353 762 1145 760 1350 882 1352 766 1143 762 1145 4247 1353 592 1150 4247 1353 764 1354 592 1150 764 1354 908 1355 767 1158 592 1150 764 1354 767 1158 882 1352 772 1356 768 1144 767 1158 908 1355 604 1055 908 1355 4303 1357 604 1055 604 1055 4303 1357 539 1056 4303 1357 770 1358 539 1056 771 1066 539 1056 770 1358 770 1358 849 1359 771 1066 771 1066 849 1359 774 1074 773 1152 768 1144 772 1356 776 1081 774 1074 775 1360 772 1356 778 1361 773 1152 849 1359 775 1360 774 1074 777 1362 553 1091 776 1081 775 1360 777 1362 776 1081 780 1099 553 1091 862 1363 553 1091 777 1362 862 1363 562 1104 780 1099 779 1364 781 1157 773 1152 778 1361 862 1363 779 1364 780 1099 778 1361 782 1365 781 1157 779 1364 3332 1366 562 1104 783 1113 562 1104 3332 1366 603 1057 781 1157 782 1365 3332 1366 3689 1367 783 1113 783 1113 3689 1367 784 1368 601 1369 603 1057 786 1370 786 1370 603 1057 782 1365 783 1113 784 1368 574 1122 575 1371 574 1122 785 1372 574 1122 784 1368 785 1372 785 1372 784 1368 886 1373 575 1371 785 1372 886 1373 575 1371 886 1373 787 1374 786 1370 789 1375 601 1369 787 1374 886 1373 888 1376 578 1139 575 1371 787 1374 578 1139 787 1374 888 1376 601 1369 789 1375 3982 1377 888 1376 788 1378 790 1140 578 1139 888 1376 790 1140 790 1140 788 1378 792 1379 792 1379 794 1380 793 1381 790 1140 792 1379 591 1147 792 1379 793 1381 591 1147 3982 1377 3916 1382 3286 1383 3916 1382 791 34 3286 1383 597 1154 591 1147 794 1380 591 1147 793 1381 794 1380 791 34 632 37 3286 1383 794 1380 797 1384 795 1385 794 1380 795 1385 597 1154 796 1159 597 1154 797 1384 597 1154 795 1385 797 1384 797 1384 798 1386 796 1159 800 1060 796 1159 798 1386 798 1386 3815 1387 800 1060 800 1060 3815 1387 3782 1331 800 1060 3782 1331 3899 1061 3695 1333 3899 1061 3782 1331 803 1388 3361 1389 846 1388 3662 1390 3329 1390 702 1391 3662 1390 3661 1392 3329 1390 187 298 846 1388 188 298 187 298 803 1388 846 1388 185 293 703 293 805 1393 3673 1391 702 1391 807 1394 3673 1391 3662 1390 702 1391 3711 1394 807 1394 704 1395 811 1393 185 293 805 1393 3711 1394 3673 1391 807 1394 3613 1395 704 1395 808 1396 809 1397 805 1393 810 1397 3613 1395 3711 1394 704 1395 809 1397 810 1397 812 1398 809 1397 811 1393 805 1393 3612 1396 808 1396 3191 1399 815 1398 812 1398 814 1400 3612 1396 3613 1395 808 1396 815 1398 809 1397 812 1398 3526 1400 814 1400 3759 1401 3638 1399 3612 1396 3191 1399 3526 1400 815 1398 814 1400 3525 1401 3526 1400 3759 1401 1023 1402 3191 1399 816 1402 1023 1402 816 1402 709 1403 1023 1402 3638 1399 3191 1399 995 1404 3759 1401 710 1404 995 1404 710 1404 713 1405 995 1404 3525 1401 3759 1401 3929 1403 1023 1402 709 1403 3564 1405 713 1405 820 1406 3930 1407 3929 1403 709 1403 3564 1405 995 1404 713 1405 3930 1407 709 1403 818 1407 3894 1406 820 1406 3336 1408 3894 1406 3564 1405 820 1406 3505 1409 3930 1407 818 1407 3505 1409 818 1407 712 1409 3912 1408 3336 1408 719 1410 3912 1408 3894 1406 3336 1408 3436 1411 3505 1409 712 1409 3436 1411 712 1409 3797 1411 4300 1410 719 1410 824 1412 4300 1410 3912 1408 719 1410 3437 1413 3436 1411 3797 1411 3437 1413 3797 1411 3465 1413 3651 1414 4300 1410 824 1412 3656 1415 3437 1413 3465 1413 3656 1415 3465 1413 717 1415 3650 1416 824 1412 3401 1416 3650 1416 3651 1414 824 1412 3669 1417 3656 1415 717 1415 3669 1417 717 1415 721 1417 3429 1418 3401 1416 828 1418 3429 1418 3650 1416 3401 1416 4229 1419 3669 1417 721 1417 4229 1419 721 1417 831 1419 3430 1420 828 1418 4234 1420 3430 1420 4234 1420 731 1421 3430 1420 3429 1418 828 1418 3719 1422 4229 1419 831 1419 3719 1422 831 1419 723 1422 4121 1421 731 1421 832 873 4121 1421 3430 1420 731 1421 3720 1423 723 1422 724 1423 1000 873 832 873 838 1424 3720 1423 3719 1422 723 1422 1000 873 4121 1421 832 873 3133 1425 724 1423 3229 1425 3133 1425 3720 1423 724 1423 4195 1424 1000 873 838 1424 3132 1426 3229 1425 841 1426 3132 1426 3133 1425 3229 1425 1002 1427 838 1424 839 1427 1002 1427 839 1427 845 1428 1002 1427 4195 1424 838 1424 1045 1429 841 1426 842 1429 1045 1429 842 1429 843 1389 1045 1429 3132 1426 841 1426 4320 1428 845 1428 737 1392 4320 1428 1002 1427 845 1428 3361 1389 843 1389 846 1388 3361 1389 1045 1429 843 1389 3661 1392 737 1392 3329 1390 3661 1392 4320 1428 737 1392 739 1430 3748 1430 3746 1430 3794 1330 3292 1329 847 1431 848 1432 1070 1432 1071 1432 3782 1331 913 1433 3757 1434 802 1332 3782 1331 3757 1434 770 1358 4017 1435 3048 1436 3758 1437 2783 1438 851 1439 849 1359 770 1358 3048 1436 911 1440 3883 1441 3047 1442 3047 1442 3883 1441 855 1443 2783 1438 852 1444 851 1439 743 1335 802 1332 3757 1434 775 1360 849 1359 3049 1445 849 1359 3048 1436 3049 1445 743 1335 3757 1434 853 1446 853 1446 851 1439 852 1444 854 1336 743 1335 853 1446 856 1447 850 1447 4302 1447 857 1448 3047 1442 855 1443 775 1360 3049 1445 777 1362 777 1362 3049 1445 4301 1449 854 1336 853 1446 1110 1450 853 1446 852 1444 1110 1450 857 1448 855 1443 4199 1451 747 1337 854 1336 3061 1452 777 1362 4301 1449 3315 1453 854 1336 1110 1450 861 1454 3315 1453 4301 1449 858 1455 861 1454 1110 1450 1111 1456 857 1448 4199 1451 3052 1457 867 1458 41 88 859 1459 748 1338 747 1337 3061 1452 861 1454 1111 1456 860 1460 748 1338 3061 1452 860 1460 41 88 970 1461 859 1459 3315 1453 858 1455 3316 1462 859 1459 970 1461 3484 1463 3508 1464 1096 1465 866 1466 4199 1451 3053 1467 3052 1457 779 1364 862 1363 863 1468 779 1364 863 1468 3332 1366 868 1339 748 1338 860 1460 3354 31 859 1459 3374 1469 859 1459 3484 1463 3374 1469 868 1339 860 1460 871 1470 866 1466 865 1471 864 1472 869 32 3354 31 3374 1469 860 1460 1111 1456 871 1470 871 1470 1111 1456 3803 1473 3316 1462 858 1455 870 1474 750 1340 868 1339 871 1470 869 32 3374 1469 873 1475 750 1340 871 1470 3528 1476 865 1471 3208 1477 3375 1478 864 1472 865 1471 3375 1478 872 1479 3316 1462 870 1474 750 1340 3528 1476 3550 1344 751 1480 869 32 873 1475 876 1481 871 1470 3803 1473 872 1479 870 1474 3690 1482 3208 1477 877 1483 874 1484 3375 1478 3208 1477 874 1484 751 1480 873 1475 3532 1485 755 1345 751 1480 3532 1485 880 1486 3053 1467 1092 1487 755 1345 3532 1485 875 1488 878 1489 3527 1489 3530 1489 3531 1490 3529 1491 3537 1492 3529 1491 3802 1493 3537 1492 1112 1494 2818 1495 884 1496 784 1368 3689 1367 881 1497 874 1484 877 1483 3533 1498 760 1350 3550 1344 879 1499 883 1346 755 1345 875 1488 880 1486 1092 1487 3520 1500 3520 1500 1092 1487 1094 1501 883 1346 875 1488 3633 1502 760 1350 879 1499 882 1352 885 1503 3533 1498 1103 1504 3533 1498 877 1483 1103 1504 757 1505 883 1346 3633 1502 882 1352 879 1499 3538 1506 886 1373 784 1368 881 1497 757 1505 3633 1502 887 1507 3521 1508 3520 1500 1094 1501 886 1373 881 1497 889 1509 888 1376 886 1373 889 1509 885 1503 1103 1504 4178 1510 4178 1510 1103 1504 1105 1511 3521 1508 1094 1501 1101 1512 4176 1513 757 1505 887 1507 772 1356 882 1352 4041 1514 882 1352 3538 1506 4041 1514 4041 1514 3538 1506 894 1515 888 1376 889 1509 4114 1516 788 1378 888 1376 4114 1516 3521 1508 1101 1512 890 1517 4178 1510 1105 1511 2796 1518 778 1361 772 1356 4041 1514 4176 1513 887 1507 895 1519 893 1520 4176 1513 895 1519 4260 1521 4178 1510 2796 1518 4019 1522 892 1522 4021 1522 788 1378 4114 1516 4261 1523 792 1379 788 1378 4261 1523 4041 1514 894 1515 4020 1524 890 1517 1101 1512 897 1525 782 1365 778 1361 899 1526 893 1520 895 1519 898 1527 4246 1528 893 1520 898 1527 894 1515 4109 1529 4020 1524 4260 1521 2796 1518 3988 1530 3988 1530 2796 1518 4314 1531 897 1525 1101 1512 3222 1532 4004 1533 792 1379 4261 1523 786 1370 782 1365 899 1526 4246 1528 898 1527 3989 1534 765 1535 4246 1528 3989 1534 3988 1530 4314 1531 3991 1536 3990 1537 896 1537 903 1537 765 1535 3989 1534 904 1538 4004 1533 4261 1523 4078 1539 905 1540 789 1375 786 1370 905 1540 786 1370 899 1526 902 1541 897 1525 3222 1532 900 1542 4070 1543 904 1538 797 1384 4004 1533 4078 1539 764 1354 765 1535 904 1538 932 1544 3678 1545 4109 1529 3678 1545 4020 1524 4109 1529 904 1538 4070 1543 909 1546 4069 1547 1109 1548 3933 1549 797 1384 4078 1539 907 1550 904 1538 909 1546 764 1354 4078 1539 901 1551 907 1550 3816 1552 3223 1553 3224 1554 798 1386 797 1384 907 1550 1109 1548 906 1555 3933 1549 764 1354 909 1546 910 1556 908 1355 764 1354 910 1556 798 1386 3817 1557 3815 1387 799 1558 3816 1552 3224 1554 906 1555 3884 1559 3933 1549 908 1355 910 1556 4016 1560 769 1561 908 1355 4016 1560 910 1556 909 1546 4015 1562 769 1561 4016 1560 4017 1435 3815 1387 1102 1563 3783 1564 3782 1331 3815 1387 3783 1564 4015 1562 909 1546 914 1565 916 1566 3225 1567 3758 1437 770 1358 769 1561 4017 1435 912 1568 3883 1441 911 1440 915 1569 916 1566 3758 1437 28 58 24 53 4293 1570 28 58 4293 1570 921 1571 1 3 918 1572 3869 5 1116 1573 1114 1574 918 1572 982 1575 1089 1576 3710 1577 28 58 921 1571 920 1578 923 62 28 58 920 1578 3869 5 918 1572 1114 1574 3 7 3869 5 3777 1579 919 1580 917 1581 4274 1582 3709 1583 3710 1577 2712 1584 3710 1577 1089 1576 2712 1584 29 64 923 62 925 1585 923 62 920 1578 925 1585 5 9 3 7 924 1586 3 7 3777 1579 924 1586 919 1580 4274 1582 3778 1587 3709 1583 2712 1584 4049 1588 3778 1587 4274 1582 922 1589 4049 1588 2712 1584 2768 1590 5 9 924 1586 4179 1591 3205 11 5 9 4179 1591 29 64 925 1585 927 66 927 66 925 1585 930 1592 937 1593 3778 1587 922 1589 3508 1464 967 1594 1096 1465 4049 1588 2768 1590 4096 1595 3982 1377 789 1375 3679 1596 789 1375 905 1540 3679 1596 938 1597 927 66 934 1598 931 15 3205 11 4179 1591 927 66 930 1592 934 1598 929 1599 3678 1545 932 1544 3916 1382 3982 1377 3679 1596 934 1598 930 1592 935 1600 4096 1595 2768 1590 4248 1601 922 1589 2762 1602 937 1593 938 1597 934 1598 4222 1603 931 15 4179 1591 4193 1604 2762 1602 943 1605 937 1593 940 1606 938 1597 4222 1603 929 1599 932 1544 933 1607 3005 18 931 15 4193 1604 932 1544 939 1608 933 1607 936 1609 934 1598 935 1600 941 1610 937 1593 943 1605 31 1611 940 1606 4273 1612 940 1606 4222 1603 4273 1612 791 34 933 1607 939 1608 945 1613 3005 18 3471 1614 936 1609 935 1600 942 1615 942 1615 935 1600 1083 1616 3005 18 4193 1604 3471 1614 31 1611 4273 1612 949 1617 942 1615 1083 1616 3509 1618 14 35 791 34 3424 1619 941 1610 943 1605 3470 1620 32 1621 31 1611 949 1617 17 41 14 35 3425 1622 14 35 3424 1619 3425 1622 3470 1620 943 1605 3518 1623 3377 1624 945 1613 3378 1625 939 1608 947 1626 948 1627 791 34 939 1608 3424 1619 945 1613 3471 1614 3378 1625 3509 1618 1083 1616 3276 1628 3278 1629 4188 1630 3277 1631 32 1621 949 1617 950 1632 34 1633 32 1621 950 1632 3380 1634 946 1634 3546 1634 948 1627 947 1626 954 1635 3547 1636 3470 1620 3518 1623 959 49 17 41 3425 1622 34 1633 950 1632 956 1637 3277 1631 4188 1630 956 1637 4188 1630 2747 1638 956 1637 957 78 4217 76 3069 1639 958 1640 3519 1641 3371 1642 10 28 3376 25 3640 1643 951 1644 3379 1644 3641 1644 959 49 3425 1622 3030 1645 960 1646 955 1647 958 1640 954 1635 947 1626 962 1648 34 1633 956 1637 961 1649 961 1649 956 1637 1087 1650 956 1637 2747 1638 1087 1650 35 1651 957 78 3069 1639 27 54 959 49 964 1652 960 1646 958 1640 3371 1642 13 30 10 28 3640 1643 959 49 3030 1645 964 1652 35 1651 3069 1639 3070 1653 3062 65 27 54 964 1652 961 1649 1087 1650 963 1654 963 1654 1087 1650 1090 1655 36 1656 35 1651 3070 1653 960 1646 3371 1642 3839 1657 3839 1657 3371 1642 3279 1658 3062 65 964 1652 3031 1659 15 39 13 30 966 1660 964 1652 3030 1645 3031 1659 36 1656 3070 1653 3058 1661 13 30 3640 1643 966 1660 968 1662 963 1654 967 1594 963 1654 1090 1655 967 1594 38 85 36 1656 3058 1661 38 85 3058 1661 972 1663 3839 1657 3279 1658 969 1664 40 1665 965 1665 3063 1665 43 91 4319 82 3064 1666 18 43 15 39 3507 1667 15 39 966 1660 3507 1667 968 1662 967 1594 3059 1668 41 88 38 85 972 1663 970 1461 41 88 972 1663 3062 65 3031 1659 3065 1669 967 1594 3508 1464 3059 1668 969 1664 3279 1658 1099 1670 3066 1671 1091 1671 1085 1671 20 45 18 43 3140 1672 47 94 43 91 3064 1666 18 43 3507 1667 3140 1672 969 1664 1099 1670 974 1673 974 1673 1099 1670 3138 1674 50 96 47 94 975 1675 979 48 20 45 3140 1672 47 94 3064 1666 975 1675 971 1676 3694 1677 975 1675 975 1675 3694 1677 977 1678 49 98 50 96 975 1675 23 52 979 48 3141 1679 979 48 3140 1672 3141 1679 974 1673 3138 1674 978 1680 23 52 3141 1679 4292 1681 980 1682 978 1680 3139 1683 978 1680 3138 1674 3139 1683 976 1684 3368 1685 981 1686 0 2 49 98 977 1678 24 53 23 52 4292 1681 49 98 975 1675 977 1678 24 53 4292 1681 4293 1570 0 2 977 1678 1116 1573 980 1682 3139 1683 982 1575 1 3 0 2 918 1572 982 1575 3139 1683 1089 1576 981 1686 3368 1685 1089 1576 0 2 1116 1573 918 1572 986 1230 983 1230 984 1230 4265 1230 984 1230 829 1230 4265 1230 986 1230 984 1230 987 1230 4265 1230 829 1230 3387 1687 990 1687 983 1687 3387 1688 983 1688 986 1688 989 1689 987 1689 829 1689 3120 1690 987 1690 989 1690 991 1230 819 1230 990 1230 991 1691 990 1691 3387 1691 992 1692 3120 1692 989 1692 3736 1230 3120 1230 992 1230 3488 1230 993 1230 819 1230 3488 1693 819 1693 991 1693 833 1694 3736 1694 992 1694 996 1695 3736 1695 833 1695 3614 1696 993 1696 3488 1696 3614 1230 3565 1230 993 1230 4122 1697 996 1697 833 1697 999 1230 996 1230 4122 1230 1128 1698 3565 1698 3614 1698 1128 1699 998 1699 3565 1699 840 1700 999 1700 4122 1700 3693 1230 999 1230 840 1230 3779 1701 998 1701 1128 1701 3779 1702 1003 1702 998 1702 4196 1230 3693 1230 840 1230 1004 1703 3693 1703 4196 1703 1005 1704 1003 1704 3779 1704 1005 1705 3866 1705 1003 1705 844 1706 1004 1706 4196 1706 3549 1230 1004 1230 844 1230 1117 1230 4323 1230 3866 1230 1117 1707 3866 1707 1005 1707 1006 1708 3549 1708 844 1708 1007 1230 3549 1230 1006 1230 1118 1709 4323 1709 1117 1709 1118 1230 4238 1230 4323 1230 1008 1710 1007 1710 1006 1710 3559 1230 1007 1230 1008 1230 3821 1230 3915 1230 4238 1230 3821 1711 4238 1711 1118 1711 1011 1230 3559 1230 1008 1230 3914 1230 3915 1230 3821 1230 1119 1230 3559 1230 1011 1230 1010 1712 3914 1712 3821 1712 806 1230 1119 1230 1011 1230 184 1713 3914 1713 1010 1713 3744 1714 1119 1714 806 1714 173 1715 184 1715 1010 1715 1012 1716 3744 1716 806 1716 3585 1717 184 1717 173 1717 4062 1718 3744 1718 1012 1718 1017 1230 3585 1230 173 1230 813 1719 4062 1719 1012 1719 4103 1720 3585 1720 1017 1720 1126 1721 4062 1721 813 1721 3235 1722 4103 1722 1017 1722 1018 1230 1126 1230 813 1230 4001 1723 4103 1723 3235 1723 3582 1230 1126 1230 1018 1230 164 1724 4001 1724 3235 1724 4034 1230 3582 1230 1018 1230 4000 1725 4001 1725 164 1725 1021 1230 4000 1230 164 1230 3581 1726 3582 1726 4034 1726 1025 1230 3581 1230 4034 1230 1024 1230 4000 1230 1021 1230 162 1230 1024 1230 1021 1230 1130 1727 3581 1727 1025 1727 1026 1728 1130 1728 1025 1728 4119 1729 1024 1729 162 1729 3544 1230 4119 1230 162 1230 1134 1730 1130 1730 1026 1730 821 1230 1134 1230 1026 1230 4006 1731 4119 1731 3544 1731 1028 1732 4006 1732 3544 1732 3734 1733 1134 1733 821 1733 822 1734 3734 1734 821 1734 210 1735 4006 1735 1028 1735 154 1230 210 1230 1028 1230 4185 1736 3734 1736 822 1736 825 1737 4185 1737 822 1737 3992 1738 210 1738 154 1738 4014 1230 3992 1230 154 1230 1140 1739 4185 1739 825 1739 826 1230 1140 1230 825 1230 1032 1230 3992 1230 4014 1230 1031 1230 1032 1230 4014 1230 1141 1740 1140 1740 826 1740 827 1741 1141 1741 826 1741 200 1742 1032 1742 1031 1742 1033 1230 200 1230 1031 1230 4182 1743 1141 1743 827 1743 1035 1230 4182 1230 827 1230 1036 1744 200 1744 1033 1744 1037 1230 1036 1230 1033 1230 4183 1745 4182 1745 1035 1745 830 1230 4183 1230 1035 1230 1038 1694 1036 1694 1037 1694 144 1746 1038 1746 1037 1746 4218 1747 4183 1747 830 1747 834 1748 4218 1748 830 1748 1041 1749 1038 1749 144 1749 3703 1750 1041 1750 144 1750 3285 1751 4218 1751 834 1751 837 1752 3285 1752 834 1752 193 1700 1041 1700 3703 1700 1042 1230 193 1230 3703 1230 1044 1753 3285 1753 837 1753 836 1754 1044 1754 837 1754 194 1230 193 1230 1042 1230 3774 1703 194 1703 1042 1703 3248 1755 1044 1755 836 1755 3362 1756 3248 1756 836 1756 1048 1757 194 1757 3774 1757 3875 1230 1048 1230 3774 1230 3249 1758 3248 1758 3362 1758 804 1230 3249 1230 3362 1230 1047 1759 1048 1759 3875 1759 132 1230 1047 1230 3875 1230 4059 1760 3249 1760 804 1760 4132 1230 4059 1230 804 1230 1050 1710 1047 1710 132 1710 134 1761 4059 1761 4132 1761 136 1230 1050 1230 132 1230 4056 1230 134 1230 4132 1230 1052 1230 1050 1230 136 1230 3579 1230 1052 1230 136 1230 189 1762 4123 1762 134 1762 189 1230 134 1230 4056 1230 3383 1230 1052 1230 3579 1230 3580 1763 3383 1763 3579 1763 1054 1764 1058 1764 4123 1764 1054 1765 4123 1765 189 1765 3813 1766 3383 1766 3580 1766 3583 1767 3813 1767 3580 1767 3556 1230 4206 1230 1058 1230 3556 1768 1058 1768 1054 1768 3814 1769 3813 1769 3583 1769 4083 1721 3814 1721 3583 1721 190 1770 1059 1770 4206 1770 190 1771 4206 1771 3556 1771 3853 1230 3814 1230 4083 1230 3979 1230 3853 1230 4083 1230 3847 1772 138 1772 1059 1772 3847 1773 1059 1773 190 1773 1060 1230 3853 1230 3979 1230 3918 1774 1060 1774 3979 1774 3848 1230 140 1230 138 1230 3848 1775 138 1775 3847 1775 3811 1230 1060 1230 3918 1230 1062 1776 3811 1776 3918 1776 199 1230 141 1230 140 1230 199 1230 140 1230 3848 1230 1063 1728 3811 1728 1062 1728 155 1730 1063 1730 1062 1730 204 1230 147 1230 141 1230 204 1777 141 1777 199 1777 4233 1230 1063 1230 155 1230 1064 1778 4233 1778 155 1778 202 1779 148 1779 147 1779 202 1780 147 1780 204 1780 1065 1230 1064 1230 1067 1230 1065 1230 4233 1230 1064 1230 209 1230 152 1230 148 1230 209 1781 148 1781 202 1781 213 1782 152 1782 209 1782 213 1230 1067 1230 152 1230 213 1230 1065 1230 1067 1230 3747 1783 3231 1784 3233 1785 847 1431 3747 1783 3233 1785 847 1431 3233 1785 3164 1786 847 1431 3164 1786 3794 1330 3805 337 3794 1330 3165 1787 3805 337 3165 1787 2018 338 2071 1788 3054 1788 3623 1788 3054 1788 3055 1788 3623 1788 3055 1788 3867 1788 3623 1788 3867 1788 2144 1788 3623 1788 2144 1788 3029 1788 3623 1788 3029 1788 3028 1788 3623 1788 3028 1788 2201 1788 3623 1788 2201 1789 3642 1789 3623 1789 3642 1788 1072 1788 3623 1788 1072 1788 4064 1788 3623 1788 4064 1788 3974 1788 3623 1788 3974 1788 2386 1788 3623 1788 2386 1788 3251 1788 3623 1788 3251 1788 3178 1788 3623 1788 3178 1790 2440 1790 3623 1790 2440 1788 2434 1788 3623 1788 2434 1791 2450 1791 3623 1791 2450 1788 1074 1788 3623 1788 1074 1788 4242 1788 3623 1788 4242 1788 3587 1788 3623 1788 3587 1788 3586 1788 3623 1788 3586 1788 2562 1788 3623 1788 2562 1788 1075 1788 3623 1788 1075 1788 3972 1788 3623 1788 3972 1792 2696 1792 3623 1792 2696 1793 3562 1793 3623 1793 3562 1794 2156 1794 3623 1794 2156 1788 4110 1788 3623 1788 4110 1788 2438 1788 3623 1788 2438 1795 2388 1795 3623 1795 2388 1796 3350 1796 3623 1796 3350 1788 3840 1788 3623 1788 3840 1797 3761 1797 3623 1797 3761 1798 3762 1798 3623 1798 3762 1788 2524 1788 3623 1788 2524 1799 3427 1799 3623 1799 3427 1788 3313 1788 3623 1788 3313 1800 2576 1800 3623 1800 2576 1801 2588 1801 3623 1801 2588 1788 2602 1788 3623 1788 2602 1788 1078 1788 3623 1788 1078 1788 4241 1788 3623 1788 4241 1788 2643 1788 3623 1788 3413 1788 1222 1788 3623 1788 1217 1788 3413 1788 3623 1788 1079 1802 1217 1802 3623 1802 1218 1788 1079 1788 3623 1788 3202 1788 1218 1788 3623 1788 3217 1803 3202 1803 3623 1803 1249 1788 3217 1788 3623 1788 1257 1788 1249 1788 3623 1788 3652 1804 1257 1804 3623 1804 3566 1788 3652 1788 3623 1788 3567 1805 3566 1805 3623 1805 1313 1788 3567 1788 3623 1788 1479 1788 1313 1788 3623 1788 1543 1788 1479 1788 3623 1788 3570 1788 1543 1788 3623 1788 1706 1788 3570 1788 3623 1788 4077 1806 1706 1806 3623 1806 1787 1788 4077 1788 3623 1788 1796 1807 1787 1807 3623 1807 3558 1788 1796 1788 3623 1788 3013 1788 3558 1788 3623 1788 3012 1788 3013 1788 3623 1788 1275 1788 3012 1788 3623 1788 1269 1788 1275 1788 3623 1788 3019 1788 1269 1788 3623 1788 3018 1808 3019 1808 3623 1808 1808 1809 3018 1809 3623 1809 3594 1788 1808 1788 3623 1788 3243 1788 3594 1788 3623 1788 3244 1788 3243 1788 3623 1788 3539 1810 3244 1810 3623 1810 1309 1811 3539 1811 3623 1811 4046 1788 1309 1788 3623 1788 1462 1788 4046 1788 3623 1788 3978 1812 1462 1812 3623 1812 1582 1813 3978 1813 3623 1813 3775 1788 1582 1788 3623 1788 1666 1814 3775 1814 3623 1814 1603 1788 1666 1788 3623 1788 1793 1815 1603 1815 3623 1815 1811 1816 1793 1816 3623 1816 3027 1788 1811 1788 3623 1788 1828 1788 3027 1788 3623 1788 1859 1788 1828 1788 3623 1788 2643 1788 1859 1788 3623 1788 2764 1817 2763 1818 4248 1601 2763 1818 4223 1819 3278 1629 4248 1820 2763 1820 3278 1820 4223 1819 3660 1821 3278 1629 2764 1817 4248 1601 2768 1590 2722 1822 977 1678 3694 1677 3278 1629 3660 1821 4188 1630 2722 1822 3694 1677 2733 1823 4199 1451 3604 1824 3053 1467 3053 1467 3604 1824 3603 1825 2774 1826 2768 1590 2712 1584 1087 1650 2747 1638 3863 1827 2733 1823 3694 1677 3032 1828 2712 1584 1089 1576 2716 1829 1087 1650 3863 1827 1090 1655 2733 1823 3032 1828 1093 1830 3053 1467 3603 1825 1092 1487 2716 1829 1089 1576 2718 1831 1092 1487 3603 1825 3807 1832 1093 1830 3032 1828 2746 1833 2718 1831 1089 1576 3368 1685 1092 1487 3807 1832 1094 1501 2746 1833 3032 1828 962 1648 1094 1501 3807 1832 4294 1834 3368 1685 976 1684 3370 1835 1094 1501 4294 1834 4174 1836 1096 1465 1097 1837 866 1466 1094 1501 4174 1836 1101 1512 2746 1833 962 1648 947 1626 3370 1838 976 1838 4205 1838 866 1466 1097 1837 865 1471 4205 1839 976 1839 3280 1839 1101 1512 4174 1836 2801 1840 4205 1841 3280 1841 4204 1841 4204 1842 3280 1842 973 1842 1101 1512 2801 1840 2797 1843 1101 1512 2797 1843 3222 1532 4204 1844 973 1844 4134 1844 947 1626 939 1608 2784 1845 4134 1846 973 1846 3494 1846 3222 1532 2797 1843 2789 1847 2784 1845 939 1608 932 1544 3494 1848 973 1848 1104 1848 3494 1849 1104 1849 3457 1849 3222 1532 2789 1847 3225 1567 2784 1845 932 1544 2791 1850 3225 1567 2789 1847 2786 1851 3457 1852 1104 1852 3486 1852 3486 1853 1104 1853 952 1853 2791 1850 932 1544 4109 1529 3225 1567 2786 1851 3758 1437 3486 1854 952 1854 3234 1854 3758 1437 2786 1851 2783 1438 4109 1529 894 1515 1108 1855 2783 1438 1107 1856 852 1444 3234 1857 952 1858 2758 1859 2758 1859 952 1858 943 1605 1108 1855 894 1515 2810 1860 894 1515 884 1496 2810 1860 852 1444 1107 1856 2834 1861 1110 1450 852 1444 2834 1861 2758 1859 943 1605 2762 1602 4069 1547 4315 1862 1109 1548 1110 1450 2834 1861 2831 1863 1111 1456 1110 1450 2831 1863 2810 1860 884 1496 2818 1495 1111 1456 2831 1863 2825 1864 2762 1602 922 1589 3107 1865 1112 1494 1111 1456 2825 1864 2818 1495 1112 1494 2825 1864 3107 1865 922 1589 2772 1866 906 1555 3100 1867 3884 1559 2772 1866 922 1589 4274 1582 2772 1866 4274 1582 3108 1868 3884 1869 3100 1869 2779 1869 3108 1868 4274 1582 2715 1870 3884 1871 2779 1871 3326 1871 3884 1559 3326 1872 855 1443 2715 1870 4274 1582 4275 1873 855 1443 3326 1872 3325 1874 2715 1870 4275 1873 2719 1875 855 1443 3325 1874 3602 1876 2719 1875 4275 1873 2722 1822 855 1443 3602 1876 4199 1451 2722 1822 4275 1873 977 1678 4170 1788 1156 1788 3745 1877 4170 1788 4171 1878 1156 1788 2898 293 1009 293 3822 1879 3682 1877 4170 1788 3745 1877 4304 1879 3822 1879 3943 1880 4304 1879 2898 293 3822 1879 3681 1881 3745 1877 1013 1881 3681 1881 1013 1881 1015 1882 3681 1881 3682 1877 3745 1877 3504 1880 3943 1880 3868 1883 3504 1880 4304 1879 3943 1880 2849 1882 1015 1882 3590 1884 2849 1882 3681 1881 1015 1882 1121 1883 3504 1880 3868 1883 1121 1883 3868 1883 1001 1885 3542 1884 2849 1882 3590 1884 3463 1885 1001 1885 3680 1886 3624 1887 3590 1884 1122 1887 3463 1885 1121 1883 1001 1885 3624 1887 3542 1884 3590 1884 3163 1886 3463 1885 3680 1886 3625 1888 1122 1887 1022 1888 3625 1888 1022 1888 3862 1889 3625 1888 3624 1887 1122 1887 3162 1890 3163 1886 3680 1886 3162 1890 3680 1886 997 1890 1129 1889 3862 1889 3733 1891 1129 1889 3625 1888 3862 1889 3357 1892 997 1890 994 1892 3357 1892 994 1892 3388 1893 3357 1892 3162 1890 997 1890 1133 1891 1129 1889 3733 1891 3358 1893 3357 1892 3388 1893 3715 1894 1133 1891 3733 1891 3715 1894 3733 1891 1135 1894 3715 1894 1135 1894 1029 1895 4258 1896 3388 1893 988 1897 4258 1896 988 1897 4266 1898 4258 1896 3358 1893 3388 1893 1137 1895 3715 1894 1029 1895 1137 1895 1029 1895 4156 1899 2878 1898 4266 1898 985 1900 2878 1898 4258 1896 4266 1898 1139 1899 1137 1895 4156 1899 3082 1900 2878 1898 985 1900 3959 1901 1139 1899 4156 1899 3959 1901 4156 1899 4157 1902 2874 1903 985 1900 3119 1903 2874 1903 3082 1900 985 1900 3960 1904 3959 1901 4157 1902 3960 1904 4157 1902 1034 1904 3960 1904 1034 1904 1145 1905 4137 1906 3119 1903 1143 1906 4137 1906 1143 1906 1144 1907 4137 1906 2874 1903 3119 1903 2841 1908 3960 1904 1145 1905 2841 1908 1145 1905 1039 1909 3905 1907 1144 1907 3540 1910 3905 1907 4137 1906 1144 1907 2844 1909 2841 1908 1039 1909 2844 1909 1039 1909 1150 1911 3904 1910 3905 1907 3540 1910 1149 1911 2844 1909 1150 1911 3923 1912 3540 1910 3541 1912 3923 1912 3541 1912 1148 1913 3923 1912 3904 1910 3540 1910 4138 1914 1150 1911 3284 1914 4138 1914 1149 1911 1150 1911 3922 1913 1148 1913 3655 1915 3922 1913 3923 1912 1148 1913 2848 1916 3284 1914 1046 1916 2848 1916 4138 1914 3284 1914 2848 1916 1046 1916 1152 1917 2862 1915 3922 1913 3655 1915 4281 1917 1152 1917 1154 1918 4281 1917 2848 1916 1152 1917 4308 1919 3655 1915 1153 1919 4308 1919 1153 1919 3548 1878 4308 1919 2862 1915 3655 1915 4230 1918 4281 1917 1154 1918 4082 298 1154 1918 4058 298 4082 298 4230 1918 1154 1918 4171 1878 3548 1878 1156 1788 4171 1878 4308 1919 3548 1878 1167 1920 2000 1921 1999 1922 1167 1920 1993 1923 2000 1921 2950 1924 2949 1925 1158 1926 1997 1927 1973 1928 1935 1929 1178 1930 1080 1931 3913 1932 1163 1933 1996 1934 1718 1935 1163 1933 1718 1935 1937 1936 1167 1920 1999 1922 1994 1937 1160 1938 2001 1939 1991 1940 1162 1941 1160 1938 1992 1942 1162 1941 1975 1943 1996 1934 1162 1941 1992 1942 1975 1943 1162 1941 1996 1934 1163 1933 2003 1944 1994 1937 1978 1945 1167 1920 2926 1946 1993 1923 1201 1947 1164 1948 1987 1949 1201 1947 1987 1949 1165 1950 1166 1951 1161 1952 2001 1939 1173 1953 2001 1939 1160 1938 1181 1954 1162 1941 1163 1933 1169 1955 1168 1956 1953 1957 1169 1955 1953 1957 1971 1958 1172 1959 1161 1952 1166 1951 1228 1960 1981 1961 1168 1956 1172 1959 1170 1962 1161 1952 1175 1963 2003 1944 2002 1964 1184 1965 1173 1953 1160 1938 1171 1966 1165 1950 1959 1967 1179 1968 1163 1933 1937 1936 1179 1968 1937 1936 1941 1969 1179 1968 1941 1969 1949 1970 1187 1971 1162 1941 1181 1954 1187 1971 1184 1965 1160 1938 1187 1971 1160 1938 1162 1941 1172 1959 2002 1964 1170 1962 2003 1944 1167 1920 1994 1937 1183 1972 1158 1926 1933 1973 1173 1953 1166 1951 2001 1939 1186 1974 1175 1963 2002 1964 1186 1974 2002 1964 1172 1959 1205 1975 1167 1920 2003 1944 1192 1976 1949 1970 1768 1977 1192 1976 1179 1968 1949 1970 1166 1951 1186 1974 1172 1959 1180 1978 1173 1953 1184 1965 1174 1979 2003 1944 1175 1963 1271 1980 1158 1926 1183 1972 1191 1981 1186 1974 1166 1951 1188 1982 1251 1983 1176 1984 1185 1985 1166 1951 1173 1953 1188 1982 1176 1984 1177 1986 1174 1979 1205 1975 2003 1944 1997 1927 1188 1982 1159 1987 1190 1988 1163 1933 1179 1968 1190 1988 1179 1968 1192 1976 1190 1988 1181 1954 1163 1933 1204 1989 1175 1963 1186 1974 1171 1966 1201 1947 1165 1950 1180 1978 1185 1985 1173 1953 1189 1990 1187 1971 1181 1954 1197 1991 1192 1976 1768 1977 1182 1992 1183 1972 1164 1948 1205 1975 2926 1946 1167 1920 1203 1993 1184 1965 1187 1971 1254 1994 1228 1960 1168 1956 1203 1993 1180 1978 1184 1965 1194 1995 1190 1988 1192 1976 1204 1989 1174 1979 1175 1963 1244 1996 1694 1997 1646 1998 1205 1975 2930 1999 2926 1946 1197 1991 1768 1977 1961 2000 1221 2001 1168 1956 1169 1955 1206 2002 1204 1989 1186 1974 1228 1960 1171 1966 1981 1961 1206 2002 1186 1974 1191 1981 1208 2003 1197 1991 1961 2000 1212 2004 1935 1929 1694 1997 1189 1990 1203 1993 1187 1971 1211 2005 1185 1985 1180 1978 1198 2006 1181 1954 1190 1988 1198 2006 1189 1990 1181 1954 1212 2004 1997 1927 1935 1929 1198 2006 1190 1988 1194 1995 1182 1992 1271 1980 1183 1972 1191 1981 1166 1951 1185 1985 1258 2007 1244 1996 1646 1998 1226 2008 1201 1947 1171 1966 1194 1995 1192 1976 1197 1991 1193 2009 1961 2000 1966 2010 1193 2009 1966 2010 1972 2011 1193 2009 1208 2003 1961 2000 1195 2012 1196 2013 1082 2014 1247 2015 1971 1958 1995 2016 1247 2015 1995 2016 1741 2017 1247 2015 1169 1955 1971 1958 1207 2018 1191 1981 1185 1985 1207 2018 1185 1985 1211 2005 1215 2019 1174 1979 1204 1989 1216 2020 1194 1995 1197 1991 1214 2021 1203 1993 1189 1990 1199 2022 1204 1989 1206 2002 1200 2023 1193 2009 1972 2011 1200 2023 1972 2011 1573 2024 1215 2019 1205 1975 1174 1979 1209 2025 1211 2005 1180 1978 1209 2025 1180 1978 1203 1993 1386 2026 1196 2013 1195 2012 1199 2022 1215 2019 1204 1989 1216 2020 1197 1991 1208 2003 1244 1996 1212 2004 1694 1997 1210 2027 1191 1981 1207 2018 1210 2027 1206 2002 1191 1981 1214 2021 1209 2025 1203 1993 1198 2006 1194 1995 1216 2020 1193 2009 1216 2020 1208 2003 1213 2028 1189 1990 1198 2006 1213 2028 1214 2021 1189 1990 1342 2029 1386 2026 1195 2012 1221 2001 1254 1994 1168 1956 1210 2027 1199 2022 1206 2002 1237 2030 1164 1948 1201 1947 1215 2019 2930 1999 1205 1975 1271 1980 2950 1924 1158 1926 1232 2031 1210 2027 1207 2018 1268 2032 1188 1982 1997 1927 1220 2033 1213 2028 1198 2006 1248 2034 1171 1966 1228 1960 1215 2019 2929 2035 2930 1999 1255 2036 1212 2004 1244 1996 1209 2025 1207 2018 1211 2005 1247 2015 1741 2017 1740 2037 1220 2033 1198 2006 1216 2020 1243 2038 1209 2025 1214 2021 1268 2032 1251 1983 1188 1982 1250 2039 1220 2033 1216 2020 1233 2040 1207 2018 1209 2025 1271 1980 2954 2041 2950 1924 1250 2039 1216 2020 1193 2009 1226 2008 1237 2030 1201 1947 3331 2042 1241 2043 1231 2044 1408 2045 1241 2043 3331 2042 1234 2046 1199 2022 1210 2027 1243 2038 1233 2040 1209 2025 1252 2047 3331 2042 3317 2048 1234 2046 1215 2019 1199 2022 1219 2049 3317 2048 3201 2050 1234 2046 1210 2027 1232 2031 1230 2051 1250 2039 1193 2009 1230 2051 1193 2009 1573 2024 1235 2052 1214 2021 1213 2028 1235 2052 1243 2038 1214 2021 1235 2052 1213 2028 1220 2033 1294 2053 1221 2001 1169 1955 1356 2054 1225 2055 3412 2056 1224 2057 1356 2054 3412 2056 1245 2058 1997 1927 1212 2004 1281 2059 1169 1955 1247 2015 1223 2060 3201 2050 1227 2061 1248 2034 1226 2008 1171 1966 1229 2062 1224 2057 3412 2056 1229 2062 3412 2056 1231 2044 1229 2062 1356 2054 1224 2057 1233 2040 1232 2031 1207 2018 1266 2063 1254 1994 1221 2001 1271 1980 1270 2064 2954 2041 1259 2065 1850 2066 1827 2067 1236 2068 1229 2062 1231 2044 1265 2069 1235 2052 1220 2033 1358 2070 1231 2044 1241 2043 1358 2070 1236 2068 1231 2044 1239 2071 1232 2031 1233 2040 1245 2058 1268 2032 1997 1927 1240 2072 1358 2070 1241 2043 1239 2071 1234 2046 1232 2031 1238 2073 1227 2061 1242 2074 2934 2075 1215 2019 1234 2046 1237 2030 1182 1992 1164 1948 1256 2076 1239 2071 1233 2040 1256 2076 1233 2040 1243 2038 2934 2075 2929 2035 1215 2019 1408 2045 1240 2072 1241 2043 1250 2039 1265 2069 1220 2033 1410 2077 1408 2045 3331 2042 1347 2078 1242 2074 3617 2079 1262 2080 1243 2038 1235 2052 1246 2081 1195 2012 1282 2082 1246 2081 1342 2029 1195 2012 1454 2083 1410 2077 3331 2042 1454 2083 3331 2042 1252 2047 1294 2053 1266 2063 1221 2001 1267 2084 1234 2046 1239 2071 1445 2085 3617 2079 3618 2086 1237 2030 1271 1980 1182 1992 1260 2087 1252 2047 3317 2048 1260 2087 1454 2083 1252 2047 1261 2088 1262 2080 1235 2052 1260 2087 3317 2048 1219 2049 1308 2089 1259 2065 1827 2067 1253 2090 1250 2039 1230 2051 1266 2063 1228 1960 1254 1994 1255 2036 1245 2058 1212 2004 1291 2091 1244 1996 1258 2007 1256 2076 1267 2084 1239 2071 1262 2080 1256 2076 1243 2038 1544 2092 1546 2093 1874 2094 1264 2095 3618 2086 1277 2096 1261 2088 1235 2052 1265 2069 1274 2097 1230 2051 1450 2098 1481 2099 1260 2087 1219 2049 1265 2069 1250 2039 1253 2090 1285 2100 1281 2059 1247 2015 1292 2101 1256 2076 1262 2080 1285 2100 1259 2065 1308 2089 1481 2099 1219 2049 3201 2050 1285 2100 1740 2037 1259 2065 1285 2100 1247 2015 1740 2037 1390 2102 1246 2081 1282 2082 1267 2084 2934 2075 1234 2046 1261 2088 1292 2101 1262 2080 1482 2103 3201 2050 1223 2060 1482 2103 1481 2099 3201 2050 1307 2104 1226 2008 1248 2034 1325 2105 1245 2058 1255 2036 1273 2106 1264 2095 1277 2096 1314 2107 1253 2090 1230 2051 1315 2108 1266 2063 1294 2053 1291 2091 1255 2036 1244 1996 2943 2109 2934 2075 1267 2084 1245 2058 2965 2110 1268 2032 1316 2111 1294 2053 1169 1955 1316 2111 1169 1955 1281 2059 1270 2064 1271 1980 1237 2030 1272 2112 1248 2034 1228 1960 1272 2112 1228 1960 1266 2063 1390 2102 1342 2029 1246 2081 3060 2113 1606 2114 3913 1932 1292 2101 1267 2084 1256 2076 1411 2115 1288 2116 1276 2117 1278 2118 1291 2091 1258 2007 1283 2119 1314 2107 1230 2051 1283 2119 1230 2051 1274 2097 1298 2120 1261 2088 1265 2069 1671 2121 1273 2106 1277 2096 1290 2122 1360 2123 1365 2124 1286 2125 1482 2103 1223 2060 1731 2126 1277 2096 1293 2127 1237 2030 2962 2128 1270 2064 1411 2115 1360 2123 1290 2122 1284 2129 1282 2082 4045 2130 1284 2129 1390 2102 1282 2082 1296 2131 1223 2060 1227 2061 1296 2131 1286 2125 1223 2060 1340 2132 1291 2091 1278 2118 1272 2112 1307 2104 1248 2034 1731 2126 1671 2121 1277 2096 1290 2122 1365 2124 1279 2133 1315 2108 1272 2112 1266 2063 1301 2134 1267 2084 1292 2101 1298 2120 1265 2069 1253 2090 1490 2135 1544 2092 1874 2094 1319 2136 1281 2059 1285 2100 1328 2137 1255 2036 1291 2091 1300 2138 1913 2139 3940 2140 1300 2138 1827 2067 1913 2139 2946 2141 1283 2119 1274 2097 1300 2138 1308 2089 1827 2067 1344 2142 1237 2030 1226 2008 1344 2142 1226 2008 1307 2104 1280 2143 2932 2144 1287 2145 1302 2146 1288 2116 1411 2115 1310 2147 1290 2122 1279 2133 1306 2148 1411 2115 1290 2122 1724 2149 1293 2127 1295 2150 1302 2146 1411 2115 1306 2148 1312 2151 1292 2101 1261 2088 1318 2152 1314 2107 1283 2119 1724 2149 1731 2126 1293 2127 1325 2105 2965 2110 1245 2058 1303 2153 1290 2122 1310 2147 1328 2137 1325 2105 1255 2036 1301 2134 2943 2109 1267 2084 1314 2107 1298 2120 1253 2090 1311 2154 1279 2133 1415 2155 1311 2154 1310 2147 1279 2133 1304 2156 2965 2110 1325 2105 1413 2157 1294 2053 1316 2111 1413 2157 1315 2108 1294 2053 1326 2158 1280 2143 1288 2116 1301 2134 2942 2159 2943 2109 1389 2160 1390 2102 1284 2129 1283 2119 2955 2161 1318 2152 1283 2119 2946 2141 2955 2161 1312 2151 1261 2088 1298 2120 1305 2162 1227 2061 1238 2073 2942 2159 1301 2134 1292 2101 1305 2162 1296 2131 1227 2061 1317 2163 1311 2154 1415 2155 1340 2132 1328 2137 1291 2091 1317 2163 1415 2155 1297 2164 1299 2165 1300 2138 3940 2140 2924 2166 2932 2144 1280 2143 1359 2167 1724 2149 1295 2150 1302 2146 1326 2158 1288 2116 1326 2158 2924 2166 1280 2143 1303 2153 1306 2148 1290 2122 1339 2168 1238 2073 1242 2074 1345 2169 1340 2132 1278 2118 1339 2168 1305 2162 1238 2073 1339 2168 1242 2074 1347 2078 2962 2128 1237 2030 1344 2142 1398 2170 1307 2104 1272 2112 4045 2130 1389 2160 1284 2129 1337 2171 1308 2089 1300 2138 1337 2171 1300 2138 1299 2165 1337 2171 1319 2136 1285 2100 1337 2171 1285 2100 1308 2089 1320 2172 2942 2159 1292 2101 1312 2151 1320 2172 1292 2101 1323 2173 1310 2147 1311 2154 1323 2173 1303 2153 1310 2147 1323 2173 1311 2154 1317 2163 1355 2174 1328 2137 1340 2132 1432 2175 1389 2160 4045 2130 1324 2176 1306 2148 1303 2153 1322 2177 1312 2151 1298 2120 1322 2177 1298 2120 1314 2107 1322 2177 1314 2107 1318 2152 1398 2170 1344 2142 1307 2104 1393 2178 1340 2132 1345 2169 1723 2179 1295 2150 3697 2180 1350 2181 1315 2108 1413 2157 1334 2182 1316 2111 1281 2059 1353 2183 1323 2173 1317 2163 1334 2182 1281 2059 1319 2136 1388 2184 1345 2169 1330 2185 1312 2151 1321 2186 2960 2187 1312 2151 2960 2187 1320 2172 1312 2151 2963 2188 1321 2186 1312 2151 1322 2177 2963 2188 1723 2179 1359 2167 1295 2150 1449 2189 1723 2179 3697 2180 1354 2190 1306 2148 1324 2176 1326 2158 1417 2191 2924 2166 1327 2192 1297 2164 1455 2193 1327 2192 1353 2183 1317 2163 1327 2192 1317 2163 1297 2164 1327 2192 1455 2193 1457 2194 1433 2195 1432 2175 4045 2130 1376 2196 1323 2173 1353 2183 1379 2197 1574 2198 1520 2199 1349 2200 1325 2105 1328 2137 1332 2201 1329 2202 1470 2203 1350 2181 1272 2112 1315 2108 1332 2201 1574 2198 1379 2197 1468 2204 1388 2184 1330 2185 1332 2201 1658 2205 1574 2198 1332 2201 1379 2197 1329 2202 1331 2206 1470 2203 1418 2207 1331 2206 1332 2201 1470 2203 1331 2206 1710 2208 1658 2205 1331 2206 1658 2205 1332 2201 1362 2209 1326 2158 1302 2146 1333 2210 1331 2206 1418 2207 1333 2210 1710 2208 1331 2206 1626 2211 1339 2168 1347 2078 1450 2098 1230 2051 1573 2024 1349 2200 1304 2156 1325 2105 1493 2212 1302 2146 1306 2148 1380 2213 1303 2153 1323 2173 1380 2213 1323 2173 1376 2196 1367 2214 1433 2195 4045 2130 1380 2213 1324 2176 1303 2153 1361 2215 1225 2055 1356 2054 1422 2216 3060 2113 1346 2217 1437 2218 3940 2140 1746 2219 1437 2218 1299 2165 3940 2140 1178 1930 1565 2220 1080 1931 1403 2221 1327 2192 1457 2194 1336 2222 1335 2223 1371 2224 1336 2222 1371 2224 1557 2225 1452 2226 1319 2136 1337 2171 1343 2227 1344 2142 1398 2170 1338 2228 1370 2229 1336 2222 1338 2228 1336 2222 1557 2225 1338 2228 1374 2230 1370 2229 1480 2231 1272 2112 1350 2181 1393 2178 1355 2174 1340 2132 1375 2232 1374 2230 1341 2233 1375 2232 1329 2202 1379 2197 1375 2232 1341 2233 1329 2202 1373 2234 3697 2180 4250 2235 1362 2209 1302 2146 1493 2212 1388 2184 1393 2178 1345 2169 1628 2236 1626 2211 1347 2078 1628 2236 1347 2078 3617 2079 1394 2237 1348 2238 1533 2239 1394 2237 1533 2239 1384 2240 1351 2241 1418 2207 1533 2239 1351 2241 1333 2210 1418 2207 1352 2242 1351 2241 1533 2239 1352 2242 1533 2239 1348 2238 1397 2243 1351 2241 1352 2242 1438 2244 1433 2195 1367 2214 1377 2245 1353 2183 1327 2192 1377 2245 1376 2196 1353 2183 1377 2245 1327 2192 1403 2221 1397 2243 1333 2210 1351 2241 1402 2246 1333 2210 1397 2243 1380 2213 1354 2190 1324 2176 1402 2246 1710 2208 1333 2210 1402 2246 1782 2247 1744 2248 1573 2024 1193 2009 1200 2023 1400 2249 1403 2221 1457 2194 1343 2227 2962 2128 1344 2142 1373 2234 1449 2189 3697 2180 1357 2250 1361 2215 1356 2054 1357 2250 1356 2054 1229 2062 1407 2251 1358 2070 1240 2072 1407 2251 1229 2062 1236 2068 1407 2251 1236 2068 1358 2070 1407 2251 1357 2250 1229 2062 1407 2251 1240 2072 1408 2045 1354 2190 1493 2212 1306 2148 1368 2252 1491 2253 1998 2254 1362 2209 1417 2191 1326 2158 1412 2255 1225 2055 1361 2215 1412 2255 1361 2215 1364 2256 1412 2255 1364 2256 1360 2123 1363 2257 1361 2215 1357 2250 1363 2257 1357 2250 1407 2251 1364 2256 1365 2124 1360 2123 1524 2258 1316 2111 1334 2182 1524 2258 1413 2157 1316 2111 1364 2256 1361 2215 1363 2257 1366 2259 1364 2256 1363 2257 1366 2259 1279 2133 1365 2124 1366 2259 1415 2155 1279 2133 1366 2259 1365 2124 1364 2256 1505 2260 1380 2213 1376 2196 1477 2261 1367 2214 3977 2262 1493 2212 1354 2190 1460 2263 1422 2216 1335 2223 1336 2222 1464 2264 1178 1930 3913 1932 1378 2265 1377 2245 1403 2221 1424 2266 1202 2267 1501 2268 1424 2266 1369 2269 1202 2267 1565 2220 1368 2252 1998 2254 1369 2269 1336 2222 1370 2229 1430 2270 1328 2137 1355 2174 1372 2271 1559 2272 1557 2225 1372 2271 1371 2224 1909 2273 1372 2271 1557 2225 1371 2224 1428 2274 1379 2197 1520 2199 1335 2223 1422 2216 1346 2217 1428 2274 1374 2230 1375 2232 1335 2223 1346 2217 1081 2275 1456 2276 1354 2190 1380 2213 1456 2276 1380 2213 1505 2260 1428 2274 1375 2232 1379 2197 1385 2277 1381 2278 1382 2279 1383 2280 1746 2219 1686 2281 1383 2280 1437 2218 1746 2219 1387 2282 1386 2026 1342 2029 1563 2283 1388 2184 1468 2204 1387 2282 1394 2237 1384 2240 1387 2282 1384 2240 1385 2277 1387 2282 1382 2279 1386 2026 1387 2282 1385 2277 1382 2279 1387 2282 1342 2029 1392 2284 1387 2282 1392 2284 1394 2237 1430 2270 1349 2200 1328 2137 1389 2160 1342 2029 1390 2102 1389 2160 1392 2284 1342 2029 1480 2231 1398 2170 1272 2112 1590 2285 1449 2189 1373 2234 1477 2261 1438 2244 1367 2214 1404 2286 2962 2128 1343 2227 1448 2287 1452 2226 1337 2171 1448 2287 1299 2165 1437 2218 1448 2287 1337 2171 1299 2165 1391 2288 1393 2178 1388 2184 1436 2289 1389 2160 1432 2175 1435 2290 1392 2284 1389 2160 1435 2290 1389 2160 1436 2289 1395 2291 1392 2284 1435 2290 1395 2291 1348 2238 1394 2237 1395 2291 1394 2237 1392 2284 1441 2292 1395 2291 1435 2290 1395 2291 1396 2293 1348 2238 1395 2291 1399 2294 1396 2293 1441 2292 1446 2295 1399 2294 1441 2292 1399 2294 1395 2291 1441 2292 1522 2296 1446 2295 1396 2293 1352 2242 1348 2238 1396 2293 1397 2243 1352 2242 1480 2231 1343 2227 1398 2170 1406 2297 1397 2243 1396 2293 1406 2297 1396 2293 1399 2294 1401 2298 1402 2246 1397 2243 1401 2298 1782 2247 1402 2246 1400 2249 1378 2265 1403 2221 1343 2227 1507 2299 1404 2286 1401 2298 1397 2243 1406 2297 1405 2300 1785 2301 1782 2247 1405 2300 1401 2298 1406 2297 1405 2300 1782 2247 1401 2298 1409 2302 1410 2077 1454 2083 1409 2302 1407 2251 1408 2045 1409 2302 1363 2257 1407 2251 1409 2302 1408 2045 1410 2077 1451 2303 1334 2182 1319 2136 1411 2115 1412 2255 1360 2123 1673 2304 3617 2079 1445 2085 1451 2303 1319 2136 1452 2226 1225 2055 1276 2117 1412 2255 1412 2255 1411 2115 1276 2117 1524 2258 1350 2181 1413 2157 1673 2304 1628 2236 3617 2079 1536 2305 1363 2257 1409 2302 1554 2306 1377 2245 1378 2265 1414 2307 1363 2257 1536 2305 1414 2307 1455 2193 1297 2164 1414 2307 1415 2155 1366 2259 1414 2307 1366 2259 1363 2257 1414 2307 1297 2164 1415 2155 1563 2283 1391 2288 1388 2184 1464 2264 1606 2114 1416 2308 1493 2212 2927 2309 1417 2191 1493 2212 1417 2191 1362 2209 1606 2114 1464 2264 3913 1932 1465 2310 1551 2311 1420 2312 1502 2313 1377 2245 1554 2306 1484 2314 1496 2315 1419 2316 1502 2313 1376 2196 1377 2245 1502 2313 1505 2260 1376 2196 1423 2317 1416 2308 1424 2266 1500 2318 1465 2310 1423 2317 1500 2318 1551 2311 1465 2310 1421 2319 1369 2269 1424 2266 1421 2319 1336 2222 1369 2269 1421 2319 1416 2308 1606 2114 1421 2319 1422 2216 1336 2222 1421 2319 1606 2114 1422 2216 1421 2319 1424 2266 1416 2308 1423 2317 1501 2268 1500 2318 1484 2314 1419 2316 1428 2274 1423 2317 1424 2266 1501 2268 3919 2320 1382 2279 1381 2278 3919 2320 1196 2013 1386 2026 3919 2320 1386 2026 1382 2279 1426 2321 1374 2230 1428 2274 1426 2321 1370 2229 1374 2230 1426 2321 1369 2269 1370 2229 1427 2322 1426 2321 1428 2274 1440 2323 1477 2261 3977 2262 1419 2316 1496 2315 1468 2204 1419 2316 1427 2322 1428 2274 1429 2324 1385 2277 1384 2240 1429 2324 1533 2239 1418 2207 1429 2324 1418 2207 1470 2203 1429 2324 1384 2240 1533 2239 1489 2325 1400 2249 1431 2326 1448 2287 1437 2218 1383 2280 1434 2327 1432 2175 1433 2195 1434 2327 1436 2289 1432 2175 1434 2327 1435 2290 1436 2289 1438 2244 1434 2327 1433 2195 1590 2285 1373 2234 4250 2235 1475 2328 1438 2244 1477 2261 1504 2329 1350 2181 1524 2258 1504 2329 1480 2231 1350 2181 1474 2330 1441 2292 1435 2290 1474 2330 1522 2296 1441 2292 1474 2330 1521 2331 1522 2296 1443 2332 1442 2333 1521 2331 1443 2332 1521 2331 1474 2330 1444 2334 1452 2226 1448 2287 1461 2335 1355 2174 1393 2178 1461 2335 1430 2270 1355 2174 1537 2336 1445 2085 3618 2086 1446 2295 1406 2297 1399 2294 1537 2336 1673 2304 1445 2085 1447 2337 1405 2300 1406 2297 1447 2337 1785 2301 1405 2300 1447 2337 1807 2338 1785 2301 1447 2337 1812 2339 1807 2338 1497 2340 1448 2287 1383 2280 1497 2340 1687 2341 1263 2342 1497 2340 1383 2280 1686 2281 1520 2199 1484 2314 1428 2274 1497 2340 1686 2281 1687 2341 1453 2343 1536 2305 1409 2302 1453 2343 1409 2302 1454 2083 1453 2343 1454 2083 1260 2087 1487 2344 1455 2193 1414 2307 1487 2344 1457 2194 1455 2193 1458 2345 1456 2276 1505 2260 1494 2346 1502 2313 1554 2306 1496 2315 1563 2283 1468 2204 1459 2347 1565 2220 1466 2348 1459 2347 1368 2252 1565 2220 1459 2347 1491 2253 1368 2252 1529 2349 1354 2190 1456 2276 1529 2349 1460 2263 1354 2190 1492 2350 1459 2347 1466 2348 1507 2299 1343 2227 1480 2231 1463 2351 1420 2312 1594 2352 1463 2351 1466 2348 1465 2310 1463 2351 1465 2310 1420 2312 1463 2351 1492 2350 1466 2348 1464 2264 1423 2317 1465 2310 1464 2264 1465 2310 1466 2348 1464 2264 1466 2348 1565 2220 1464 2264 1416 2308 1423 2317 3334 2353 3335 2353 1425 2353 1471 2354 1440 2323 3977 2262 1202 2267 1426 2321 1427 2322 1202 2267 1369 2269 1426 2321 1499 2355 1202 2267 1427 2322 1575 2356 1554 2306 1378 2265 1575 2356 1378 2265 1400 2249 1510 2357 1499 2355 1427 2322 1510 2357 1427 2322 1419 2316 1510 2357 1419 2316 1468 2204 1575 2356 1400 2249 1489 2325 1578 2358 4250 2235 4190 2359 1510 2357 1468 2204 1330 2185 1469 2360 1508 2361 1381 2278 1469 2360 1385 2277 1429 2324 1469 2360 1381 2278 1385 2277 1470 2203 1469 2360 1429 2324 1476 2362 1440 2323 1471 2354 1476 2362 1477 2261 1440 2323 1472 2363 1435 2290 1434 2327 1472 2363 1474 2330 1435 2290 1472 2363 1438 2244 1475 2328 1472 2363 1434 2327 1438 2244 1472 2363 1475 2328 1473 2364 1472 2363 1443 2332 1474 2330 1472 2363 1473 2364 1443 2332 1493 2212 2928 2365 2927 2309 1476 2362 1475 2328 1477 2261 1476 2362 1473 2364 1475 2328 1391 2288 1461 2335 1393 2178 1541 2366 1489 2325 1431 2326 1542 2367 1496 2315 1484 2314 1478 2368 1442 2333 1443 2332 1478 2368 1443 2332 1473 2364 1591 2369 1563 2283 1496 2315 1578 2358 1682 2370 4250 2235 1478 2368 1566 2371 1745 2372 1478 2368 1745 2372 1442 2333 1514 2373 1566 2371 1478 2368 1514 2373 1780 2374 1566 2371 1444 2334 1451 2303 1452 2226 1514 2373 1611 2375 1780 2374 1528 2376 1529 2349 1456 2276 1528 2376 1456 2276 1458 2345 1532 2377 1812 2339 1447 2337 1586 2378 1524 2258 1334 2182 1586 2378 1334 2182 1451 2303 1483 2379 1481 2099 1482 2103 1483 2379 1453 2343 1260 2087 1483 2379 1482 2103 1286 2125 1483 2379 1260 2087 1481 2099 1485 2380 1453 2343 1483 2379 1486 2381 1431 2326 1400 2249 1486 2381 1457 2194 1487 2344 1486 2381 1487 2344 1488 2382 1486 2381 1488 2382 1534 2383 1486 2381 1400 2249 1457 2194 1604 2384 1575 2356 1489 2325 1553 2385 1263 2342 1289 2386 1609 2387 1391 2288 1563 2283 1553 2385 1497 2340 1263 2342 1682 2370 1590 2285 4250 2235 1490 2135 1459 2347 1492 2350 1490 2135 1491 2253 1459 2347 1568 2388 1444 2334 1448 2287 1460 2263 1581 2389 1493 2212 1550 2390 1490 2135 1492 2350 1550 2390 1544 2092 1490 2135 1498 2391 1550 2390 1492 2350 1498 2391 1492 2350 1463 2351 1498 2391 1495 2392 1550 2390 1498 2391 1463 2351 1594 2352 1498 2391 1594 2352 1645 2393 1558 2394 1448 2287 1497 2340 1498 2391 1645 2393 1495 2392 1558 2394 1497 2340 1553 2385 1558 2394 1568 2388 1448 2287 1500 2318 1501 2268 1202 2267 1538 2395 1471 2354 3977 2262 1500 2318 1202 2267 1499 2355 1506 2396 1502 2313 1494 2346 1506 2396 1505 2260 1502 2313 1506 2396 1458 2345 1505 2260 1511 2397 1500 2318 1499 2355 1508 2361 1372 2271 1909 2273 1508 2361 1509 2398 1467 2399 1508 2361 3919 2320 1381 2278 1508 2361 1559 2272 1372 2271 1508 2361 1909 2273 1509 2398 1508 2361 1467 2399 3919 2320 1512 2400 1499 2355 1510 2357 1512 2400 1511 2397 1499 2355 1512 2400 1345 2169 1278 2118 1512 2400 1330 2185 1345 2169 1512 2400 1510 2357 1330 2185 1561 2401 1508 2361 1469 2360 1561 2401 1559 2272 1508 2361 1561 2401 1469 2360 1470 2203 1561 2401 1470 2203 1329 2202 1516 2402 1471 2354 1538 2395 1515 2403 1516 2402 1513 2404 1515 2403 1473 2364 1476 2362 1515 2403 1471 2354 1516 2402 1515 2403 1514 2373 1478 2368 1515 2403 1513 2404 1514 2373 1515 2403 1476 2362 1471 2354 1515 2403 1478 2368 1473 2364 1519 2405 1513 2404 1516 2402 1519 2405 1516 2402 1517 2406 1562 2407 1517 2406 3864 2408 1562 2407 3864 2408 1635 2409 1562 2407 1519 2405 1517 2406 1518 2410 1562 2407 1635 2409 1464 2264 1565 2220 1178 1930 1518 2410 1635 2409 1655 2411 1578 2358 4190 2359 1552 2412 1447 2337 1406 2297 1446 2295 1621 2413 1480 2231 1504 2329 1447 2337 1446 2295 1522 2296 1587 2414 1484 2314 1520 2199 1447 2337 1522 2296 1532 2377 1587 2414 1542 2367 1484 2314 1598 2415 1519 2405 1562 2407 1598 2415 1513 2404 1519 2405 1530 2416 1521 2331 1442 2333 1530 2416 1532 2377 1522 2296 1530 2416 1522 2296 1521 2331 1526 2417 1514 2373 1513 2404 1526 2417 1513 2404 1598 2415 1523 2418 1530 2416 1442 2333 1523 2418 1442 2333 1745 2372 1659 2419 1581 2389 1460 2263 1525 2420 1526 2417 1598 2415 1526 2417 1611 2375 1514 2373 1526 2417 1805 2421 1611 2375 1503 2422 1461 2335 1391 2288 1525 2420 1818 2423 1805 2421 1525 2420 1805 2421 1526 2417 1527 2424 1818 2423 1525 2420 1616 2425 1529 2349 1528 2376 1527 2424 1819 2426 1818 2423 1616 2425 1460 2263 1529 2349 1649 2427 1530 2416 1523 2418 1649 2427 1532 2377 1530 2416 1532 2377 1830 2428 1812 2339 1531 2429 1532 2377 1649 2427 1531 2429 1830 2428 1532 2377 1531 2429 1831 2430 1830 2428 1696 2431 1575 2356 1604 2384 1677 2432 3618 2086 1264 2095 1535 2433 1487 2344 1414 2307 1535 2433 1488 2382 1487 2344 1535 2433 1534 2383 1488 2382 1535 2433 1536 2305 1453 2343 1535 2433 1485 2380 1534 2383 1535 2433 1414 2307 1536 2305 1535 2433 1453 2343 1485 2380 1577 2434 1485 2380 1483 2379 1577 2434 1296 2131 1305 2162 1577 2434 1286 2125 1296 2131 1577 2434 1483 2379 1286 2125 1677 2432 1537 2336 3618 2086 1539 2435 1431 2326 1486 2381 1539 2435 1672 2436 1541 2366 1539 2435 1486 2381 1534 2383 1539 2435 1541 2366 1431 2326 1579 2437 1672 2436 1539 2435 1579 2437 1534 2383 1485 2380 1579 2437 1539 2435 1534 2383 1605 2438 1604 2384 1489 2325 1605 2438 1489 2325 1541 2366 1517 2406 1516 2402 1538 2395 1545 2439 1546 2093 1544 2092 1545 2439 1544 2092 1550 2390 1545 2439 1547 2440 1546 2093 1689 2441 1506 2396 1494 2346 1548 2442 1545 2439 1550 2390 1548 2442 1547 2440 1545 2439 1542 2367 1591 2369 1496 2315 1581 2389 2928 2365 1493 2212 1585 2443 1878 2444 1571 2445 1585 2443 1645 2393 1620 2446 1585 2443 1495 2392 1645 2393 1585 2443 1620 2446 1878 2444 1574 2198 1587 2414 1520 2199 1549 2447 1548 2442 1550 2390 1549 2447 1585 2443 1548 2442 1549 2447 1495 2392 1585 2443 1549 2447 1550 2390 1495 2392 1551 2311 1500 2318 1511 2397 1684 2448 1578 2358 1552 2412 1586 2378 1504 2329 1524 2258 1555 2449 1551 2311 1511 2397 1614 2450 1553 2385 1289 2386 1678 2451 1494 2346 1554 2306 1678 2451 1554 2306 1575 2356 1614 2450 1558 2394 1553 2385 1652 2452 1586 2378 1451 2303 1556 2453 1512 2400 1278 2118 1652 2452 1451 2303 1444 2334 1556 2453 1555 2449 1511 2397 1556 2453 1278 2118 1258 2007 1556 2453 1511 2397 1512 2400 1602 2454 1568 2388 1558 2394 1560 2455 1338 2228 1557 2225 1609 2387 1503 2422 1391 2288 1559 2272 1560 2455 1557 2225 1605 2438 1541 2366 1672 2436 1560 2455 1559 2272 1561 2401 1581 2389 2938 2456 2928 2365 1560 2455 1561 2401 1329 2202 1560 2455 1341 2233 1374 2230 1560 2455 1329 2202 1341 2233 1734 2457 1616 2425 1528 2376 1560 2455 1374 2230 1338 2228 1599 2458 1655 2411 1720 2459 1599 2458 1562 2407 1518 2410 1599 2458 1518 2410 1655 2411 1599 2458 1598 2415 1562 2407 1591 2369 1609 2387 1563 2283 1601 2460 1599 2458 1720 2459 1601 2460 1720 2459 1737 2461 1566 2371 1649 2427 1523 2418 1566 2371 1523 2418 1745 2372 1567 2462 1566 2371 1780 2374 1637 2463 1504 2329 1586 2378 1643 2464 1444 2334 1568 2388 1623 2465 1591 2369 1542 2367 1569 2466 1525 2420 1598 2415 1569 2466 1527 2424 1525 2420 1593 2467 1264 2095 1273 2106 1570 2468 1834 2469 1819 2426 1570 2468 1569 2466 1600 2470 1570 2468 1819 2426 1527 2424 1593 2467 1677 2432 1264 2095 1570 2468 1527 2424 1569 2466 1570 2468 1760 2471 1834 2469 1648 2472 1531 2429 1649 2427 1648 2472 1831 2430 1531 2429 1648 2472 1839 2473 1831 2430 1613 2474 1854 2475 1760 2471 1613 2474 1570 2468 1600 2470 1613 2474 1760 2471 1570 2468 1650 2476 1572 2477 1839 2473 1650 2476 1839 2473 1648 2472 3864 2408 1517 2406 1538 2395 1588 2478 1587 2414 1574 2198 2977 2479 1480 2231 1621 2413 1794 2480 1575 2356 1696 2431 1794 2480 1678 2451 1575 2356 1576 2481 1485 2380 1577 2434 1576 2481 1577 2434 1305 2162 1576 2481 1579 2437 1485 2380 1625 2482 1576 2481 1305 2162 1625 2482 1305 2162 1339 2168 1506 2396 1528 2376 1458 2345 1627 2483 1576 2481 1625 2482 1627 2483 1579 2437 1576 2481 1580 2484 1824 2485 1802 2486 1580 2484 1802 2486 1704 2487 1580 2484 1704 2487 1672 2436 1580 2484 1579 2437 1627 2483 1580 2484 1672 2436 1579 2437 2977 2479 1507 2299 1480 2231 1631 2488 2938 2456 1581 2389 1583 2489 1540 2490 1547 2440 1583 2489 1547 2440 1548 2442 1639 2491 1583 2489 1548 2442 1639 2491 1632 2492 1583 2489 1584 2493 1684 2448 1552 2412 1584 2493 1552 2412 3571 2494 1640 2495 1439 2496 1783 2497 1635 2409 3864 2408 1636 2498 1422 2216 1606 2114 3060 2113 1784 2499 1639 2491 1548 2442 1784 2499 1640 2495 1639 2491 1784 2499 1548 2442 1585 2443 1784 2499 1439 2496 1640 2495 1564 2500 1637 2463 1586 2378 1784 2499 1585 2443 1571 2445 1643 2464 1568 2388 1602 2454 1589 2501 1551 2311 1555 2449 1589 2501 1594 2352 1420 2312 1589 2501 1420 2312 1551 2311 1610 2502 1609 2387 1591 2369 1592 2503 1645 2393 1594 2352 1592 2503 1589 2501 1555 2449 1592 2503 1594 2352 1589 2501 1661 2504 1614 2450 1289 2386 1661 2504 1642 2505 1641 2506 1661 2504 1289 2386 1642 2505 1595 2507 1555 2449 1556 2453 1595 2507 1556 2453 1258 2007 1595 2507 1592 2503 1555 2449 1258 2007 1646 1998 1595 2507 1596 2508 1737 2461 4295 2509 1596 2508 1601 2460 1737 2461 1597 2510 1599 2458 1601 2460 1691 2511 1542 2367 1587 2414 1597 2510 1600 2470 1569 2466 1597 2510 1598 2415 1599 2458 1597 2510 1601 2460 1612 2512 1597 2510 1569 2466 1598 2415 1597 2510 1612 2512 1600 2470 1596 2508 1612 2512 1601 2460 1685 2513 1528 2376 1506 2396 1608 2514 1596 2508 4295 2509 1675 2515 1696 2431 1604 2384 1608 2514 4295 2509 1788 2516 1675 2515 1604 2384 1605 2438 1658 2205 1588 2478 1574 2198 1607 2517 1788 2516 3468 2518 1607 2517 1608 2514 1788 2516 1654 2519 1596 2508 1608 2514 1654 2519 1608 2514 1607 2517 1611 2375 1567 2462 1780 2374 1643 2464 1652 2452 1444 2334 1659 2419 1631 2488 1581 2389 1615 2520 1612 2512 1596 2508 1615 2520 1596 2508 1654 2519 1617 2521 1612 2512 1615 2520 1617 2521 1600 2470 1612 2512 1617 2521 1613 2474 1600 2470 1660 2522 1614 2450 1661 2504 1660 2522 1602 2454 1558 2394 2993 2523 1591 2369 1623 2465 1660 2522 1558 2394 1614 2450 1619 2524 1617 2521 1615 2520 1663 2525 1460 2263 1616 2425 1617 2521 1761 2526 1854 2475 1617 2521 1854 2475 1613 2474 1870 2527 1650 2476 1647 2528 1870 2527 1572 2477 1650 2476 1618 2529 1875 2530 1761 2526 1618 2529 1761 2526 1617 2521 1618 2529 1617 2521 1619 2524 1665 2531 1875 2530 1618 2529 1665 2531 1618 2529 1619 2524 1665 2531 1879 2532 1875 2530 1753 2533 1685 2513 1506 2396 1753 2533 1506 2396 1689 2441 1637 2463 1621 2413 1504 2329 1691 2511 1623 2465 1542 2367 1624 2534 1625 2482 1339 2168 1624 2534 1626 2211 1628 2236 1624 2534 1339 2168 1626 2211 1680 2535 1627 2483 1625 2482 1680 2535 1625 2482 1624 2534 1680 2535 1624 2534 1628 2236 1629 2536 1630 2537 1824 2485 1629 2536 1824 2485 1580 2484 1629 2536 1580 2484 1627 2483 1629 2536 1681 2538 1630 2537 1663 2525 1659 2419 1460 2263 1659 2419 2937 2539 1631 2488 1634 2540 1588 2478 1658 2205 1708 2541 1587 2414 1588 2478 1708 2541 1691 2511 1587 2414 1633 2542 1632 2492 1639 2491 1638 2543 1684 2448 1633 2542 1638 2543 1633 2542 1639 2491 1637 2463 2977 2479 1621 2413 1734 2457 1663 2525 1616 2425 1644 2544 1638 2543 1639 2491 1742 2545 1586 2378 1652 2452 1644 2544 1642 2505 1638 2543 1742 2545 1564 2500 1586 2378 1644 2544 1639 2491 1640 2495 1641 2506 1642 2505 1644 2544 1644 2544 1783 2497 1641 2506 1633 2542 1584 2493 3571 2494 1644 2544 1640 2495 1783 2497 1695 2546 1620 2446 1645 2393 1695 2546 1645 2393 1592 2503 1688 2547 1494 2346 1678 2451 1695 2546 1646 1998 1694 1997 1688 2547 1689 2441 1494 2346 1646 1998 1695 2546 1592 2503 1646 1998 1592 2503 1595 2507 1651 2548 1607 2517 3468 2518 1650 2476 1649 2427 1566 2371 1650 2476 1648 2472 1649 2427 1650 2476 1567 2462 1647 2528 1650 2476 1566 2371 1567 2462 1699 2549 1651 2548 3026 2550 1653 2551 1651 2548 1699 2549 1653 2551 1607 2517 1651 2548 1653 2551 1615 2520 1654 2519 1653 2551 1654 2519 1607 2517 1711 2552 1588 2478 1634 2540 1655 2411 1635 2409 1636 2498 1656 2553 1567 2462 1611 2375 1715 2554 1696 2431 1675 2515 1656 2553 1647 2528 1567 2462 1656 2553 1611 2375 1805 2421 1657 2555 1699 2549 3026 2550 2993 2523 1610 2502 1591 2369 1657 2555 3026 2550 1822 2556 1697 2557 1805 2421 1818 2423 1697 2557 1656 2553 1805 2421 1700 2558 1699 2549 1657 2555 1755 2559 1656 2553 1697 2557 1755 2559 1647 2528 1656 2553 1710 2208 1634 2540 1658 2205 1659 2419 2941 2560 2937 2539 1662 2561 1647 2528 1755 2559 1633 2542 1684 2448 1584 2493 1662 2561 1868 2562 1870 2527 1726 2563 1660 2522 1661 2504 1662 2561 1870 2527 1647 2528 1662 2561 1877 2564 1868 2562 1726 2563 1661 2504 1641 2506 1662 2561 1713 2565 1877 2564 1664 2566 1619 2524 1615 2520 1664 2566 1665 2531 1619 2524 1685 2513 1734 2457 1528 2376 1655 2411 1636 2498 3776 2567 1896 2568 1669 2569 1897 2570 1896 2568 1879 2532 1665 2531 1896 2568 1665 2531 1664 2566 1896 2568 1664 2566 1669 2569 1668 2571 1974 2572 1897 2570 1668 2571 1897 2570 1669 2569 1668 2571 1669 2569 1667 2573 1714 2574 2993 2523 1623 2465 1714 2574 1623 2465 1691 2511 1670 2575 1593 2467 1273 2106 1670 2575 1273 2106 1671 2121 1722 2576 1288 2116 1280 2143 1722 2576 1280 2143 1287 2145 1674 2577 1605 2438 1672 2436 1676 2578 1628 2236 1673 2304 1674 2577 1675 2515 1605 2438 1676 2578 1680 2535 1628 2236 1786 2579 1602 2454 1660 2522 1674 2577 1672 2436 1704 2487 1676 2578 1537 2336 1677 2432 1676 2578 1673 2304 1537 2336 1736 2580 1680 2535 1676 2578 1725 2581 1652 2452 1643 2464 1795 2582 1688 2547 1678 2451 1795 2582 1678 2451 1794 2480 1679 2583 1627 2483 1680 2535 1679 2583 1629 2536 1627 2483 1679 2583 1681 2538 1629 2536 1679 2583 1680 2535 1736 2580 1736 2580 1681 2538 1679 2583 1683 2584 1682 2370 1578 2358 1759 2585 1711 2552 1634 2540 1759 2585 1634 2540 1710 2208 1742 2545 1637 2463 1564 2500 1711 2552 1708 2541 1588 2478 1683 2584 1684 2448 1638 2543 1683 2584 1578 2358 1684 2448 1725 2581 1643 2464 1602 2454 1707 2586 1734 2457 1685 2513 1729 2587 1682 2370 1683 2584 1729 2587 1683 2584 1638 2543 1690 2588 1687 2341 1729 2587 1690 2588 1729 2587 1638 2543 1690 2588 1263 2342 1687 2341 1690 2588 1638 2543 1642 2505 1690 2588 1642 2505 1289 2386 1690 2588 1289 2386 1263 2342 1693 2589 1878 2444 1620 2446 1693 2589 1692 2590 1878 2444 1693 2589 1620 2446 1695 2546 1693 2589 1695 2546 1694 1997 1693 2589 1694 1997 1935 1929 1703 2591 1657 2555 1822 2556 1703 2591 1822 2556 1821 2592 1702 2593 2993 2523 1714 2574 1775 2594 1696 2431 1715 2554 1775 2594 1794 2480 1696 2431 1701 2595 1697 2557 1818 2423 1803 2596 1593 2467 1670 2575 1701 2595 1818 2423 1819 2426 1701 2595 1819 2426 1834 2469 1701 2595 1755 2559 1697 2557 1698 2597 1615 2520 1653 2551 1698 2597 1653 2551 1699 2549 1698 2597 1664 2566 1615 2520 1698 2597 1669 2569 1664 2566 1698 2597 1749 2598 1667 2573 1698 2597 1667 2573 1669 2569 1698 2597 1700 2558 1749 2598 1698 2597 1699 2549 1700 2558 1756 2599 1701 2595 1834 2469 1756 2599 1755 2559 1701 2595 1709 2600 1657 2555 1703 2591 1709 2600 1700 2558 1657 2555 1709 2600 1749 2598 1700 2558 1705 2601 1703 2591 1821 2592 1778 2602 1704 2487 1802 2486 1778 2602 1674 2577 1704 2487 1705 2601 1709 2600 1703 2591 1705 2601 1821 2592 1843 2603 1712 2604 1705 2601 1843 2603 1720 2459 1655 2411 3776 2567 1622 2605 2977 2479 1637 2463 1781 2606 1707 2586 1685 2513 1728 2607 1708 2541 1711 2552 1781 2606 1685 2513 1753 2533 1750 2608 1709 2600 1705 2601 1744 2248 1710 2208 1402 2246 1744 2248 1759 2585 1710 2208 1712 2604 1750 2608 1705 2601 1748 2609 1712 2604 1843 2603 1748 2609 1843 2603 3186 2610 1737 2461 3776 2567 4295 2509 1758 2611 1900 2612 1713 2565 1758 2611 1662 2561 1755 2559 1758 2611 1912 2613 1900 2612 1758 2611 1713 2565 1662 2561 1752 2614 1691 2511 1708 2541 1752 2614 1714 2574 1691 2511 1747 2615 1711 2552 1759 2585 1719 2616 1667 2573 1749 2598 1719 2616 1668 2571 1667 2573 1737 2461 1720 2459 3776 2567 1716 2617 1718 1935 1974 2572 1716 2617 1974 2572 1668 2571 1716 2617 1668 2571 1719 2616 1717 2618 1937 1936 1718 1935 1717 2618 1718 1935 1716 2617 1717 2618 1719 2616 1751 2619 1717 2618 1716 2617 1719 2616 1721 2620 1941 1969 1937 1936 1721 2620 1949 1970 1941 1969 1721 2620 1937 1936 1717 2618 1721 2620 1717 2618 1751 2619 1790 2621 1660 2522 1726 2563 1790 2621 1786 2579 1660 2522 1840 2622 1688 2547 1795 2582 1722 2576 1276 2117 1288 2116 1276 2117 1287 2145 1722 2576 1727 2623 1359 2167 1723 2179 1772 2624 1759 2585 1744 2248 1688 2547 1753 2533 1689 2441 1727 2623 1724 2149 1359 2167 1727 2623 1723 2179 1449 2189 1747 2615 1728 2607 1711 2552 1733 2625 1708 2541 1728 2607 1632 2492 1633 2542 3571 2494 1783 2497 1726 2563 1641 2506 1730 2626 1449 2189 1590 2285 1730 2626 1590 2285 1682 2370 1730 2626 1682 2370 1729 2587 3939 2627 1727 2623 1449 2189 3939 2627 1671 2121 1731 2126 3939 2627 1449 2189 1730 2626 3939 2627 1730 2626 1729 2587 3939 2627 1731 2126 1724 2149 3939 2627 1670 2575 1671 2121 3939 2627 1732 2628 1803 2596 3939 2627 1803 2596 1670 2575 3939 2627 1724 2149 1727 2623 1735 2629 1677 2432 1593 2467 1735 2629 1803 2596 1743 2630 1752 2614 1702 2593 1714 2574 1735 2629 1593 2467 1803 2596 1752 2614 2994 2631 1702 2593 1735 2629 1676 2578 1677 2432 1733 2625 1777 2632 2994 2631 1733 2625 2994 2631 1752 2614 1735 2629 1736 2580 1676 2578 1738 2633 1995 2016 1970 2634 1738 2633 1970 2634 1681 2538 1797 2635 1637 2463 1742 2545 1738 2633 1681 2538 1736 2580 4094 2636 1913 2139 1827 2067 4094 2636 1732 2628 3939 2627 4094 2636 1827 2067 1732 2628 4094 2636 3940 2140 1913 2139 1739 2637 1736 2580 1735 2629 1739 2637 1741 2017 1995 2016 1739 2637 1738 2633 1736 2580 1739 2637 1735 2629 1743 2630 1739 2637 1740 2037 1741 2017 1739 2637 1259 2065 1740 2037 1739 2637 1850 2066 1259 2065 1739 2637 1995 2016 1738 2633 1739 2637 1743 2630 1850 2066 1746 2219 3939 2627 1729 2587 1782 2247 1772 2624 1744 2248 1746 2219 1729 2587 1687 2341 3940 2140 3939 2627 1746 2219 1746 2219 1687 2341 1686 2281 1791 2638 1675 2515 1674 2577 1791 2638 1715 2554 1675 2515 1760 2471 1756 2599 1834 2469 1791 2638 1674 2577 1778 2602 1760 2471 1854 2475 1761 2526 1763 2639 1748 2609 3186 2610 2941 2560 1659 2419 1663 2525 1750 2608 1769 2640 1751 2619 1750 2608 1749 2598 1709 2600 1750 2608 1712 2604 1769 2640 1750 2608 1719 2616 1749 2598 1750 2608 1751 2619 1719 2616 1733 2625 1752 2614 1708 2541 1762 2641 1712 2604 1748 2609 1762 2641 1748 2609 1763 2639 1810 2642 1781 2606 1753 2533 1762 2641 1769 2640 1712 2604 1810 2642 1753 2533 1688 2547 1757 2643 1754 2644 1912 2613 1757 2643 1755 2559 1756 2599 1757 2643 1758 2611 1755 2559 1757 2643 1932 2645 1754 2644 1757 2643 1912 2613 1758 2611 1757 2643 1875 2530 1938 2646 1757 2643 1756 2599 1760 2471 1772 2624 1747 2615 1759 2585 1757 2643 1938 2646 1932 2645 1757 2643 1760 2471 1761 2526 1757 2643 1761 2526 1875 2530 1788 2516 4295 2509 3468 2518 1764 2647 3911 2648 1972 2011 1764 2647 1766 2649 3911 2648 1764 2647 1765 2650 1766 2649 1764 2647 1762 2641 1763 2639 1771 2651 1764 2647 1972 2011 1771 2651 1769 2640 1762 2641 1771 2651 1762 2641 1764 2647 1776 2652 1728 2607 1747 2615 1767 2653 1751 2619 1769 2640 1767 2653 1768 1977 1949 1970 1767 2653 1721 2620 1751 2619 1767 2653 1949 1970 1721 2620 1786 2579 1725 2581 1602 2454 1770 2654 1972 2011 1966 2010 1770 2654 1961 2000 1768 1977 1770 2654 1768 1977 1767 2653 1770 2654 1966 2010 1961 2000 1770 2654 1771 2651 1972 2011 1770 2654 1769 2640 1771 2651 1770 2654 1767 2653 1769 2640 1773 2655 1450 2098 1573 2024 1773 2655 1274 2097 1450 2098 1851 2656 1652 2452 1725 2581 4060 2657 1573 2024 1773 2655 1893 2658 1688 2547 1840 2622 1893 2658 1810 2642 1688 2547 1773 2655 2946 2141 1274 2097 1774 2659 1772 2624 1782 2247 1773 2655 4060 2657 2946 2141 1776 2652 1733 2625 1728 2607 1813 2660 1747 2615 1772 2624 1707 2586 1663 2525 1734 2457 1800 2661 1791 2638 1778 2602 1798 2662 1779 2663 1777 2632 1798 2662 1777 2632 1733 2625 1814 2664 1772 2624 1774 2659 1806 2665 1632 2492 3571 2494 1789 2666 1783 2497 1439 2496 1789 2666 1439 2496 1784 2499 1785 2301 1774 2659 1782 2247 1547 2440 1806 2665 4259 2667 1798 2662 1733 2625 1776 2652 1848 2668 1790 2621 1726 2563 1848 2668 1726 2563 1783 2497 1848 2668 1783 2497 1789 2666 1851 2656 1742 2545 1652 2452 1792 2669 1791 2638 1800 2661 1792 2669 1715 2554 1791 2638 1792 2669 1775 2594 1715 2554 1832 2670 1725 2581 1786 2579 1651 2548 3468 2518 3026 2550 1813 2660 1776 2652 1747 2615 1795 2582 1794 2480 1775 2594 1797 2635 1622 2605 1637 2463 1804 2671 1774 2659 1785 2301 1804 2671 1785 2301 1807 2338 1801 2672 1772 2624 1814 2664 1801 2672 1813 2660 1772 2624 1816 2673 1547 2440 4259 2667 1816 2673 4259 2667 3934 2674 1851 2656 1797 2635 1742 2545 1817 2675 1798 2662 1776 2652 1817 2675 1776 2652 1813 2660 1798 2662 2999 2676 1779 2663 1799 2677 1795 2582 1775 2594 1804 2671 1814 2664 1774 2659 1841 2678 1800 2661 1778 2602 1841 2678 1802 2486 1824 2485 1841 2678 1778 2602 1802 2486 1732 2628 1743 2630 1803 2596 1815 2679 1804 2671 1807 2338 1797 2635 1829 2680 1622 2605 1812 2339 1815 2679 1807 2338 1547 2440 1540 2490 1806 2665 1842 2681 1792 2669 1800 2661 1820 2682 1801 2672 1814 2664 1809 2683 1707 2586 1781 2606 1809 2683 1781 2606 1810 2642 1822 2556 3026 2550 1821 2592 1820 2682 1813 2660 1801 2672 1820 2682 1817 2675 1813 2660 1826 2684 1814 2664 1804 2671 1826 2684 1820 2682 1814 2664 1825 2685 1815 2679 1812 2339 1930 2686 1775 2594 1792 2669 1546 2093 1816 2673 3934 2674 2944 2687 1663 2525 1707 2586 1817 2675 2999 2676 1798 2662 1874 2094 3934 2674 1849 2688 1903 2689 1810 2642 1893 2658 1903 2689 1809 2683 1810 2642 1847 2690 1784 2499 1571 2445 1847 2690 1789 2666 1784 2499 1817 2675 1836 2691 2999 2676 1833 2692 1815 2679 1825 2685 1799 2677 1840 2622 1795 2582 1826 2684 1804 2671 1815 2679 1850 2066 1743 2630 1732 2628 1830 2428 1825 2685 1812 2339 2944 2687 2941 2560 1663 2525 1852 2693 1817 2675 1820 2682 1883 2694 1797 2635 1851 2656 1833 2692 1826 2684 1815 2679 1835 2695 1820 2682 1826 2684 1823 2696 1824 2485 1630 2537 1823 2696 1841 2678 1824 2485 1509 2398 3986 2697 1899 2698 1827 2067 1850 2066 1732 2628 1843 2603 1821 2592 3186 2610 1866 2699 1786 2579 1790 2621 1855 2700 1829 2680 1797 2635 1855 2700 1882 2701 1829 2680 1838 2702 1833 2692 1825 2685 1838 2702 1825 2685 1830 2428 1831 2430 1838 2702 1830 2428 1846 2703 1826 2684 1833 2692 1835 2695 1852 2693 1820 2682 1930 2686 1792 2669 1842 2681 1894 2704 1851 2656 1725 2581 1894 2704 1725 2581 1832 2670 1852 2693 1836 2691 1817 2675 1845 2705 1835 2695 1826 2684 1844 2706 1833 2692 1838 2702 1939 2707 1866 2699 1790 2621 1939 2707 1790 2621 1848 2668 1852 2693 1862 2708 1836 2691 1863 2709 2944 2687 1707 2586 1837 2710 1847 2690 1571 2445 1837 2710 1571 2445 1878 2444 1839 2473 1838 2702 1831 2430 1371 2224 1081 2275 3986 2697 1853 2711 1852 2693 1835 2695 1799 2677 1893 2658 1840 2622 1865 2712 1841 2678 1823 2696 1865 2712 1800 2661 1841 2678 1865 2712 1842 2681 1800 2661 1874 2094 1546 2093 3934 2674 1844 2706 1846 2703 1833 2692 1930 2686 1799 2677 1775 2594 1845 2705 1826 2684 1846 2703 1856 2713 1844 2706 1838 2702 1866 2699 1832 2670 1786 2579 1871 2714 1789 2666 1847 2690 1853 2711 1835 2695 1845 2705 1871 2714 1848 2668 1789 2666 1490 2135 1874 2094 1849 2688 1857 2715 1846 2703 1844 2706 1858 2716 1838 2702 1839 2473 1858 2716 1856 2713 1838 2702 1869 2717 1858 2716 1839 2473 1869 2717 1839 2473 1572 2477 1856 2713 1857 2715 1844 2706 1853 2711 1862 2708 1852 2693 1763 2639 3186 2610 3187 2718 1371 2224 1335 2223 1081 2275 1960 2719 1707 2586 1809 2683 1764 2647 1763 2639 3187 2718 1883 2694 1855 2700 1797 2635 1869 2717 1572 2477 1870 2527 1867 2720 1845 2705 1846 2703 1894 2704 1883 2694 1851 2656 1867 2720 1853 2711 1845 2705 1876 2721 1858 2716 1869 2717 1942 2722 1809 2683 1903 2689 1765 2650 1764 2647 3187 2718 1860 2723 1862 2708 1853 2711 1860 2723 1861 2724 1862 2708 1863 2709 2949 1925 2944 2687 1873 2725 1856 2713 1858 2716 1867 2720 1860 2723 1853 2711 1936 2726 1893 2658 1799 2677 1883 2694 1882 2701 1855 2700 1864 2727 1846 2703 1857 2715 1872 2728 1857 2715 1856 2713 1872 2728 1864 2727 1857 2715 1880 2729 1858 2716 1876 2721 1864 2727 1867 2720 1846 2703 1766 2649 3187 2718 3911 2648 1766 2649 1765 2650 3187 2718 1924 2730 1865 2712 1823 2696 1866 2699 1894 2704 1832 2670 1886 2731 1856 2713 1873 2725 1583 2489 1632 2492 1806 2665 1942 2722 1960 2719 1809 2683 1890 2732 1870 2527 1868 2562 1890 2732 1876 2721 1869 2717 1890 2732 1869 2717 1870 2527 1922 2733 1842 2681 1865 2712 1922 2733 1865 2712 1924 2730 1871 2714 1847 2690 1837 2710 1861 2724 1860 2723 1867 2720 1947 2734 1799 2677 1930 2686 1880 2729 1873 2725 1858 2716 1491 2253 1849 2688 1998 2254 1491 2253 1490 2135 1849 2688 1886 2731 1872 2728 1856 2713 1890 2732 1868 2562 1877 2564 1889 2735 1864 2727 1872 2728 1692 2590 1837 2710 1878 2444 1692 2590 1871 2714 1837 2710 1901 2736 1867 2720 1864 2727 1871 2714 1939 2707 1848 2668 1884 2737 1861 2724 1867 2720 1887 2738 1886 2731 1873 2725 1887 2738 1873 2725 1880 2729 1907 2739 1866 2699 1939 2707 1883 2694 1881 2740 1882 2701 1892 2741 1876 2721 1890 2732 1887 2738 1880 2729 1876 2721 1885 2742 1630 2537 1681 2538 1885 2742 1823 2696 1630 2537 1885 2742 1924 2730 1823 2696 1888 2743 1872 2728 1886 2731 1904 2744 1888 2743 1886 2731 1904 2744 1886 2731 1887 2738 1888 2743 1889 2735 1872 2728 1902 2745 1876 2721 1892 2741 1940 2746 1883 2694 1894 2704 1914 2747 1864 2727 1889 2735 1914 2747 1901 2736 1864 2727 1915 2748 1889 2735 1888 2743 1915 2748 1914 2747 1889 2735 1933 1973 1903 2689 1893 2658 1891 2749 1890 2732 1877 2564 1891 2749 1713 2565 1900 2612 1891 2749 1892 2741 1890 2732 1933 1973 1893 2658 1936 2726 1891 2749 1877 2564 1713 2565 1901 2736 1884 2737 1867 2720 1987 1949 1947 2734 1930 2686 1977 2750 1894 2704 1866 2699 1902 2745 1887 2738 1876 2721 1909 2273 1371 2224 3986 2697 1540 2490 1583 2489 1806 2665 1895 2751 1904 2744 1887 2738 1898 2752 1891 2749 1900 2612 1985 2753 1922 2733 1924 2730 1960 2719 1863 2709 1707 2586 1910 2754 1888 2743 1904 2744 1910 2754 1915 2748 1888 2743 1912 2613 1898 2752 1900 2612 1908 2755 1892 2741 1891 2749 1908 2755 1891 2749 1898 2752 1908 2755 1902 2745 1892 2741 1922 2733 1930 2686 1842 2681 1914 2747 1884 2737 1901 2736 1906 2756 1887 2738 1902 2745 1905 2757 1884 2737 1914 2747 1919 2758 1902 2745 1908 2755 1895 2751 1910 2754 1904 2744 1973 1928 1871 2714 1692 2590 1920 2759 1887 2738 1906 2756 1920 2759 1895 2751 1887 2738 1916 2760 1908 2755 1898 2752 1916 2760 1919 2758 1908 2755 1509 2398 1909 2273 3986 2697 1911 2761 1885 2742 1681 2538 1918 2762 1910 2754 1895 2751 1911 2761 1681 2538 1970 2634 1917 2763 1912 2613 1754 2644 1917 2763 1898 2752 1912 2613 1917 2763 1916 2760 1898 2752 1918 2762 1915 2748 1910 2754 1915 2748 1905 2757 1914 2747 1928 2764 1919 2758 1916 2760 3919 2320 1509 2398 1899 2698 1940 2746 1881 2740 1883 2694 1931 2765 1917 2763 1754 2644 1940 2746 2985 2766 1881 2740 1926 2767 1928 2764 1916 2760 1926 2767 1917 2763 1931 2765 1926 2767 1916 2760 1917 2763 1921 2768 1895 2751 1920 2759 1921 2768 1918 2762 1895 2751 1929 2769 1915 2748 1918 2762 1925 2770 1906 2756 1902 2745 1925 2770 1902 2745 1919 2758 1921 2768 1920 2759 1906 2756 1932 2645 1931 2765 1754 2644 2949 1925 1863 2709 1960 2719 1928 2764 1925 2770 1919 2758 1177 1986 1866 2699 1907 2739 1177 1986 1977 2750 1866 2699 1929 2769 1905 2757 1915 2748 1981 1961 1922 2733 1985 2753 1976 2771 1799 2677 1947 2734 1976 2771 1936 2726 1799 2677 1923 2772 1985 2753 1924 2730 1923 2772 1885 2742 1911 2761 1923 2772 1924 2730 1885 2742 1925 2770 1921 2768 1906 2756 1954 2773 1926 2767 1931 2765 1929 2769 1927 2774 1905 2757 1946 2775 1925 2770 1928 2764 1934 2776 1929 2769 1918 2762 1959 1967 1930 2686 1922 2733 1935 1929 1692 2590 1693 2589 1943 2777 1931 2765 1932 2645 1943 2777 1932 2645 1938 2646 1935 1929 1973 1928 1692 2590 1943 2777 1954 2773 1931 2765 1944 2778 1918 2762 1921 2768 1952 2779 1946 2775 1928 2764 1952 2779 1928 2764 1926 2767 1954 2773 1952 2779 1926 2767 1158 1926 1903 2689 1933 1973 1158 1926 1942 2722 1903 2689 1951 2780 1921 2768 1925 2770 1927 2774 1929 2769 1934 2776 1950 2781 1943 2777 1938 2646 1907 2739 1939 2707 1871 2714 1977 2750 1940 2746 1894 2704 1968 2782 1952 2779 1954 2773 1955 2783 1943 2777 1950 2781 1955 2783 1954 2773 1943 2777 1962 2784 1934 2776 1918 2762 1962 2784 1918 2762 1944 2778 1945 2785 1944 2778 1921 2768 1945 2785 1921 2768 1951 2780 1964 2786 1951 2780 1925 2770 1964 2786 1925 2770 1946 2775 1948 2787 1950 2781 1938 2646 1948 2787 1875 2530 1879 2532 1948 2787 1938 2646 1875 2530 1987 1949 1976 2771 1947 2734 1965 2788 1946 2775 1952 2779 1965 2788 1964 2786 1946 2775 1956 2789 1955 2783 1950 2781 1958 2790 1896 2568 1897 2570 1958 2790 1948 2787 1879 2532 1958 2790 1879 2532 1896 2568 1968 2782 1965 2788 1952 2779 1953 1957 1923 2772 1911 2761 1956 2789 1954 2773 1955 2783 1956 2789 1968 2782 1954 2773 1158 1926 1960 2719 1942 2722 1962 2784 1927 2774 1934 2776 1963 2791 1951 2780 1964 2786 1963 2791 1945 2785 1951 2780 1973 1928 1159 1987 1871 2714 1962 2784 1982 2792 1927 2774 1981 1961 1959 1967 1922 2733 1164 1948 1976 2771 1987 1949 1168 1956 1985 2753 1923 2772 1168 1956 1923 2772 1953 1957 1957 2793 1956 2789 1950 2781 1957 2793 1950 2781 1948 2787 1959 1967 1987 1949 1930 2686 1957 2793 1948 2787 1958 2790 1988 2794 1965 2788 1968 2782 1159 1987 1907 2739 1871 2714 1967 2795 1956 2789 1980 2796 1967 2795 1968 2782 1956 2789 1983 2797 1957 2793 1958 2790 1972 2011 3911 2648 1573 2024 1979 2798 1963 2791 1964 2786 1979 2798 1964 2786 1965 2788 1990 2799 1967 2795 1980 2796 1565 2220 1998 2254 1080 1931 1945 2785 1962 2784 1944 2778 2985 2766 1940 2746 1977 2750 1967 2795 1988 2794 1968 2782 1969 2800 1958 2790 1897 2570 1969 2800 1897 2570 1974 2572 1969 2800 1983 2797 1958 2790 1986 2801 1956 2789 1957 2793 1971 1958 1970 2634 1995 2016 1986 2801 1980 2796 1956 2789 1971 1958 1953 1957 1911 2761 1971 1958 1911 2761 1970 2634 1986 2801 1957 2793 1983 2797 1158 1926 2949 1925 1960 2719 1993 1923 1962 2784 1945 2785 1978 1945 1965 2788 1988 2794 1978 1945 1979 2798 1965 2788 1975 1943 1969 2800 1974 2572 1975 1943 1974 2572 1718 1935 1976 2771 1933 1973 1936 2726 1989 2802 1988 2794 1967 2795 1177 1986 1176 1984 2985 2766 1989 2802 1967 2795 1990 2799 1177 1986 2985 2766 1977 2750 1993 1923 1982 2792 1962 2784 1546 2093 1547 2440 1816 2673 2000 1921 1945 2785 1963 2791 1188 1982 1177 1986 1907 2739 1159 1987 1188 1982 1907 2739 1991 1940 1980 2796 1986 2801 1991 1940 1990 2799 1980 2796 1999 1922 1963 2791 1979 2798 2000 1921 1993 1923 1945 2785 1171 1966 1959 1967 1981 1961 1984 2803 1969 2800 1975 1943 1984 2803 1983 2797 1969 2800 1984 2803 1986 2801 1983 2797 1168 1956 1981 1961 1985 2753 1183 1972 1933 1973 1976 2771 1170 1962 1989 2802 1990 2799 2002 1964 1988 2794 1989 2802 2002 1964 1989 2802 1170 1962 1165 1950 1987 1949 1959 1967 1992 1942 1984 2803 1975 1943 1994 1937 1999 1922 1979 2798 1994 1937 1979 2798 1978 1945 1196 2013 3919 2320 1899 2698 1196 2013 1899 2698 1082 2014 2002 1964 1978 1945 1988 2794 1999 1922 2000 1921 1963 2791 1996 1934 1975 1943 1718 1935 1164 1948 1183 1972 1976 2771 1160 1938 1991 1940 1986 2801 1997 1927 1159 1987 1973 1928 1160 1938 1986 2801 1984 2803 1160 1938 1984 2803 1992 1942 1161 1952 1170 1962 1990 2799 1161 1952 1991 1940 2001 1939 1161 1952 1990 2799 1991 1940 2003 1944 1978 1945 2002 1964 2004 2804 2005 2805 2701 2806 2667 2807 2676 2808 2007 2809 2703 2810 2024 2811 2008 2812 2702 2813 2013 2814 2020 2815 2242 2816 2686 2817 2037 2818 2698 2819 2020 2815 2009 2820 2010 2821 2701 2806 2014 2822 2657 2823 2684 2824 2705 2825 2699 2826 2016 2827 2025 2828 2704 2829 2675 2830 2980 2831 2345 2832 2010 2821 2012 2833 2011 2834 2345 2832 2012 2833 2676 2808 2704 2829 2007 2809 2020 2815 2013 2814 2029 2835 2016 2827 2009 2820 2026 2836 2009 2820 2020 2815 2019 2837 2005 2805 2025 2828 2017 2838 2634 2839 2101 2840 3702 2841 2701 2806 2005 2805 2017 2838 2692 2842 2008 2812 2013 2814 2013 2814 2008 2812 2024 2811 2014 2822 2701 2806 2038 2843 2701 2806 2017 2838 2038 2843 2025 2828 2016 2827 2015 2844 2016 2827 2026 2836 2015 2844 2026 2836 2009 2820 2031 2845 2009 2820 2019 2837 2031 2845 2675 2830 2688 2846 2980 2831 2010 2821 2014 2822 2033 2847 2012 2833 2010 2821 2033 2847 2011 2834 2012 2833 2023 2848 2021 2849 2022 2850 2023 2848 2022 2850 2011 2834 2023 2848 2013 2814 2024 2811 2029 2835 2088 2851 3232 2852 2089 2853 2020 2815 2029 2835 2019 2837 2025 2828 2015 2844 2034 2854 2017 2838 2025 2828 2034 2854 2014 2822 2038 2843 2033 2847 2029 2835 2024 2811 2036 2855 2026 2836 2031 2845 2015 2844 2017 2838 2034 2854 2027 2856 2038 2843 2017 2838 2027 2856 2021 2849 2023 2848 2028 2857 2684 2824 2007 2809 2093 2858 2705 2825 2684 2824 2093 2858 2012 2833 2033 2847 2042 2859 2028 2857 2023 2848 2042 2859 2023 2848 2012 2833 2042 2859 2031 2845 2019 2837 2041 2860 2019 2837 2029 2835 2041 2860 2650 2861 2021 2849 2030 2862 2673 2863 2650 2861 2030 2862 2015 2844 2031 2845 2032 2864 2263 2865 2242 2816 2044 2866 2033 2847 2038 2843 2039 2867 2027 2856 2034 2854 2050 2868 2033 2847 2039 2867 2042 2859 2034 2854 2015 2844 2040 2869 2015 2844 2032 2864 2040 2869 2679 2870 2673 2863 2057 2871 2673 2863 2030 2862 2057 2871 2029 2835 2036 2855 2041 2860 2028 2857 2042 2859 2048 2872 2007 2809 2704 2829 2064 2873 2038 2843 2027 2856 2039 2867 2030 2862 2021 2849 2049 2874 2021 2849 2028 2857 2049 2874 2031 2845 2041 2860 2032 2864 2042 2859 2039 2867 2045 2875 2042 2859 2045 2875 2054 2876 2686 2817 2705 2825 2138 2877 2037 2818 2686 2817 2138 2877 2048 2872 2042 2859 2054 2876 2039 2867 2027 2856 2043 2878 2027 2856 2050 2868 2043 2878 2034 2854 2040 2869 2060 2879 2050 2868 2034 2854 2060 2879 2041 2860 2036 2855 2046 2880 2057 2871 2030 2862 2058 2881 2030 2862 2049 2874 2058 2881 2040 2869 2032 2864 2066 2882 2039 2867 2043 2878 2045 2875 2032 2864 2041 2860 2062 2883 2047 2884 2679 2870 2056 2885 2679 2870 2057 2871 2056 2885 2093 2858 2007 2809 2078 2886 2040 2869 2066 2882 2060 2879 2028 2857 2048 2872 2051 2887 2049 2874 2028 2857 2051 2887 2045 2875 2043 2878 2065 2888 2048 2872 2054 2876 2051 2887 2047 2884 2056 2885 2063 2889 2049 2874 2051 2887 2053 2890 2058 2881 2049 2874 2053 2890 2062 2883 2041 2860 2046 2880 2050 2868 2060 2879 2043 2878 2032 2864 2062 2883 2066 2882 2054 2876 2045 2875 2055 2891 2045 2875 2065 2888 2055 2891 2060 2879 2066 2882 2061 2892 2056 2885 2057 2871 2067 2893 2057 2871 2058 2881 2067 2893 2044 2866 2242 2816 2135 2894 2242 2816 2037 2818 2135 2894 2043 2878 2060 2879 2059 2895 2060 2879 2061 2892 2059 2895 2066 2882 2062 2883 2086 2896 2051 2887 2054 2876 2073 2897 2051 2887 2073 2897 2085 2898 2053 2890 2051 2887 2085 2898 2068 2899 2063 2889 2084 2900 3702 2841 2413 2901 1076 2902 2704 2829 2980 2831 2064 2873 2055 2891 2065 2888 2072 2903 2063 2889 2056 2885 2069 2904 2056 2885 2067 2893 2069 2904 2062 2883 2046 2880 2086 2896 2043 2878 2059 2895 2080 2905 2065 2888 2043 2878 2080 2905 2061 2892 2066 2882 2090 2906 2067 2893 2058 2881 2070 2907 2058 2881 2053 2890 2070 2907 2054 2876 2055 2891 2073 2897 2007 2809 2064 2873 2078 2886 2066 2882 2086 2896 2090 2906 2046 2880 2104 2908 2086 2896 2068 2899 2084 2900 2102 2909 2077 2910 2068 2899 2102 2909 2090 2906 2086 2896 2110 2911 2084 2900 2063 2889 2091 2912 2063 2889 2069 2904 2091 2912 2053 2890 2085 2898 2070 2907 2055 2891 2072 2903 2074 2913 2073 2897 2055 2891 2074 2913 2076 2914 3412 2056 2079 2915 2067 2893 2070 2907 2098 2916 2069 2904 2067 2893 2098 2916 3327 2917 2076 2914 2081 2918 2065 2888 2080 2905 2099 2919 2072 2903 2065 2888 2099 2919 2076 2914 2079 2915 2075 2920 2079 2915 3412 2056 2087 2921 2085 2898 2073 2897 2108 2922 2073 2897 2074 2913 2108 2922 2080 2905 2059 2895 2111 2923 2061 2892 2090 2906 2059 2895 2080 2905 2111 2923 2099 2919 2076 2914 2075 2920 2133 2924 2082 2925 3327 2917 2083 2926 2064 2873 2980 2831 2095 2927 2077 2910 2102 2909 2092 2928 2642 2929 2077 2910 2092 2928 2087 2921 3412 2056 2106 2930 2059 2895 2090 2906 2110 2911 3412 2056 1225 2055 2106 2930 2102 2909 2084 2900 2091 2912 2076 2914 2133 2924 2081 2918 2070 2907 2085 2898 2097 2931 2037 2818 2138 2877 2135 2894 2102 2909 2091 2912 2094 2932 2087 2921 2106 2930 2118 2933 2070 2907 2097 2931 2098 2916 2079 2915 2087 2921 2226 2934 2087 2921 2118 2933 2226 2934 2091 2912 2069 2904 2103 2935 2069 2904 2098 2916 2103 2935 2075 2920 2079 2915 2121 2936 2079 2915 2226 2934 2121 2936 2086 2896 2104 2908 2110 2911 2633 2937 2642 2929 2225 2938 1573 2024 2633 2937 2225 2938 2642 2929 2092 2928 2225 2938 2075 2920 2121 2936 2133 2924 2093 2858 2078 2886 2138 2877 2705 2825 2093 2858 2138 2877 2099 2919 2111 2923 2109 2939 2124 2940 2082 2925 2127 2941 2072 2903 2099 2919 2074 2913 2085 2898 2108 2922 2097 2931 2098 2916 2097 2931 2113 2942 2035 2943 4071 2944 2052 2945 2074 2913 2099 2919 2100 2946 2099 2919 2109 2939 2100 2946 2098 2916 2113 2942 2103 2935 2108 2922 2074 2913 2112 2947 2225 2938 2092 2928 2119 2948 2102 2909 2094 2932 2119 2948 2092 2928 2102 2909 2119 2948 2094 2932 2091 2912 2103 2935 2059 2895 2110 2911 2130 2949 2106 2930 1225 2055 2107 2950 2111 2923 2059 2895 2130 2949 2114 2951 2124 2940 2129 2952 2110 2911 2104 2908 2105 2953 2104 2908 2971 2954 2105 2953 2108 2922 2112 2947 2117 2955 2097 2931 2108 2922 2117 2955 2106 2930 2107 2950 2118 2933 2078 2886 2064 2873 2140 2956 2097 2931 2117 2955 2113 2942 2103 2935 2113 2942 2123 2957 2135 2894 2138 2877 2122 2958 2109 2939 2111 2923 2120 2959 2103 2935 2123 2957 2094 2932 2112 2947 2074 2913 2132 2960 2074 2913 2100 2946 2132 2960 2083 2926 3327 2917 2125 2961 2110 2911 2105 2953 2130 2949 2081 2918 2133 2924 2131 2962 3327 2917 2081 2918 2131 2962 2111 2923 2130 2949 2120 2959 2138 2877 2078 2886 2164 2963 2113 2942 2117 2955 2126 2964 2113 2942 2126 2964 2123 2957 2130 2949 2105 2953 2971 2954 2112 2947 2132 2960 2116 2965 2117 2955 2112 2947 2116 2965 3987 2966 2114 2951 2139 2967 3327 2917 2131 2962 2142 2968 2109 2939 2120 2959 2100 2946 2125 2961 3327 2917 2142 2968 2132 2960 2100 2946 2115 2969 2044 2866 2135 2894 2137 2970 2123 2957 2126 2964 2128 2971 2094 2932 2123 2957 2153 2972 2100 2946 2120 2959 2146 2973 2083 2926 2125 2961 2134 2974 2082 2925 2083 2926 2134 2974 2119 2948 2094 2932 2153 2972 2117 2955 2116 2965 2136 2975 2126 2964 2117 2955 2136 2975 2082 2925 2134 2974 2157 2976 2127 2941 2082 2925 2157 2976 2130 2949 2971 2954 2141 2977 2064 2873 2095 2927 2140 2956 2164 2963 2078 2886 2169 2978 2120 2959 2130 2949 2146 2973 2078 2886 2140 2956 2169 2978 2123 2957 2128 2971 2143 2979 2116 2965 2132 2960 2147 2980 2115 2969 2100 2946 2146 2973 2119 2948 2153 2972 3910 2981 2132 2960 2115 2969 2147 2980 2126 2964 2136 2975 2128 2971 2153 2972 2123 2957 2143 2979 2128 2971 2136 2975 2152 2982 2134 2974 2125 2961 2158 2983 2157 2976 2134 2974 2158 2983 2095 2927 2096 2984 2140 2956 2137 2970 2135 2894 2160 2985 2135 2894 2122 2958 2160 2985 2116 2965 2147 2980 2154 2986 2152 2982 2136 2975 2154 2986 2124 2940 2127 2941 2145 2987 2136 2975 2116 2965 2154 2986 2127 2941 2157 2976 2145 2987 3702 2841 2101 2840 2369 2988 2122 2958 2138 2877 2148 2989 2138 2877 2164 2963 2148 2989 2146 2973 2130 2949 2141 2977 2139 2967 2114 2951 2163 2990 2129 2952 2124 2940 2159 2991 2124 2940 2145 2987 2159 2991 2128 2971 2152 2982 2143 2979 2096 2984 2171 2992 2169 2978 2140 2956 2096 2984 2169 2978 2115 2969 2146 2973 2147 2980 1076 2902 2413 2901 2367 2993 2179 2994 3987 2966 2155 2995 3910 2981 2153 2972 2150 2996 3909 2997 3910 2981 2150 2996 2147 2980 2146 2973 2168 2998 1076 2902 2367 2993 3563 2999 2146 2973 2141 2977 2940 3000 2159 2991 2145 2987 2175 3001 2145 2987 2157 2976 2175 3001 2166 3002 3909 2997 2150 2996 2114 2951 2129 2952 2149 3003 2129 2952 2159 2991 2149 3003 2154 2986 2147 2980 2162 3004 3987 2966 2139 2967 2151 3005 2139 2967 2163 2990 2151 3005 3987 2966 2151 3005 2161 3006 2155 2995 3987 2966 2161 3006 2143 2979 2152 2982 2174 3007 2114 2951 2149 3003 2163 2990 2157 2976 2158 2983 2232 3008 2147 2980 2168 2998 2162 3004 2146 2973 2940 3000 2168 2998 2302 3009 2137 2970 2177 3010 2153 2972 2173 3011 2150 2996 2154 2986 2162 3004 2152 2982 2172 3012 2179 2994 2240 3013 2122 2958 2148 2989 2178 3014 2160 2985 2122 2958 2178 3014 2549 3015 3712 3016 2359 3017 2166 3002 2150 2996 2173 3011 2149 3003 2159 2991 2206 3018 3702 2841 2369 2988 2413 2901 2163 2990 2149 3003 2206 3018 2153 2972 2143 2979 2174 3007 2173 3011 2153 2972 2174 3007 2155 2995 2161 3006 2165 3019 2163 2990 2206 3018 2216 3020 2148 2989 2164 2963 2204 3021 2164 2963 2169 2978 2204 3021 2163 2990 2216 3020 2151 3005 2155 2995 2165 3019 2179 2994 2179 2994 2165 3019 2255 3022 2152 2982 2176 3023 2174 3007 2167 3024 4060 2657 2166 3002 2157 2976 2232 3008 2175 3001 2137 2970 2160 2985 2177 3010 2159 2991 2175 3001 2230 3025 2166 3002 2173 3011 2951 3026 2167 3024 2166 3002 2951 3026 2940 3000 2180 3027 2168 2998 3400 3028 2172 3012 2240 3013 2182 3029 2302 3009 2177 3010 2176 3023 2152 2982 2162 3004 2171 2992 2979 3030 2169 2978 3400 3028 2240 3013 2365 3031 2162 3004 2168 2998 2180 3027 2958 3032 2951 3026 2173 3011 2174 3007 2176 3023 2184 3033 2174 3007 2184 3033 2173 3011 2249 3034 1276 2117 2250 3035 2151 3005 2216 3020 2237 3036 2161 3006 2151 3005 2237 3036 2359 3017 3712 3016 2306 3037 2176 3023 2162 3004 2945 3038 2162 3004 2180 3027 2945 3038 2179 2994 2255 3022 2240 3013 2161 3006 2237 3036 2165 3019 2249 3034 2250 3035 2233 3039 2320 3040 2182 3029 2355 3041 2251 3042 2203 3043 2185 3044 2159 2991 2230 3025 2206 3018 1287 2145 2931 3045 2272 3046 2176 3023 2945 3038 2181 3047 2176 3023 2181 3047 2184 3033 2958 3032 2173 3011 2184 3033 3763 212 2958 3032 2184 3033 2249 3034 2233 3039 2170 3048 2088 2851 2089 2853 2227 3049 2216 3020 2206 3018 2220 3050 2160 2985 2178 3014 2177 3010 2189 3051 2373 3052 2187 3053 2373 3052 2316 3054 2187 3053 2449 3055 2186 3056 2187 3053 2203 3043 2170 3048 2235 3057 2316 3054 2261 3058 2187 3053 2187 3053 2186 3056 2190 3059 2186 3056 2188 3060 2190 3059 2429 3061 2189 3051 2190 3059 2189 3051 2187 3053 2190 3059 2190 3059 2188 3060 2191 3062 2430 3063 2429 3061 2191 3062 2188 3060 2192 3064 2191 3062 2429 3061 2190 3059 2191 3062 2191 3062 2192 3064 2193 3065 2192 3064 2194 3066 2193 3065 2451 3067 2430 3063 2193 3065 2430 3063 2191 3062 2193 3065 2178 3014 2148 2989 2247 3068 2148 2989 2204 3021 2247 3068 2473 3069 2451 3067 2195 3070 2193 3065 2194 3066 2195 3070 2451 3067 2193 3065 2195 3070 2194 3066 2614 3071 2195 3070 2195 3070 2614 3071 2211 3072 2614 3071 2640 3073 2211 3072 2473 3069 2195 3070 2211 3072 2501 3074 2473 3069 2211 3072 2519 3075 2531 3076 2196 3077 2417 3078 2217 3079 2196 3077 2446 3080 2417 3078 2196 3077 2217 3079 2519 3075 2196 3077 2446 3080 2196 3077 2197 3081 2452 3082 2479 3083 2197 3081 2479 3083 2446 3080 2197 3081 2196 3077 2531 3076 2197 3081 2531 3076 2556 3084 2197 3081 2452 3082 2197 3081 2198 3085 2456 3086 2452 3082 2198 3085 2556 3084 2202 3087 2198 3085 2486 3088 2456 3086 2198 3085 2197 3081 2556 3084 2198 3085 2486 3088 2198 3085 2200 3089 3400 3028 2365 3031 2328 3090 2199 3091 2486 3088 2200 3089 2198 3085 2202 3087 2200 3089 2202 3087 2550 3092 2200 3089 2251 3042 2185 3044 2319 3093 2107 2950 1225 2055 2228 3094 2235 3057 2170 3048 2233 3039 2169 2978 2979 3030 2204 3021 2203 3043 2235 3057 2185 3044 2206 3018 2230 3025 2205 3095 2206 3018 2205 3095 2259 3096 2258 3097 2259 3096 2208 3098 2398 3099 2207 3100 2208 3098 2512 3101 2501 3074 2209 3102 2501 3074 2211 3072 2209 3102 2209 3102 2211 3072 2210 3103 2470 3104 2209 3102 2210 3103 2661 3105 2212 3106 2213 3107 2640 3073 2661 3105 2213 3107 2211 3072 2640 3073 2213 3107 2212 3106 2242 2816 2213 3107 2210 3103 2211 3072 2213 3107 2214 3108 2413 2901 2244 3109 2217 3079 2417 3078 2244 3109 2417 3078 2214 3108 2244 3109 2216 3020 2220 3050 2237 3036 2499 3110 2519 3075 2218 3111 2215 3112 2499 3110 2218 3111 2217 3079 2244 3109 2218 3111 2519 3075 2217 3079 2218 3111 2199 3091 2200 3089 2221 3113 2491 3114 2199 3091 2221 3113 2219 3115 2221 3113 2266 3116 2221 3113 2200 3089 2266 3116 2200 3089 2550 3092 2222 3117 2550 3092 2568 3118 2222 3117 2266 3116 2200 3089 2222 3117 2177 3010 2178 3014 2357 3119 2178 3014 2247 3068 2357 3119 2568 3118 2223 3120 2224 3121 2222 3117 2568 3118 2224 3121 2225 2938 2119 2948 3910 2981 1573 2024 2225 2938 3910 2981 2107 2950 2228 3094 2226 2934 2118 2933 2107 2950 2226 2934 2170 3048 2229 3122 2248 3123 2228 3094 1225 2055 2248 3123 2229 3122 2228 3094 2248 3123 2170 3048 2203 3043 2229 3122 2203 3043 2251 3042 2276 3124 2229 3122 2203 3043 2276 3124 2175 3001 2232 3008 2231 3125 2205 3095 2230 3025 2231 3125 2230 3025 2175 3001 2231 3125 2205 3095 2231 3125 2238 3126 2208 3098 2259 3096 2238 3126 2259 3096 2205 3095 2238 3126 2270 3127 2220 3050 2234 3128 2206 3018 2259 3096 2234 3128 2220 3050 2206 3018 2234 3128 2398 3099 2208 3098 2236 3129 2208 3098 2238 3126 2236 3129 2253 3130 2398 3099 2236 3129 2422 3131 2447 3132 2239 3133 2208 3098 2207 3100 2239 3133 2207 3100 2422 3131 2239 3133 2979 3030 2323 3134 2204 3021 2470 3104 2210 3103 2496 3135 2210 3103 2213 3107 2241 3136 2241 3136 2213 3107 2242 2816 2244 3109 2413 2901 2243 3137 2365 3031 2240 3013 2339 3138 2218 3111 2244 3109 2243 3137 2474 3139 2215 3112 2243 3137 2215 3112 2218 3111 2243 3137 2266 3116 2222 3117 2246 3140 2222 3117 2224 3121 2246 3140 2223 3120 2245 3141 2271 3142 2245 3141 2273 3143 2271 3142 2931 3045 2317 3144 2272 3046 2224 3121 2223 3120 2271 3142 2246 3140 2224 3121 2271 3142 2255 3022 2165 3019 2370 3145 2131 2962 2133 2924 2277 3146 2133 2924 2121 2936 2277 3146 2142 2968 2131 2962 2277 3146 2121 2936 2226 2934 2277 3146 2170 3048 2248 3123 2249 3034 2248 3123 1276 2117 1225 2055 1276 2117 2249 3034 2248 3123 2231 3125 2232 3008 2279 3147 2238 3126 2231 3125 2279 3147 2240 3013 2255 3022 2339 3138 2232 3008 2158 2983 2279 3147 2276 3124 2251 3042 2252 3148 2319 3093 2185 3044 2350 3149 2275 3150 2276 3124 2252 3148 2251 3042 2319 3093 2252 3148 2319 3093 2281 3151 2252 3148 2238 3126 2279 3147 2254 3152 2253 3130 2236 3129 2256 3153 2236 3129 2238 3126 2256 3153 2238 3126 2254 3152 2256 3153 2282 3154 2253 3130 2256 3153 2270 3127 2234 3128 2257 3155 2234 3128 2259 3096 2257 3155 2258 3097 2261 3058 2257 3155 2261 3058 2316 3054 2257 3155 2259 3096 2258 3097 2257 3155 2316 3054 2270 3127 2257 3155 2208 3098 2239 3133 2260 3156 2239 3133 2447 3132 2260 3156 2261 3058 2258 3097 2260 3156 2258 3097 2208 3098 2260 3156 2515 3157 2496 3135 2262 3158 2496 3135 2210 3103 2262 3158 2350 3149 2185 3044 2296 3159 2210 3103 2241 3136 2262 3158 4108 3160 2328 3090 2268 3161 2515 3157 2262 3158 2286 3162 2586 3163 2515 3157 2286 3162 2247 3068 2204 3021 2323 3134 2286 3162 2262 3158 2288 3164 2262 3158 2241 3136 2288 3164 2182 3029 2177 3010 2348 3165 2241 3136 2242 2816 2263 2865 2288 3164 2241 3136 2263 2865 2044 2866 2137 2970 2263 2865 2237 3036 2220 3050 2270 3127 2243 3137 2369 2988 2264 3166 2243 3137 2264 3166 2265 3167 2474 3139 2243 3137 2265 3167 2468 3168 2474 3139 2265 3167 2219 3115 2266 3116 2269 3169 2266 3116 2246 3140 2269 3169 2526 3170 2219 3115 2269 3169 2529 3171 2526 3170 2269 3169 2529 3171 2269 3169 2267 3172 2520 3173 2529 3171 2267 3172 2267 3172 2269 3169 2293 3174 2269 3169 2246 3140 2293 3174 3165 1787 3794 1330 3164 1786 2293 3174 2246 3140 2294 3175 2182 3029 2348 3165 2355 3041 2271 3142 2273 3143 2294 3175 2272 3046 2317 3144 2318 3176 2246 3140 2271 3142 2294 3175 2274 3177 2606 3178 2294 3175 2273 3143 2274 3177 2294 3175 2185 3044 2235 3057 2296 3159 2228 3094 2229 3122 2276 3124 2275 3150 2226 2934 2276 3124 2226 2934 2228 3094 2276 3124 2125 2961 2142 2968 2278 3179 2142 2968 2277 3146 2278 3179 2158 2983 2125 2961 2278 3179 2279 3147 2158 2983 2278 3179 2277 3146 2226 2934 2280 3180 2279 3147 2278 3179 2280 3180 2226 2934 2275 3150 2280 3180 2278 3179 2277 3146 2280 3180 2254 3152 2279 3147 2280 3180 2280 3180 2275 3150 2283 3181 2254 3152 2280 3180 2283 3181 2281 3151 2282 3154 2283 3181 2275 3150 2252 3148 2283 3181 2252 3148 2281 3151 2283 3181 2256 3153 2254 3152 2283 3181 2282 3154 2256 3153 2283 3181 2447 3132 2449 3055 2285 3182 2260 3156 2447 3132 2285 3182 2282 3154 2281 3151 2284 3183 2449 3055 2187 3053 2285 3182 2187 3053 2261 3058 2285 3182 2281 3151 2319 3093 2284 3183 2261 3058 2260 3156 2285 3182 4108 3160 2268 3161 2410 3184 2586 3163 2286 3162 2287 3185 2286 3162 2288 3164 2300 3186 2287 3185 2286 3162 2300 3186 2288 3164 2263 2865 2289 3187 2263 2865 2137 2970 2289 3187 2137 2970 2302 3009 2289 3187 2264 3166 2369 2988 2394 3188 2264 3166 2394 3188 2303 3189 2265 3167 2264 3166 2290 3190 2468 3168 2265 3167 2290 3190 2264 3166 2303 3189 2290 3190 2520 3173 2267 3172 2291 3191 2267 3172 2293 3174 2291 3191 2291 3191 2293 3174 2292 3192 2567 3193 2291 3191 2292 3192 2235 3057 2233 3039 2351 3194 2294 3175 2606 3178 2295 3195 2293 3174 2294 3175 2295 3195 2611 3196 2629 3197 2295 3195 2606 3178 2611 3196 2295 3195 2101 2840 3707 3198 2394 3188 2282 3154 2284 3183 2297 3199 2250 3035 2272 3046 2318 3176 2307 3200 2300 3186 2299 3201 2299 3201 2300 3186 2309 3202 2288 3164 2289 3187 2309 3202 2289 3187 2302 3009 2309 3202 2300 3186 2288 3164 2309 3202 2302 3009 2182 3029 2309 3202 2394 3188 2301 3203 2359 3017 2303 3189 2394 3188 2359 3017 2303 3189 2359 3017 2304 3204 2414 3205 2444 3206 2304 3204 2290 3190 2303 3189 2304 3204 2444 3206 2290 3190 2304 3204 2296 3159 2235 3057 2349 3207 2567 3193 2292 3192 2305 3208 2292 3192 2293 3174 2314 3209 2233 3039 2250 3035 2351 3194 2293 3174 2295 3195 2314 3209 2237 3036 2270 3127 2316 3054 2298 3210 2307 3200 2308 3211 2165 3019 2237 3036 2316 3054 2307 3200 2299 3201 2308 3211 2006 3212 2587 3213 2308 3211 2309 3202 2182 3029 2320 3040 2299 3201 2309 3202 2320 3040 2306 3037 2227 3049 2310 3214 2359 3017 2306 3037 2310 3214 2227 3049 2400 3215 2310 3214 2304 3204 2359 3017 2424 3216 2359 3017 2310 3214 2424 3216 2310 3214 2400 3215 2424 3216 2414 3205 2304 3204 2424 3216 2305 3208 2292 3192 2311 3217 2292 3192 2314 3209 2311 3217 2582 3218 2305 3208 2311 3217 2565 3219 2582 3218 2312 3220 2311 3217 2314 3209 2312 3220 2582 3218 2311 3217 2312 3220 2165 3019 2316 3054 2370 3145 2314 3209 2295 3195 2313 3221 2629 3197 2315 3222 2313 3221 2295 3195 2629 3197 2313 3221 2314 3209 2313 3221 2326 3223 2315 3222 2325 3224 2326 3223 2313 3221 2315 3222 2326 3223 2317 3144 2920 3225 2318 3176 2308 3211 2299 3201 2329 3226 2297 3199 2284 3183 2352 3227 2319 3093 2350 3149 2352 3227 2299 3201 2320 3040 2329 3226 2006 3212 2308 3211 2329 3226 2284 3183 2319 3093 2352 3227 2329 3226 2320 3040 2321 3228 2320 3040 2355 3041 2321 3228 2355 3041 2372 3229 2321 3228 2565 3219 2312 3220 2322 3230 2312 3220 2314 3209 2327 3231 2347 3232 4108 3160 2324 3233 2314 3209 2326 3223 2327 3231 2325 3224 2669 3234 2327 3231 2669 3234 2664 3235 2327 3231 2326 3223 2325 3224 2327 3231 2328 3090 2365 3031 2268 3161 2235 3057 2351 3194 2349 3207 2006 3212 2329 3226 2330 3236 2329 3226 2321 3228 2052 2945 2364 3237 2382 3238 2052 2945 2330 3236 2329 3226 2052 2945 2321 3228 2364 3237 2052 2945 2382 3238 2376 3239 2052 2945 2597 3240 2322 3230 2332 3241 2322 3230 2312 3220 2332 3241 2312 3220 2327 3231 2332 3241 2255 3022 2370 3145 2189 3051 2608 3242 2597 3240 2331 3243 2339 3138 2255 3022 2189 3051 2597 3240 2332 3241 2331 3243 2331 3243 2332 3241 2343 3244 2332 3241 2327 3231 2343 3244 2327 3231 2664 3235 2333 3245 2343 3244 2327 3231 2333 3245 2668 3246 2338 3247 2333 3245 2664 3235 2668 3246 2333 3245 2177 3010 2357 3119 2348 3165 2250 3035 1276 2117 2334 3248 2272 3046 2250 3035 2334 3248 2376 3239 2389 3249 2335 3250 2035 2943 2052 2945 2335 3250 2052 2945 2376 3239 2335 3250 2389 3249 2035 2943 2335 3250 2610 3251 2604 3252 2336 3253 2604 3252 2608 3242 2336 3253 2608 3242 2331 3243 2336 3253 2331 3243 2343 3244 2336 3253 2610 3251 2336 3253 2337 3254 2336 3253 2343 3244 2337 3254 2343 3244 2333 3245 2346 3255 2333 3245 2338 3247 2346 3255 2338 3247 2344 3256 2346 3255 2340 3257 1287 2145 1276 2117 2340 3257 1276 2117 2334 3248 2272 3046 2334 3248 2340 3257 1287 2145 2272 3046 2340 3257 2622 3258 2610 3251 2341 3259 2607 3260 2649 3261 2341 3259 2610 3251 2337 3254 2341 3259 2649 3261 2622 3258 2341 3259 2296 3159 2349 3207 2366 3262 2607 3260 2341 3259 2342 3263 2021 2849 2650 2861 2342 3263 2022 2850 2021 2849 2342 3263 2011 2834 2022 2850 2342 3263 2337 3254 2343 3244 2342 3263 2343 3244 2346 3255 2342 3263 2341 3259 2337 3254 2342 3263 2650 2861 2607 3260 2342 3263 2346 3255 2344 3256 2345 2832 2011 2834 2342 3263 2345 2832 2342 3263 2346 3255 2345 2832 2355 3041 2348 3165 2354 3264 4108 3160 2410 3184 2324 3233 3909 2997 2166 3002 4060 2657 4060 2657 3910 2981 1573 2024 2352 3227 2350 3149 2368 3265 2350 3149 2296 3159 2368 3265 2351 3194 2250 3035 2356 3266 2250 3035 2318 3176 2356 3266 2357 3119 2247 3068 2358 3267 2349 3207 2351 3194 2353 3268 2370 3145 2316 3054 2373 3052 3707 3198 2301 3203 2394 3188 2282 3154 2297 3199 2360 3269 2253 3130 2282 3154 2360 3269 2101 2840 2394 3188 2369 2988 2366 3262 2349 3207 2353 3268 2247 3068 2323 3134 2358 3267 2368 3265 2296 3159 2363 3270 2351 3194 2356 3266 2371 3271 2920 3225 2921 3272 2318 3176 2321 3228 2372 3229 2364 3237 2361 3273 2347 3232 2483 3274 2366 3262 2353 3268 2380 3275 2372 3229 2355 3041 2377 3276 2355 3041 2354 3264 2377 3276 2318 3176 2921 3272 2356 3266 2296 3159 2366 3262 2363 3270 2357 3119 2358 3267 2348 3165 2358 3267 2323 3134 2978 3277 2353 3268 2351 3194 2371 3271 2978 3277 3238 3278 2358 3267 2354 3264 2348 3165 2374 3279 2253 3130 2360 3269 2398 3099 2297 3199 2352 3227 2391 3280 2360 3269 2297 3199 2391 3280 2352 3227 2368 3265 2391 3280 2347 3232 2324 3233 2387 3281 2368 3265 2363 3270 2384 3282 2417 3078 3563 2999 2367 2993 2382 3238 2364 3237 2379 3283 2364 3237 2372 3229 2379 3283 2339 3138 2430 3063 2365 3031 2268 3161 2365 3031 2410 3184 2353 3268 2371 3271 2381 3284 2356 3266 2921 3272 2371 3271 2391 3280 2368 3265 2375 3285 2370 3145 2373 3052 2189 3051 2380 3275 2353 3268 2381 3284 2363 3270 2366 3262 2384 3282 2398 3099 2360 3269 2397 3286 2354 3264 2374 3279 2377 3276 2360 3269 2391 3280 2397 3286 2324 3233 2410 3184 2459 3287 2387 3281 2324 3233 2459 3287 3871 3288 2362 3289 2390 3290 3252 3291 2361 3273 2378 3292 2366 3262 2380 3275 2406 3293 2384 3282 2366 3262 2406 3293 2348 3165 2358 3267 2374 3279 3563 2999 2417 3078 2362 3289 2380 3275 2381 3284 4158 3294 2358 3267 3238 3278 2374 3279 2339 3138 2189 3051 2429 3061 2430 3063 2339 3138 2429 3061 2372 3229 2377 3276 2379 3283 2347 3232 2387 3281 2483 3274 2382 3238 2379 3283 2399 3295 2389 3249 2376 3239 2399 3295 2376 3239 2382 3238 2399 3295 2089 2853 2389 3249 2400 3215 2381 3284 2371 3271 2404 3296 2921 3272 2383 3297 2371 3271 2397 3286 2391 3280 2395 3298 2375 3285 2368 3265 2403 3299 2368 3265 2384 3282 2403 3299 2406 3293 2380 3275 3738 3300 2380 3275 4158 3294 3738 3300 2379 3283 2377 3276 2436 3301 2227 3049 2089 2853 2400 3215 2365 3031 2430 3063 2408 3302 3250 3303 3252 3291 2396 3304 2365 3031 2408 3302 2410 3184 2377 3276 2374 3279 2409 3305 2389 3249 2399 3295 2400 3215 2383 3297 2923 3306 2371 3271 2391 3280 2375 3285 2415 3307 2395 3298 2391 3280 2415 3307 2374 3279 3238 3278 2393 3308 4158 3294 2381 3284 2404 3296 2384 3282 2406 3293 2403 3299 2398 3099 2397 3286 2401 3309 2422 3131 2207 3100 2401 3309 2207 3100 2398 3099 2401 3309 2371 3271 2923 3306 2402 3310 2371 3271 2402 3310 2404 3296 3870 3311 3871 3288 2486 3088 2406 3293 3738 3300 3739 3312 2374 3279 2393 3308 2409 3305 2377 3276 2409 3305 2436 3301 3870 3311 2486 3088 2423 3313 2415 3307 2375 3285 2412 3314 2367 2993 2413 2901 2214 3108 2403 3299 2406 3293 2407 3315 2404 3296 2402 3310 2933 3316 3177 3317 1073 3317 3176 3317 2403 3299 2407 3315 2458 3318 2379 3283 2436 3301 2399 3295 2413 2901 2369 2988 2243 3137 2401 3309 2397 3286 2435 3319 2397 3286 2395 3298 2435 3319 2375 3285 2403 3299 2412 3314 2399 3295 2436 3301 2426 3320 3349 3321 3870 3311 3434 3322 2406 3293 3739 3312 2407 3315 2385 3323 2411 3323 2439 3323 3870 3311 2423 3313 3434 3322 3739 3312 4158 3294 2428 3324 4158 3294 2404 3296 2428 3324 2435 3319 2395 3298 2419 3325 2395 3298 2415 3307 2419 3325 2404 3296 2933 3316 2418 3326 2400 3215 2399 3295 2416 3327 2412 3314 2403 3299 2427 3328 2403 3299 2458 3318 2427 3328 3348 3329 2443 3330 2437 3331 2424 3216 2400 3215 2431 3332 2390 3290 2362 3289 2479 3083 2422 3131 2401 3309 2448 3333 2447 3132 2422 3131 2448 3333 2407 3315 3739 3312 2425 3334 2399 3295 2426 3320 2416 3327 2400 3215 2416 3327 2431 3332 2415 3307 2412 3314 2419 3325 3426 3335 2420 3336 2307 3200 2436 3301 2409 3305 2421 3337 2367 2993 2214 3108 2417 3078 3318 3338 3426 3335 2445 3339 2424 3216 2431 3332 2414 3205 2458 3318 2407 3315 2461 3340 2428 3324 2404 3296 2939 3341 2419 3325 2412 3314 2432 3342 3426 3335 2307 3200 2298 3210 3348 3329 2437 3331 3351 3343 2362 3289 2417 3078 2446 3080 2479 3083 2362 3289 2446 3080 2404 3296 2418 3326 2939 3341 2378 3292 2361 3273 2470 3104 2401 3309 2435 3319 2460 3344 2448 3333 2401 3309 2460 3344 2425 3334 3739 3312 2467 3345 2407 3315 2425 3334 2461 3340 2436 3301 2421 3337 2976 3346 2426 3320 2436 3301 2976 3346 3426 3335 2298 3210 2445 3339 3739 3312 2428 3324 2467 3345 2378 3292 2470 3104 3252 3291 2433 3347 2441 3348 2528 3349 3319 3350 3318 3338 2442 3351 2412 3314 2427 3328 2472 3352 2432 3342 2412 3314 2472 3352 2427 3328 2458 3318 2472 3352 2408 3302 2430 3063 2451 3067 2460 3344 2435 3319 2454 3353 2435 3319 2419 3325 2454 3353 2410 3184 2408 3302 2473 3069 2408 3302 2451 3067 2473 3069 2290 3190 2444 3206 2468 3168 2461 3340 2425 3334 2453 3354 2447 3132 2448 3333 2463 3355 2449 3055 2447 3132 2463 3355 2467 3345 2428 3324 2457 3356 2444 3206 2414 3205 2489 3357 2414 3205 2431 3332 2462 3358 4071 2944 3319 3350 2465 3359 2390 3290 2479 3083 2452 3082 3871 3288 2390 3290 2452 3082 3871 3288 2452 3082 2456 3086 2486 3088 3871 3288 2456 3086 2454 3353 2419 3325 2455 3360 2416 3327 2426 3320 2469 3361 2454 3353 2455 3360 2482 3362 2428 3324 2939 3341 2457 3356 2410 3184 2473 3069 2459 3287 3318 3338 2445 3339 2587 3213 2445 3339 2298 3210 2308 3211 2433 3347 2528 3349 2507 3363 2472 3352 2458 3318 2461 3340 2431 3332 2416 3327 2487 3364 2445 3339 2308 3211 2587 3213 2460 3344 2454 3353 2464 3365 2448 3333 2460 3344 2464 3365 2444 3206 2489 3357 2468 3168 2463 3355 2448 3333 2464 3365 2425 3334 2467 3345 2500 3366 2453 3354 2425 3334 2500 3366 2433 3347 2507 3363 2476 3367 2414 3205 2462 3358 2489 3357 2455 3360 2419 3325 2432 3342 2939 3341 2466 3368 2457 3356 2462 3358 2431 3332 2487 3364 2426 3320 2976 3346 2469 3361 2472 3352 2461 3340 2492 3369 3318 3338 2587 3213 2442 3351 2455 3360 2432 3342 2471 3370 2482 3362 2455 3360 2471 3370 2449 3055 2463 3355 2186 3056 2476 3367 2507 3363 2517 3371 3252 3291 2470 3104 2496 3135 2396 3304 3252 3291 2496 3135 2387 3281 2459 3287 2473 3069 2441 3348 3469 3372 2488 3373 2454 3353 2482 3362 2502 3374 2464 3365 2454 3353 2502 3374 2471 3370 2432 3342 2481 3375 2432 3342 2472 3352 2481 3375 2387 3281 2473 3069 2501 3074 2500 3366 2467 3345 2485 3376 2468 3168 2489 3357 2505 3377 2441 3348 2488 3373 2528 3349 2492 3369 2461 3340 2477 3378 2461 3340 2453 3354 2477 3378 2475 3379 4071 2944 2035 2943 2467 3345 2457 3356 2485 3376 3469 3372 3434 3322 2221 3113 2186 3056 2463 3355 2506 3380 2463 3355 2464 3365 2506 3380 2492 3369 2477 3378 2493 3381 2484 3382 2476 3367 2478 3383 2416 3327 2469 3361 2487 3364 2476 3367 2517 3371 2478 3383 2493 3381 2477 3378 2480 3384 2483 3274 2387 3281 2512 3101 2489 3357 2462 3358 2975 3385 2457 3356 2466 3368 2485 3376 2475 3379 2035 2943 2389 3249 2405 3386 3250 3303 2287 3185 2474 3139 2468 3168 2495 3387 2466 3368 2947 3388 2485 3376 2462 3358 2487 3364 2504 3389 2481 3375 2472 3352 2494 3390 2472 3352 2492 3369 2494 3390 3250 3303 2396 3304 2515 3157 2396 3304 2496 3135 2515 3157 2477 3378 2453 3354 2500 3366 2361 3273 2483 3274 2209 3102 2483 3274 2512 3101 2209 3102 2513 3391 2481 3375 2494 3390 3434 3322 2423 3313 2491 3114 3469 3372 2221 3113 2488 3373 2215 3112 2474 3139 2509 3392 2468 3168 2505 3377 2495 3387 2506 3380 2464 3365 2490 3393 3434 3322 2491 3114 2221 3113 2492 3369 2493 3381 2494 3390 2477 3378 2500 3366 2480 3384 2489 3357 2975 3385 2505 3377 2423 3313 2486 3088 2199 3091 2474 3139 2495 3387 2509 3392 2497 3394 2484 3382 2498 3395 2480 3384 2500 3366 2539 3396 2405 3386 2287 3185 2420 3336 2975 3385 2462 3358 2504 3389 2387 3281 2501 3074 2512 3101 2490 3393 2464 3365 2503 3397 2464 3365 2502 3374 2503 3397 2497 3394 2498 3395 2537 3398 2471 3370 2481 3375 2545 3399 2500 3366 2485 3376 2539 3396 2488 3373 2221 3113 2219 3115 2506 3380 2490 3393 2536 3400 2478 3383 2517 3371 2540 3401 2188 3060 2186 3056 2536 3400 2186 3056 2506 3380 2536 3400 2215 3112 2509 3392 2514 3402 2499 3110 2215 3112 2514 3402 2598 3403 4075 3404 4074 3405 2488 3373 2219 3115 2528 3349 2495 3387 2505 3377 2523 3406 2508 3407 2598 3403 4074 3405 2539 3396 2485 3376 2516 3408 2423 3313 2199 3091 2491 3114 2475 3379 2389 3249 2598 3403 2478 3383 2540 3401 2484 3382 2505 3377 2975 3385 2510 3409 2545 3399 2481 3375 2513 3391 2503 3397 2502 3374 2521 3410 2502 3374 2482 3362 2521 3410 2509 3392 2495 3387 2530 3411 2498 3395 2484 3382 2518 3412 2537 3398 2498 3395 2518 3412 2361 3273 2209 3102 2470 3104 2497 3394 2537 3398 2511 3413 3428 3414 2497 3394 2511 3413 2485 3376 2947 3388 2516 3408 2514 3402 2509 3392 2535 3415 2508 3407 4074 3405 2560 3416 2499 3110 2514 3402 2525 3417 2519 3075 2499 3110 2525 3417 2513 3391 2494 3390 2538 3418 2495 3387 2523 3406 2530 3411 2509 3392 2530 3411 2535 3415 2188 3060 2536 3400 2192 3064 2490 3393 2503 3397 2532 3419 2536 3400 2490 3393 2532 3419 2517 3371 2507 3363 2520 3173 2523 3406 2505 3377 2510 3409 2482 3362 2471 3370 2554 3420 2521 3410 2482 3362 2554 3420 2514 3402 2535 3415 2525 3417 2493 3381 2480 3384 2569 3421 2494 3390 2493 3381 2538 3418 2528 3349 2219 3115 2526 3170 2510 3409 2522 3422 2523 3406 2530 3411 2523 3406 2534 3423 1077 3424 3428 3414 2527 3425 2525 3417 2535 3415 2542 3426 2513 3391 2538 3418 2583 3427 2507 3363 2528 3349 2529 3171 2480 3384 2539 3396 2569 3421 2535 3415 2530 3411 2541 3428 3428 3414 2511 3413 2564 3429 3428 3414 2564 3429 2527 3425 2519 3075 2525 3417 2551 3430 2531 3076 2519 3075 2551 3430 2507 3363 2529 3171 2520 3173 2420 3336 2287 3185 2300 3186 2517 3371 2520 3173 2540 3401 2307 3200 2420 3336 2300 3186 2503 3397 2521 3410 2533 3431 2532 3419 2503 3397 2533 3431 2523 3406 2522 3422 2534 3423 2522 3422 2991 3432 2534 3423 2530 3411 2534 3423 2541 3428 2554 3420 2471 3370 2545 3399 2528 3349 2526 3170 2529 3171 2554 3420 2545 3399 2544 3433 2484 3382 2540 3401 2567 3193 2518 3412 2484 3382 2567 3193 3708 3434 2508 3407 2549 3015 2525 3417 2542 3426 2551 3430 2542 3426 2535 3415 2547 3435 2535 3415 2541 3428 2547 3435 2192 3064 2536 3400 2580 3436 2536 3400 2532 3419 2580 3436 2569 3421 2539 3396 2546 3437 2531 3076 2551 3430 2556 3084 2583 3427 2538 3418 2584 3438 2538 3418 2493 3381 2584 3438 2549 3015 2508 3407 2560 3416 2541 3428 2534 3423 2557 3439 2551 3430 2542 3426 2543 3440 2555 3441 1077 3424 2548 3442 2556 3084 2551 3430 2563 3443 2533 3431 2521 3410 2596 3444 2542 3426 2547 3435 2543 3440 2544 3433 2545 3399 2513 3391 2534 3423 2991 3432 2557 3439 2541 3428 2557 3439 2547 3435 2544 3433 2513 3391 2628 3445 2539 3396 2516 3408 2546 3437 2543 3440 2547 3435 2574 3446 2540 3401 2520 3173 2291 3191 2202 3087 2556 3084 2566 3447 2547 3435 2557 3439 2558 3448 3250 3303 2515 3157 2586 3163 2540 3401 2291 3191 2567 3193 2550 3092 2202 3087 2566 3447 2551 3430 2543 3440 2552 3449 2563 3443 2551 3430 2552 3449 2580 3436 2532 3419 2570 3450 2532 3419 2533 3431 2570 3450 3314 3451 2555 3441 2553 3452 2513 3391 2583 3427 2559 3453 2521 3410 2554 3420 2573 3454 2554 3420 2544 3433 2573 3454 2596 3444 2521 3410 2573 3454 2628 3445 2513 3391 2559 3453 2556 3084 2563 3443 2575 3455 2566 3447 2556 3084 2575 3455 2550 3092 2566 3447 2568 3118 2584 3438 2493 3381 2571 3456 2493 3381 2569 3421 2571 3456 2548 3442 1077 3424 2604 3252 2511 3413 2537 3398 2565 3219 2465 3359 3319 3350 2052 2945 2557 3439 2577 3457 2558 3448 2547 3435 2558 3448 2574 3446 2559 3453 2583 3427 2561 3458 2518 3412 2567 3193 2305 3208 2537 3398 2518 3412 2305 3208 2543 3440 2574 3446 2579 3459 2552 3449 2543 3440 2579 3459 2511 3413 2565 3219 2564 3429 2548 3442 2604 3252 2555 3441 2566 3447 2575 3455 2568 3118 2574 3446 2558 3448 2578 3460 2575 3455 2563 3443 2585 3461 2563 3443 2552 3449 2585 3461 2537 3398 2305 3208 2582 3218 2533 3431 2596 3444 2625 3462 2570 3450 2533 3431 2625 3462 3630 3463 3314 3451 2572 3464 2568 3118 2575 3455 2581 3465 2558 3448 2577 3457 2578 3460 2577 3457 2995 3466 2578 3460 2574 3446 2578 3460 2579 3459 2569 3421 2546 3437 2571 3456 2194 3066 2192 3064 2599 3467 2192 3064 2580 3436 2599 3467 2537 3398 2582 3218 2565 3219 2465 3359 2052 2945 4071 2944 2585 3461 2552 3449 2593 3468 2552 3449 2579 3459 2593 3468 2561 3458 2583 3427 2584 3438 2575 3455 2585 3461 2592 3469 2581 3465 2575 3455 2592 3469 1077 3424 2527 3425 2608 3242 2571 3456 2546 3437 2621 3470 2223 3120 2568 3118 2245 3141 2561 3458 2584 3438 2641 3471 2579 3459 2578 3460 2590 3472 3250 3303 2586 3163 2287 3185 2527 3425 2564 3429 2597 3240 2641 3471 2584 3438 2601 3473 2564 3429 2565 3219 2322 3230 2581 3465 2592 3469 2600 3474 2245 3141 2568 3118 2600 3474 2568 3118 2581 3465 2600 3474 1077 3424 2608 3242 2604 3252 3981 3475 3630 3463 2589 3476 3707 3198 3708 3434 2591 3477 2592 3469 2585 3461 2595 3478 2585 3461 2593 3468 2595 3478 2995 3466 2973 3479 2590 3472 2578 3460 2995 3466 2590 3472 3630 3463 2572 3464 2607 3260 2572 3464 3314 3451 2607 3260 2593 3468 2579 3459 2594 3480 2579 3459 2590 3472 2594 3480 2973 3479 2998 3481 2590 3472 2527 3425 2597 3240 2608 3242 2273 3143 2245 3141 2603 3482 2274 3177 2273 3143 2603 3482 2628 3445 2559 3453 2561 3458 2564 3429 2322 3230 2597 3240 2194 3066 2599 3467 2614 3071 2573 3454 2544 3433 2605 3483 2596 3444 2573 3454 2605 3483 2625 3462 2596 3444 2605 3483 2599 3467 2580 3436 2624 3484 2580 3436 2570 3450 2624 3484 2600 3474 2592 3469 2617 3485 2595 3478 2593 3468 2615 3486 2584 3438 2571 3456 2601 3473 3981 3475 2589 3476 2679 2870 2593 3468 2594 3480 2615 3486 2592 3469 2595 3478 2617 3485 4112 3487 3981 3475 2613 3488 2245 3141 2600 3474 2609 3489 2603 3482 2245 3141 2609 3489 2594 3480 2590 3472 2998 3481 2615 3486 2594 3480 2620 3490 2555 3441 2604 3252 2610 3251 2553 3452 2555 3441 2610 3251 2595 3478 2615 3486 2612 3491 2617 3485 2595 3478 2612 3491 2609 3489 2600 3474 2616 3492 2601 3473 2571 3456 2957 3493 2607 3260 3314 3451 2649 3261 2605 3483 2544 3433 2627 3494 3314 3451 2553 3452 2649 3261 3981 3475 2679 2870 2047 2884 2570 3450 2625 3462 2624 3484 2613 3488 3981 3475 2047 2884 2606 3178 2274 3177 2618 3495 2274 3177 2603 3482 2618 3495 2611 3196 2606 3178 2618 3495 2549 3015 2560 3416 3712 3016 2600 3474 2617 3485 2639 3496 2616 3492 2600 3474 2639 3496 2612 3491 2615 3486 2635 3497 2553 3452 2610 3251 2622 3258 2649 3261 2553 3452 2622 3258 3232 2852 4075 3404 2598 3403 2630 3498 4112 3487 2068 2899 2613 3488 2047 2884 4112 3487 2640 3073 2614 3071 2638 3499 2614 3071 2599 3467 2638 3499 2615 3486 2620 3490 2635 3497 2599 3467 2624 3484 2638 3499 2611 3196 2618 3495 2629 3197 2571 3456 2621 3470 2957 3493 2603 3482 2609 3489 2619 3500 2618 3495 2603 3482 2619 3500 2594 3480 2998 3481 2620 3490 2635 3497 2620 3490 2632 3501 2589 3476 3630 3463 2673 2863 2998 3481 2626 3502 2620 3490 2617 3485 2612 3491 2636 3503 2612 3491 2635 3497 2636 3503 2624 3484 2625 3462 2651 3504 2639 3496 2617 3485 2636 3503 2619 3500 2609 3489 2631 3505 2609 3489 2616 3492 2631 3505 2626 3502 2646 3506 2620 3490 3708 3434 2549 3015 2359 3017 3708 3434 2359 3017 2591 3477 2627 3494 2544 3433 2628 3445 2627 3494 2628 3445 2667 2807 2618 3495 2619 3500 2644 3507 2315 3222 2629 3197 2644 3507 2629 3197 2618 3495 2644 3507 2589 3476 2673 2863 2679 2870 2630 3498 2077 2910 2642 2929 2636 3503 2635 3497 2659 3508 2616 3492 2639 3496 2654 3509 2631 3505 2616 3492 2654 3509 3911 2648 2630 3498 2633 2937 2630 3498 2642 2929 2633 2937 2667 2807 2628 3445 2676 2808 2628 3445 2561 3458 2676 2808 2644 3507 2619 3500 2637 3510 2638 3499 2624 3484 2656 3511 2315 3222 2644 3507 2325 3224 2619 3500 2631 3505 2654 3509 2625 3462 2605 3483 2651 3504 2640 3073 2638 3499 2661 3105 2659 3508 2635 3497 2652 3512 2635 3497 2632 3501 2652 3512 2641 3471 2601 3473 2648 3513 2632 3501 2620 3490 2646 3506 2561 3458 2641 3471 2675 2830 2676 2808 2561 3458 2675 2830 3630 3463 2607 3260 2650 2861 2637 3510 2619 3500 2663 3514 2619 3500 2654 3509 2663 3514 2636 3503 2659 3508 2662 3515 2654 3509 2639 3496 2662 3515 2639 3496 2636 3503 2662 3515 2632 3501 2646 3506 2645 3516 2644 3507 2637 3510 2647 3517 2325 3224 2644 3507 2653 3518 2644 3507 2647 3517 2653 3518 2601 3473 2957 3493 2623 3519 2648 3513 2601 3473 2623 3519 2662 3515 2659 3508 2660 3520 2647 3517 2637 3510 2658 3521 2637 3510 2663 3514 2658 3521 2656 3511 2624 3484 2657 2823 2624 3484 2651 3504 2657 2823 2651 3504 2605 3483 2627 3494 3630 3463 2650 2861 2673 2863 3232 2852 2598 3403 2089 2853 2598 3403 2389 3249 2089 2853 2325 3224 2653 3518 2669 3234 2653 3518 2647 3517 2666 3522 2663 3514 2654 3509 2655 3523 2654 3509 2662 3515 2655 3523 2652 3512 2632 3501 2665 3524 2659 3508 2652 3512 2660 3520 2638 3499 2656 3511 2661 3105 2632 3501 2645 3516 2665 3524 2634 2839 3707 3198 2101 2840 2647 3517 2658 3521 2666 3522 2655 3523 2662 3515 2674 3525 2662 3515 2660 3520 2674 3525 2442 3351 2587 3213 2006 3212 2653 3518 2666 3522 2670 3526 2669 3234 2653 3518 2670 3526 2663 3514 2655 3523 2672 3527 2658 3521 2663 3514 2672 3527 2068 2899 4112 3487 2063 2889 4112 3487 2047 2884 2063 2889 2664 3235 2669 3234 2671 3528 2683 3529 2660 3520 2677 3530 2660 3520 2652 3512 2677 3530 2645 3516 2687 3531 2665 3524 2666 3522 2658 3521 2678 3532 2666 3522 2678 3532 2670 3526 2591 3477 2359 3017 2301 3203 2655 3523 2674 3525 2672 3527 2668 3246 2664 3235 2681 3533 2664 3235 2671 3528 2681 3533 1573 2024 3911 2648 2633 2937 2660 3520 2683 3529 2682 3534 2674 3525 2660 3520 2682 3534 2651 3504 2627 3494 2684 2824 2657 2823 2651 3504 2684 2824 2212 3106 2661 3105 2242 2816 2668 3246 2681 3533 2338 3247 2669 3234 2670 3526 2685 3535 2661 3105 2656 3511 2686 2817 2656 3511 2657 2823 2686 2817 2671 3528 2669 3234 2685 3535 2683 3529 2677 3530 2691 3536 2658 3521 2672 3527 2678 3532 2641 3471 2648 3513 2675 2830 2670 3526 2678 3532 2690 3537 2672 3527 2674 3525 2680 3538 2077 2910 2630 3498 2068 2899 2442 3351 2006 3212 2330 3236 2652 3512 2665 3524 2687 3531 2052 2945 3319 3350 2330 3236 2677 3530 2652 3512 2687 3531 3319 3350 2442 3351 2330 3236 2672 3527 2680 3538 2694 3539 2678 3532 2672 3527 2694 3539 2681 3533 2671 3528 2697 3540 2671 3528 2685 3535 2697 3540 2674 3525 2682 3534 2680 3538 2678 3532 2694 3539 2690 3537 2675 2830 2648 3513 2688 2846 2338 3247 2681 3533 2689 3541 2681 3533 2697 3540 2689 3541 2344 3256 2338 3247 2689 3541 2680 3538 2682 3534 2693 3542 2693 3542 2682 3534 2695 3543 2682 3534 2683 3529 2695 3543 2677 3530 2687 3531 2691 3536 2591 3477 2301 3203 3707 3198 2648 3513 2623 3519 2688 2846 2685 3535 2670 3526 2706 3544 2670 3526 2690 3537 2706 3544 2683 3529 2691 3536 2695 3543 2627 3494 2667 2807 2684 2824 2697 3540 2685 3535 2699 2826 2690 3537 2694 3539 2698 2819 2344 3256 2689 3541 2700 3545 2344 3256 2700 3545 2345 2832 2680 3538 2693 3542 2702 2813 2694 3539 2680 3538 2702 2813 2689 3541 2697 3540 2004 2804 2700 3545 2689 3541 2004 2804 2685 3535 2706 3544 2699 2826 2661 3105 2686 2817 2242 2816 2693 3542 2695 3543 2692 2842 2676 2808 2675 2830 2704 2829 2695 3543 2691 3536 2703 2810 2691 3536 2687 3531 2703 2810 2697 3540 2699 2826 2005 2805 2004 2804 2697 3540 2005 2805 2690 3537 2698 2819 2707 3546 2706 3544 2690 3537 2707 3546 2694 3539 2702 2813 2020 2815 2698 2819 2694 3539 2020 2815 2699 2826 2706 3544 2016 2827 2686 2817 2657 2823 2705 2825 2700 3545 2004 2804 2701 2806 2684 2824 2667 2807 2007 2809 2345 2832 2700 3545 2010 2821 2700 3545 2701 2806 2010 2821 2702 2813 2693 3542 2013 2814 2692 2842 2695 3543 2008 2812 2695 3543 2703 2810 2008 2812 2005 2805 2699 2826 2025 2828 2693 3542 2692 2842 2013 2814 2707 3546 2698 2819 2009 2820 2706 3544 2707 3546 2016 2827 2707 3546 2009 2820 2016 2827 2711 3547 3908 3548 3002 3549 3087 3550 2774 1826 2712 1584 2710 3551 3098 3552 2708 3553 2716 1829 3087 3550 2712 1584 2714 3554 2708 3553 2715 1870 2714 3554 2710 3551 2708 3553 2713 3555 3087 3550 2716 1829 3478 3556 2713 3555 2716 1829 2717 3557 2714 3554 2715 1870 3479 3558 3478 3556 2716 1829 2719 1875 2717 3557 2715 1870 3950 3559 3479 3558 2716 1829 2723 3560 2717 3557 2719 1875 3705 3561 3479 3558 3950 3559 2723 3560 2719 1875 2722 1822 2726 3562 2989 3563 2721 3564 3952 3565 3951 3565 3369 3565 4284 3566 3208 1477 865 1471 2724 3567 3705 3561 3950 3559 2725 3568 2723 3560 2722 1822 1098 3569 3954 3570 1095 3571 4290 3572 4284 3566 865 1471 4290 3572 865 1471 1097 1837 2728 3573 2725 3568 2722 1822 2730 3574 3954 3570 1098 3569 2730 3574 1098 3569 2731 3575 2733 1823 2728 3573 2722 1822 2729 3576 1097 1837 1096 1465 2729 3576 4290 3572 1097 1837 2732 3577 2728 3573 2733 1823 1100 3578 2730 3574 2731 3575 2734 3579 2730 3574 1100 3578 2732 3577 2733 1823 1093 1830 2737 3580 2729 3576 1096 1465 2736 3581 1100 3578 4135 3582 2736 3581 2734 3579 1100 3578 2737 3580 1096 1465 967 1594 2738 3583 967 1594 1090 1655 2738 3583 2737 3580 967 1594 2741 3584 2732 3577 1093 1830 3495 3585 3515 3585 2735 3585 3765 3586 3517 3586 3496 3586 3637 3587 1093 1830 2746 1833 1088 3588 2738 3583 1090 1655 2974 3589 2738 3583 1088 3588 3458 3590 3764 3590 2740 3590 3637 3587 2741 3584 1093 1830 3459 3591 2183 3591 3456 3591 3742 3592 3863 1827 2747 1638 3740 3593 2974 3589 1088 3588 2745 3594 2974 3589 3740 3593 2746 1833 109 3595 3637 3587 3485 3596 3460 3596 2739 3596 103 3597 2743 3598 2749 3599 3698 3600 3263 3601 3741 3602 3742 3592 2747 1638 4188 1630 2753 3603 103 3597 2749 3599 3487 3604 109 3595 2746 1833 2748 3605 2744 3606 953 3607 3659 3608 2748 3605 953 3607 2753 3603 2749 3599 2754 3609 3312 3610 3699 3610 3658 3610 947 1626 3487 3604 2746 1833 2755 3611 3487 3604 947 1626 2752 3612 2753 3603 2754 3609 2752 3612 2754 3609 2758 1859 1084 3613 2751 3613 2750 3613 3090 3614 3310 3615 4224 3616 2756 3617 2755 3611 947 1626 2756 3617 2784 1845 2759 3618 2757 3619 2752 3612 2758 1859 2756 3617 947 1626 2784 1845 3091 3620 3090 3614 4224 3616 4225 3621 3091 3620 4224 3616 3175 3622 2757 3619 2758 1859 2762 1602 3175 3622 2758 1859 3653 3623 3091 3620 4225 3621 2764 1817 3653 3623 4225 3621 2767 3624 3175 3622 2762 1602 2766 3625 2767 3624 2762 1602 3152 3626 3653 3623 2764 1817 3150 3627 2771 3628 2972 3629 3152 3626 2764 1817 2768 1590 3099 3630 2767 3624 2766 3625 3089 3631 3152 3626 2768 1590 3109 3632 3099 3630 2766 3625 3098 3552 3099 3630 3109 3632 4208 3633 2771 3628 3150 3627 2711 3547 3002 3549 4208 3633 3088 3634 3003 3634 3151 3634 3086 3635 2769 3635 2773 3635 3087 3550 2768 1590 2774 1826 3087 3550 3089 3631 2768 1590 3098 3552 3109 3632 2708 3553 4102 3636 2967 3637 2775 3638 2834 1861 2777 3639 2835 3640 2777 3639 2834 1861 1107 1856 4102 3636 2775 3638 2776 3641 3373 3642 4102 3636 2776 3641 4039 3643 4102 3636 3373 3642 2780 3644 2777 3639 1107 1856 3227 3645 4039 3643 3373 3642 1113 3646 3227 3645 3373 3642 2782 3647 2780 3644 1107 1856 2782 3647 1107 1856 2783 1438 3228 3648 3227 3645 1113 3646 3228 3648 1113 3646 906 1555 2787 3649 2782 3647 2783 1438 2787 3649 2783 1438 2786 1851 3730 3650 3228 3648 906 1555 3730 3650 906 1555 1109 1548 2784 1845 102 3651 2759 3618 2788 3652 2983 3653 2785 3654 128 3655 2787 3649 2786 1851 2784 1845 4161 3656 102 3651 2793 3657 128 3655 2786 1851 2788 3652 2792 3658 2983 3653 3730 3650 1109 1548 4315 1862 2789 1847 2793 3657 2786 1851 4161 3656 2784 1845 2791 1850 2792 3658 3730 3650 4315 1862 3450 3659 2792 3658 4315 1862 2794 201 2793 3657 2789 1847 112 3660 4161 3656 2791 1850 2795 3661 2789 1847 2797 1843 2984 3662 2792 3658 3450 3659 2795 3661 2794 201 2789 1847 4325 3663 112 3660 2791 1850 2796 1518 2798 3664 4314 1531 3451 3665 2798 3664 2796 1518 2799 209 2794 201 2795 3661 4325 3663 2791 1850 1106 3666 2806 3667 3450 3659 2802 3668 2806 3667 2984 3662 3450 3659 111 3669 4325 3663 1106 3666 4227 3670 2797 1843 2801 1840 4227 3670 2795 3661 2797 1843 4227 3670 2799 209 2795 3661 2805 3671 2799 209 4227 3670 120 3672 1106 3666 1108 1855 1105 1511 3451 3665 2796 1518 120 3672 111 3669 1106 3666 2803 3673 3451 3665 1105 1511 2808 3674 3451 3665 2803 3673 4256 3675 4173 210 2804 3676 4168 3677 2806 3667 2802 3668 2801 1840 2807 3678 4227 3670 1103 1504 2803 3673 1105 1511 119 3679 120 3672 1108 1855 2807 3678 2801 1840 2809 3680 2810 1860 119 3679 1108 1855 2936 3681 2807 3678 2809 3680 2803 3673 1103 1504 877 1483 2811 3682 119 3679 2810 1860 2936 3681 2809 3680 2812 3683 2816 3684 2803 3673 877 1483 2814 3685 2936 3681 2812 3683 2813 3686 2814 3685 2812 3683 2819 3687 2814 3685 2813 3686 3976 3688 2816 3684 877 1483 2815 3689 2811 3682 2810 1860 3976 3688 877 1483 3208 1477 2815 3689 2810 1860 2818 1495 2819 3687 2813 3686 1086 3690 3209 3691 3976 3688 3208 1477 2823 3692 2815 3689 2818 1495 2821 3693 2819 3687 1086 3690 2820 3694 2721 3564 2989 3563 2822 3695 2821 3693 1086 3690 2827 3696 2822 3695 1086 3690 2824 3697 2823 3692 2818 1495 2826 3698 2818 1495 2825 1864 2826 3698 2824 3697 2818 1495 3937 3699 2822 3695 2827 3696 2832 3700 2826 3698 2825 1864 3858 3701 3605 3701 4200 3701 2831 1863 2832 3700 2825 1864 2830 3702 3937 3699 2827 3696 2829 3703 3857 3704 1115 3705 3796 3706 2832 3700 2831 1863 2967 3637 3857 3704 2829 3703 2775 3638 2967 3637 2829 3703 2834 1861 3796 3706 2831 1863 2835 3640 3796 3706 2834 1861 4007 660 2982 660 2836 660 4008 660 2838 660 2982 660 4008 660 2982 660 4007 660 1138 660 4007 660 2836 660 2837 3707 2840 3707 2838 3707 2837 3708 2838 3708 4008 3708 2839 3709 1138 3709 2836 3709 3714 3710 1138 3710 2839 3710 2843 660 3466 660 2840 660 2843 3711 2840 3711 2837 3711 2986 3712 3714 3712 2839 3712 3729 3713 3714 3713 2986 3713 4044 3714 3467 3714 3466 3714 4044 3715 3466 3715 2843 3715 2845 3716 3729 3716 2986 3716 1127 3717 3729 3717 2845 3717 4127 660 3200 660 3467 660 4127 3718 3467 3718 4044 3718 3477 3719 1127 3719 2845 3719 1124 660 1127 660 3477 660 4126 3720 3200 3720 4127 3720 4126 660 2847 660 3200 660 3902 3721 1124 3721 3477 3721 1125 660 1124 660 3902 660 2846 3722 2847 3722 4126 3722 2846 660 3226 660 2847 660 3861 3723 1125 3723 3902 3723 3543 3724 1125 3724 3861 3724 4253 3725 3226 3725 2846 3725 4253 660 3876 660 3226 660 3860 3726 3543 3726 3861 3726 1120 660 3543 660 3860 660 2853 3727 3876 3727 4253 3727 2853 660 2852 660 3876 660 2990 660 1120 660 3860 660 2850 660 1120 660 2990 660 2851 3728 2856 3728 2852 3728 2851 3729 2852 3729 2853 3729 2854 3730 2850 3730 2990 3730 2857 660 2850 660 2854 660 1155 3731 2856 3731 2851 3731 1155 660 2855 660 2856 660 2968 660 2857 660 2854 660 2859 660 2855 660 1155 660 2858 3732 2857 3732 2968 3732 133 3733 2859 3733 1155 3733 2860 3734 2858 3734 2968 3734 4097 3735 2859 3735 133 3735 1157 3736 2858 3736 2860 3736 137 3737 4097 3737 133 3737 2992 660 1157 660 2860 660 4098 3738 4097 3738 137 3738 4309 660 1157 660 2992 660 2864 660 4098 660 137 660 3110 660 4309 660 2992 660 2863 3739 4098 3739 2864 3739 1151 3740 4309 3740 3110 3740 3343 660 2863 660 2864 660 3092 3741 1151 3741 3110 3741 3399 3742 2863 3742 3343 3742 2868 3743 1151 3743 3092 3743 2869 660 3399 660 3343 660 3093 660 2868 660 3092 660 2870 3744 3399 3744 2869 3744 1146 3745 2868 3745 3093 3745 3634 3746 2870 3746 2869 3746 3126 3747 1146 3747 3093 3747 3409 3748 2870 3748 3634 3748 1147 3749 1146 3749 3126 3749 146 660 3409 660 3634 660 3095 660 1147 660 3126 660 2872 3750 3409 3750 146 3750 145 660 2872 660 146 660 1142 3751 1147 3751 3095 3751 3094 660 1142 660 3095 660 115 3752 2872 3752 145 3752 3901 660 115 660 145 660 3685 3753 1142 3753 3094 3753 3000 660 3685 660 3094 660 2876 3754 115 3754 3901 3754 150 3755 2876 3755 3901 3755 2877 3756 3685 3756 3000 3756 3001 3757 2877 3757 3000 3757 2880 3758 2876 3758 150 3758 157 660 2880 660 150 660 3083 660 2877 660 3001 660 4278 660 3083 660 3001 660 104 3759 2880 3759 157 3759 156 660 104 660 157 660 1136 3760 3083 3760 4278 3760 3483 3761 1136 3761 4278 3761 2881 3709 104 3709 156 3709 3609 3762 2881 3762 156 3762 2882 3763 1136 3763 3483 3763 3482 660 2882 660 3483 660 4220 660 2881 660 3609 660 2884 660 4220 660 3609 660 1131 3764 2882 3764 3482 3764 3404 3765 1131 3765 3482 3765 127 3716 4220 3716 2884 3716 3497 660 127 660 2884 660 1132 3766 1131 3766 3404 3766 3405 660 1132 660 3404 660 123 660 127 660 3497 660 4286 3767 123 3767 3497 3767 2887 3768 1132 3768 3405 3768 2925 660 2887 660 3405 660 2886 3769 123 3769 4286 3769 4310 3770 2886 3770 4286 3770 1123 3771 2887 3771 2925 3771 4153 660 1123 660 2925 660 2888 3772 2886 3772 4310 3772 170 3724 2888 3724 4310 3724 3503 3773 1123 3773 4153 3773 3130 660 3503 660 4153 660 2889 3726 2888 3726 170 3726 2893 660 2889 660 170 660 2895 3774 3503 3774 3130 3774 3129 660 2895 660 3130 660 3044 3775 2889 3775 2893 3775 3020 3776 3044 3776 2893 3776 2894 3777 2895 3777 3129 3777 3787 3778 2894 3778 3129 3778 3043 3730 3044 3730 3020 3730 4270 660 3043 660 3020 660 4282 3779 2894 3779 3787 3779 3788 660 4282 660 3787 660 122 660 3043 660 4270 660 135 3732 122 3732 4270 3732 3595 660 4282 660 3788 660 3595 3780 2899 3780 4282 3780 118 3734 122 3734 135 3734 2902 3781 118 3781 135 3781 3573 3782 3161 3782 2899 3782 3573 3783 2899 3783 3595 3783 116 660 118 660 2902 660 2901 3784 116 3784 2902 3784 108 3785 3161 3785 3573 3785 108 660 2905 660 3161 660 105 660 116 660 2901 660 2904 3740 105 3740 2901 3740 110 3786 2905 3786 108 3786 110 660 168 660 2905 660 106 3787 105 3787 2904 3787 3338 3788 106 3788 2904 3788 2906 3789 168 3789 110 3789 2906 660 163 660 168 660 3218 3790 106 3790 3338 3790 3339 3791 3218 3791 3338 3791 2909 660 3921 660 163 660 2909 3792 163 3792 2906 3792 2910 660 3218 660 3339 660 143 660 2910 660 3339 660 2908 660 3920 660 3921 660 2908 3793 3921 3793 2909 3793 2911 660 2910 660 143 660 2912 3794 2911 3794 143 3794 113 660 2913 660 3920 660 113 3795 3920 3795 2908 3795 125 660 2911 660 2912 660 3574 3796 125 3796 2912 3796 114 660 3511 660 2913 660 114 3797 2913 3797 113 3797 2914 660 3574 660 151 660 2914 660 125 660 3574 660 2916 3798 153 3798 3511 3798 2916 3799 3511 3799 114 3799 121 3800 153 3800 2916 3800 121 660 151 660 153 660 121 3757 2914 3757 151 3757 3908 3548 2024 2811 3002 3549 2917 3801 2918 3802 1927 2774 2383 3297 2921 3272 4173 210 1982 2792 2917 3801 1927 2774 2923 3306 2383 3297 4173 210 2919 3803 2036 2855 3908 3548 2927 3804 2921 3804 1417 3804 2883 3805 2917 3801 1982 2792 2927 2309 4173 210 2921 3272 2883 3805 1982 2792 1993 1923 1417 2191 2921 3272 2920 3225 2922 3806 2046 2880 2919 3803 4256 3675 2402 3310 2923 3306 2885 3807 2883 3805 1993 1923 4256 3675 2923 3306 4173 210 2885 3807 1993 1923 2926 1946 2924 2166 1417 2191 2920 3225 2720 3808 2104 2908 2922 3806 2317 3144 2924 2166 2920 3225 2933 3316 2402 3310 4256 3675 4166 3809 2885 3807 2926 1946 4166 3809 2926 1946 2930 1999 4181 208 4173 210 2927 2309 4181 208 2927 2309 2928 2365 3953 3810 2971 2954 2720 3808 2938 2456 4181 208 2928 2365 2932 2144 2924 2166 2317 3144 2935 3811 2930 1999 2929 2035 2935 3811 4166 3809 2930 1999 2931 3045 2932 2144 2317 3144 2141 2977 3953 3810 4236 3812 2418 3326 2933 3316 4256 3675 2934 2075 2935 3811 2929 2035 2891 3813 2935 3811 2934 2075 3882 3814 2418 3326 4256 3675 4221 3815 2938 2456 1631 2488 4221 3815 1631 2488 2937 2539 4154 3816 2940 3000 4236 3812 2943 2109 2891 3813 2934 2075 4221 3815 4181 208 2938 2456 2939 3341 2418 3326 3882 3814 2941 2560 4221 3815 2937 2539 2180 3027 2940 3000 4154 3816 3516 3817 2180 3027 4154 3816 3881 3818 2939 3341 3882 3814 2890 3819 2891 3813 2943 2109 4311 3820 4221 3815 2941 2560 2942 2159 2890 3819 2943 2109 2944 2687 4311 3820 2941 2560 2945 3038 2180 3027 3516 3817 4038 3821 2466 3368 3881 3818 2947 3388 2466 3368 4038 3821 2181 3047 2945 3038 3516 3817 2948 3822 4311 3820 2944 2687 2949 1925 2948 3822 2944 2687 4116 3823 2947 3388 4038 3821 2955 2161 2946 2141 2167 3024 2516 3408 2947 3388 4116 3823 2952 3824 2948 3822 2949 1925 2950 1924 2952 3824 2949 1925 2951 3026 2955 2161 2167 3024 3763 212 2184 3033 2181 3047 3763 212 2181 3047 3516 3817 3955 3825 2516 3408 4116 3823 2546 3437 2516 3408 3955 3825 3199 3826 2952 3824 2950 1924 2954 2041 3199 3826 2950 1924 2621 3470 3955 3825 2828 3827 2621 3470 2546 3437 3955 3825 2953 3828 3199 3826 2954 2041 1318 2152 2951 3026 2958 3032 1318 2152 2955 2161 2951 3026 1270 2064 2953 3828 2954 2041 2956 3829 2942 2159 1320 2172 2956 3829 1320 2172 2960 2187 2956 3829 2890 3819 2942 2159 2957 3493 2828 3827 3856 3830 2957 3493 2621 3470 2828 3827 2959 3831 2953 3828 1270 2064 2962 2128 2959 3831 1270 2064 1322 2177 1318 2152 2958 3032 2623 3519 3856 3830 3859 3832 2623 3519 2957 3493 3856 3830 1322 2177 2958 3032 3763 212 2842 3833 2959 3831 2962 2128 2961 211 2956 3829 2960 2187 2961 211 1322 2177 3763 212 2961 211 2960 2187 1321 2186 2961 211 1321 2186 2963 2188 1404 2286 2842 3833 2962 2128 2961 211 2963 2188 1322 2177 1287 2145 2932 2144 2931 3045 2688 2846 2623 3519 3859 3832 2985 2766 1176 1984 3476 3834 1176 1984 1251 1983 2964 3835 1251 1983 1268 2032 2987 3836 1268 2032 2965 2110 2988 3837 2965 2110 1304 2156 2966 3838 1349 2200 1430 2270 3889 3839 1430 2270 1461 2335 4214 3840 2778 3841 2688 2846 3859 3832 1461 2335 1503 2422 4264 3842 1610 2502 2993 2523 3893 3843 2993 2523 1702 2593 2969 3844 1702 2593 2994 2631 2866 3845 2994 2631 1777 2632 2970 3846 1862 2708 1861 2724 3731 3847 4060 2657 2167 3024 2946 2141 2940 3000 2141 2977 4236 3812 4073 3848 1404 2286 1507 2299 2141 2977 2971 2954 3953 3810 2971 2954 2104 2908 2720 3808 4073 3848 2842 3833 1404 2286 2104 2908 2046 2880 2922 3806 2046 2880 2036 2855 2919 3803 2036 2855 2024 2811 3908 3548 2024 2811 2703 2810 3002 3549 2703 2810 2687 3531 4208 3633 2687 3531 2645 3516 2771 3628 2645 3516 2646 3506 2972 3629 2646 3506 2626 3502 2761 3849 2626 3502 2998 3481 2760 3850 2998 3481 2973 3479 3311 3851 2973 3479 2995 3466 3698 3600 2981 3852 2980 2831 2778 3841 2995 3466 2577 3457 3263 3601 2577 3457 2557 3439 3264 3853 2557 3439 2991 3432 3473 3854 2991 3432 2522 3422 3472 3855 2522 3422 2510 3409 4053 3856 2510 3409 2975 3385 2727 3857 2975 3385 2504 3389 4285 3858 2487 3364 2469 3361 3210 3859 2469 3361 2976 3346 2817 3860 2976 3346 2421 3337 3975 3861 2393 3308 2392 3862 4168 3677 4091 3863 1507 2299 2977 2479 2392 3862 2978 3277 2806 3667 4091 3863 4073 3848 1507 2299 2978 3277 2323 3134 2984 3662 2323 3134 2979 3030 2792 3658 2979 3030 2171 2992 2983 3653 2171 2992 2096 2984 2785 3654 2096 2984 2095 2927 2781 3864 2095 2927 2980 2831 2981 3852 2980 2831 2688 2846 2778 3841 2466 3368 2939 3341 3881 3818 2781 3864 2095 2927 2981 3852 3886 3865 2977 2479 1622 2605 3886 3865 4091 3863 2977 2479 2785 3654 2096 2984 2781 3864 3885 3866 1622 2605 1829 2680 3885 3866 3886 3865 1622 2605 2983 3653 2171 2992 2785 3654 3851 3867 1829 2680 1882 2701 3851 3867 3885 3866 1829 2680 2792 3658 2979 3030 2983 3653 1881 2740 3851 3867 1882 2701 3852 3868 3851 3867 1881 2740 2984 3662 2323 3134 2792 3658 2985 2766 3852 3868 1881 2740 2806 3667 2978 3277 2984 3662 3476 3834 3852 3868 2985 2766 4168 3677 2392 3862 2806 3667 2964 3835 3476 3834 1176 1984 4180 3869 2393 3308 4168 3677 2409 3305 2393 3308 4180 3869 2987 3836 2964 3835 1251 1983 2421 3337 2409 3305 4180 3869 3975 3861 2421 3337 4180 3869 2988 3837 2987 3836 1268 2032 2817 3860 2976 3346 3975 3861 2966 3838 2988 3837 2965 2110 3210 3859 2469 3361 2817 3860 3889 3839 1304 2156 1349 2200 3889 3839 2966 3838 1304 2156 2487 3364 3210 3859 3754 3870 2504 3389 2487 3364 3754 3870 4285 3858 2504 3389 3754 3870 4214 3840 3889 3839 1430 2270 2727 3857 2975 3385 4285 3858 4264 3842 4214 3840 1461 2335 4053 3856 2510 3409 2727 3857 4197 3871 1503 2422 1609 2387 4197 3871 4264 3842 1503 2422 3472 3855 2522 3422 4053 3856 1610 2502 4197 3871 1609 2387 3893 3843 4197 3871 1610 2502 3473 3854 2991 3432 3472 3855 2969 3844 3893 3843 2993 2523 3264 3853 2557 3439 3473 3854 2866 3845 2969 3844 1702 2593 3263 3601 2577 3457 3264 3853 2970 3846 2866 3845 2994 2631 3698 3600 2995 3466 3263 3601 2996 3872 1777 2632 1779 2663 2996 3872 2970 3846 1777 2632 3311 3851 2973 3479 3698 3600 2997 3873 1779 2663 2999 2676 2997 3873 2996 3872 1779 2663 2760 3850 2998 3481 3311 3851 1836 2691 2997 3873 2999 2676 2761 3849 2626 3502 2760 3850 2873 3874 2997 3873 1836 2691 1862 2708 2873 3874 1836 2691 2972 3629 2646 3506 2761 3849 3731 3847 2873 3874 1862 2708 2771 3628 2645 3516 2972 3629 3753 3875 3731 3847 1861 2724 1884 2737 3753 3875 1861 2724 4208 3633 2687 3531 2771 3628 2879 3876 3753 3875 1884 2737 1905 2757 2879 3876 1884 2737 3002 3549 2703 2810 4208 3633 2918 3802 2879 3876 1905 2757 1927 2774 2918 3802 1905 2757 682 1208 12 1210 683 949 4148 1226 686 945 689 1228 696 55 605 1161 607 943 3171 29 16 38 4305 1213 684 1220 3171 29 4305 1213 671 948 684 1220 4305 1213 3770 44 3771 1223 688 946 687 1221 4306 1217 3770 44 4147 1229 22 50 691 944 22 50 692 1235 691 944 689 1228 4324 47 4148 1226 4148 1226 695 1233 686 945 607 943 692 1235 696 55 605 1161 4106 1164 607 943

 0.02213078 0.06989294 0.08603501 0.03009289 0.06527817 0.08665996 0 0.07251787 0.08731299 0.007517993 0.07303166 0.08649098 0.003539264 0.07332795 0.08653903 0.003676772 0.07226562 0.08742302 0.005582809 0.06992805 0.08820098 -0.002432823 0.07022231 0.08819603 0.007422268 0.07194006 0.08740103 0.01276546 0.07315915 0.08502697 0.01973354 0.06767952 0.08776998 0.02154779 0.06866341 0.08714997 0.01242935 0.07235515 0.08636099 0.00737369 0.07093524 0.087893 0.004998803 0.06897372 0.08818203 0.02410018 0.06608742 0.08762103 0.01218569 0.07127702 0.08730298 0.01680696 0.07007145 0.08730298 0.0172562 0.07130885 0.08623498 0.01186776 0.07002186 0.08792799 0.02636218 0.06845092 0.085711 0.02607065 0.06702786 0.08692198 0.01627647 0.06757944 0.08802199 -0.05071896 0.05437344 0.07999402 -0.06517648 -0.02745938 0.07233399 -0.06763005 -0.02382957 0.07132899 -0.05041843 0.053869 0.08193701 -0.06695109 0.007326066 0.081137 -0.05449676 -0.03195405 0.07788801 -0.07070118 0.0167979 0.07692801 -0.07048535 0.01449865 0.07775998 -0.06952559 0.01908951 0.078372 -0.04770708 -0.03096801 0.079409 -0.05205935 -0.02934861 0.07921701 -0.05002528 -0.05153548 0.06582099 -0.04721218 -0.05488878 0.06330996 -0.05185633 0.04823333 0.08479398 -0.001060009 -0.07226002 0.06003981 -0.002524435 -0.07244712 0.05854499 -0.00124228 -0.07152152 0.06275695 1.62705e-5 -0.07045191 0.06526994 -0.0292328 -0.05034661 0.07721501 -0.06960767 0.02345502 0.07649499 -0.002852976 -0.07046771 0.06514596 -0.001277685 -0.06918889 0.06756198 -0.05970329 -0.0277884 0.07710999 -0.006448507 -0.07110804 0.06311696 -0.07054787 0.01954025 0.07625198 -0.01208102 -0.07153373 0.05854797 -0.00651735 -0.07223999 0.05838495 -0.05341929 0.04292953 0.08529698 -0.02046597 -0.04902791 0.07785695 -0.04631727 -0.05374628 0.06735599 -0.05157059 -0.05148619 0.06168997 -0.04692006 0.05769824 0.08083403 -0.006381869 -0.07024604 0.06517797 -0.06365591 0.005228519 0.082255 -0.04556918 -0.05303651 0.069202 -0.001637995 -0.06747591 0.069884 -0.006245136 -0.06907862 0.06732296 -0.002552926 -0.06347268 0.07351195 -0.06791079 0.02955394 0.075235 -0.006725728 -0.07182222 0.06061196 -0.03974747 -0.05557548 0.07089096 -0.06920731 -0.0191549 0.07195699 -0.05800396 -0.02490729 0.078803 1.38643e-5 -0.06089979 0.07510995 -0.05206871 0.04202282 0.08569401 -0.0686348 0.0161876 0.07997399 -0.01196044 -0.07048463 0.06304895 -0.06670296 -0.02358531 0.07300597 -0.06473898 0.01299172 0.08244198 -0.01212167 -0.07115441 0.06064099 -0.04990625 0.05315881 0.08341699 -0.04632014 0.05752265 0.08257299 -0.07077676 -0.0190072 0.067523 1.32209e-5 -0.058218 0.07629597 -0.04279112 -0.05033159 0.073731 -0.01611268 -0.06982862 0.06273496 -0.01144134 -0.06847292 0.06734997 -0.0117622 -0.069588 0.06525099 -0.01642566 -0.07075178 0.05766385 -0.06991505 -0.01920819 0.07036799 -0.05436527 -0.04869014 0.06124198 -0.05754894 -0.04497021 0.06184583 -0.05100548 -0.05155563 0.06358498 -0.01086819 -0.06614774 0.07056897 -0.01636219 -0.0705986 0.05911588 -0.06647509 0.01537734 0.08162897 -0.06822466 0.02281737 0.07937598 0 -0.05965387 0.075715 -0.05420327 -0.04838109 0.06375497 -0.02141487 -0.06942743 0.05788195 -0.02866762 -0.04769003 0.07786995 -0.005839407 -0.06640791 0.070858 -0.00501883 -0.0570541 0.07667797 -0.06689947 -0.0186786 0.07544201 -0.04470646 -0.05211728 0.07103598 -0.05407047 -0.02512627 0.07971101 -0.06760358 0.02840292 0.077883 -0.0661621 0.02550143 0.08111101 -0.06164103 0.004188895 0.082484 -0.02030086 -0.06873828 0.06298196 -0.07058846 -0.01382428 0.07267999 -0.05702757 -0.04549032 0.062756 -0.04939407 -0.05102276 0.067716 -0.01582247 -0.06876629 0.06537795 -0.064538 -0.02313059 0.07570099 -0.03392302 -0.04616588 0.077838 -0.06571507 0.03327953 0.07797598 -0.06224864 -0.02204477 0.07773196 -0.06252956 0.01437419 0.08317202 -0.05241554 -0.02236717 0.08019 -0.009475708 -0.0588901 0.075854 -0.07353645 -0.00404191 0.06869399 -0.06828808 -0.0190199 0.07356297 -0.04136389 -0.04876869 0.07528698 -0.06540578 0.02118057 0.08214402 -0.04864943 -0.0503928 0.069458 -0.05764853 -0.02283138 0.07941699 -0.06679308 0.02773743 0.079629 -0.02050077 -0.06960463 0.05899482 -0.00473392 -0.06145143 0.07478398 -4.04805e-5 -0.05534815 0.07713896 -0.01550549 -0.06764781 0.06749397 -0.05038255 0.046871 0.085707 -0.07182109 -0.01362818 0.06963801 -0.07296037 -0.008828222 0.068645 -0.0712248 -0.01384091 0.07125198 -0.04861468 0.05197316 0.08511799 -0.06961405 -0.0137313 0.07437098 -0.04043346 0.06152236 0.08359801 -0.06349098 -0.01773709 0.078408 -0.0604614 -0.04096591 0.06338298 -0.06360048 0.01840144 0.083045 -0.02582317 -0.06784111 0.05919599 -0.05241245 -0.04833734 0.06767201 -0.06247335 0.02395445 0.08349198 -0.06006646 -0.02137058 0.07899302 -0.04517054 0.05690455 0.084607 -0.04746395 0.04722243 0.08623301 -0.0195719 -0.06682473 0.06730496 -0.04025065 0.05596816 0.08686399 -0.06078416 0.02268767 0.08394402 -0.02552855 -0.06758493 0.06114298 -0.04751247 0.05049479 0.08597803 -0.0655018 -0.01818138 0.07690197 -0.07301187 -0.007350683 0.07011097 -0.06769728 -0.01289981 0.07690399 -0.0508219 -0.04764908 0.07057398 -0.0546506 -0.04416018 0.07020801 -0.07147467 -0.009029984 0.073327 -0.0447098 0.05423444 0.08600199 -0.0150485 -0.0652011 0.07079195 -0.06007558 -0.04093188 0.06523096 -0.07365328 6.88632e-4 0.07001501 -0.05626291 -0.04513329 0.06635499 -0.06583255 -0.01330721 0.07823598 -0.04778206 -0.04734575 0.07320994 -0.04650199 0.0494939 0.086322 -0.04003429 -0.04455 0.07745397 -0.07069009 -0.009038031 0.07468396 -0.02526342 -0.06689602 0.06368499 -0.06065905 0.01634001 0.08355897 -0.03588145 -0.04220813 0.078426 -0.05759656 -0.01856517 0.08025801 -0.01174795 -0.0625118 0.07364398 -0.07207626 -0.006552934 0.07304596 -0.04640793 -0.0453068 0.07528197 -0.01931816 -0.06547999 0.06937795 -0.07331818 -0.001402378 0.07124596 -0.06489038 -0.01316356 0.07886999 -0.06316739 0.03777229 0.07901597 -0.03971326 -0.04675436 0.07673901 -0.04590499 -0.04224991 0.07690197 -0.0548923 -0.02179479 0.08013397 -0.06086838 -0.0404883 0.06257599 -0.06430625 0.03243809 0.08102601 -0.07218605 -0.002071619 0.07428598 -0.06297159 -0.01280874 0.07987499 -0.06189894 -0.01735788 0.07928699 -0.0185256 -0.0626313 0.072501 -0.06207036 0.03656816 0.08173501 -0.0539093 -0.04295033 0.07222998 -0.03720808 -0.04390722 0.07803899 -0.03121376 -0.06571352 0.05806297 -0.0588063 -0.040313 0.069126 -0.02491295 -0.06610798 0.06573098 -0.07345747 0.006601214 0.07109701 -0.05139815 -0.04198098 0.07471698 -0.03313887 -0.04274588 0.07840698 -0.03182876 -0.06529289 0.05993795 -0.06601476 -0.03120929 0.06602597 -0.06322938 -0.03607016 0.06582099 -0.05789256 -0.03982698 0.070948 -0.07144016 -0.002152502 0.075576 -0.03018689 -0.06503188 0.06362998 -0.04700249 -0.0386424 0.07789295 -0.03583246 -0.06336063 0.05836486 -0.07289606 5.85663e-4 0.07319301 -0.06271499 -0.0358392 0.06782197 -0.05981534 0.03544962 0.08372497 -0.06941628 -0.006345093 0.07725 -0.04218226 -0.04054015 0.07823395 -0.01085674 -0.05282813 0.07749199 -0.05948507 -0.04066258 0.06736099 -0.05657345 -0.03907233 0.07298201 -0.06020915 0.04178196 0.08055901 -0.02376669 -0.06382054 0.06980496 -0.06320315 0.0289241 0.08283901 -0.06357407 -0.03605008 0.06423801 -0.03986942 -0.04059058 0.078462 -0.05725449 0.04686182 0.07987499 -0.05969119 0.04112595 0.081851 -0.0666266 -0.00672847 0.07939201 -0.05479836 -0.03799933 0.07505899 -0.01572996 -0.05793428 0.07584398 -0.05944412 0.03001046 0.08437699 -0.06051737 0.04206997 0.07964098 -0.05781579 0.03049767 0.084706 -0.02929013 -0.06317669 0.06793999 -0.05782127 -0.01417851 0.08083999 -0.04006189 -0.06077343 0.05928695 -0.02447706 -0.06509548 0.06773597 -0.06095886 -0.03516358 0.07156395 -0.07266789 0.006218135 0.07425296 -0.04973906 -0.0407449 0.07617801 -0.01254338 -0.0553267 0.07692998 -0.05307745 -0.03698933 0.076469 -0.06096124 -0.0147798 0.08016598 -0.0195409 -0.05184239 0.07748299 -0.01818525 -0.06014883 0.07432699 -0.06058335 0.03098434 0.08394402 -0.0717796 0.006350457 0.07606297 -0.0588063 0.02214431 0.08408498 -0.06297749 -0.006048381 0.081092 -0.06501597 -0.005859911 0.08038896 -0.07297438 0.01108109 0.071729 -0.0722717 4.62848e-4 0.07463699 -0.04569613 -0.0376088 0.078435 -0.01766008 -0.05849277 0.07534295 -0.04975038 -0.03765189 0.07746297 -0.02296596 -0.06127929 0.07260698 -0.05702829 0.04600763 0.08142501 -0.02108556 -0.05670946 0.07579696 -0.0612151 -0.003543913 0.081748 -0.0724951 0.01143741 0.07398098 -0.06050378 -0.01239871 0.08070296 -0.0651412 -0.03118801 0.068771 -0.05687767 0.03634119 0.08485102 -0.0573017 -0.01169091 0.081124 -0.05576318 0.04460865 0.08371502 -0.03546679 -0.06101638 0.06681197 -0.06665557 -0.02783846 0.069332 -0.05390608 0.05048149 0.08101201 -0.02772372 -0.06068861 0.07164996 -0.06191337 -0.03555917 0.06975597 -0.05682855 0.03912734 0.08458203 -0.03886795 -0.06123012 0.06142497 -0.07213538 0.01519525 0.07342499 -0.02687674 -0.05348408 0.07645297 -0.04992789 -0.03493398 0.07831096 -0.07196557 0.01082032 0.07551199 -0.07101857 0.01093953 0.07728797 -0.0634185 -0.03106272 0.07187896 -0.06766265 -0.02794069 0.065517 -0.03462857 -0.06290048 0.06377995 -0.06293427 -0.0297079 0.073592 -0.04807066 -0.0336827 0.07894897 -0.06924015 0.01050156 0.07944399 -0.05366498 0.05019772 0.08182197 -0.05657804 -0.03300976 0.07652497 -0.05164778 0.03965979 0.085617 -0.0702846 0.004594862 0.07815599 -0.05419278 0.05083066 0.07943302 -0.0433138 -0.05843943 0.06074297 -0.03996402 -0.05946069 0.06479996 -0.07002955 0.02391964 0.074189 -0.05899667 -0.03422421 0.07431095 -0.07157486 0.01490104 0.07554996 -0.04189908 -0.05717581 0.066899 -0.06853687 -0.02395004 0.06926101 -0.0582056 0.04025763 0.083566 -0.04373615 -0.0337094 0.079198 -0.04287844 -0.05804657 0.06352496 -0.05305075 -0.03108757 0.07860398 -0.0613749 -0.02871692 0.07555699 -0.05316126 0.04964596 0.08303201 -0.05494529 0.03817743 0.08526098 -0.01894205 -0.05374217 0.07705497 -0.05101919 -0.05211418 0.06068783 -0.07114487 0.01991373 0.07375997 -0.04125052 -0.05633068 0.06882297 -0.04672217 -0.05440759 0.06553095 -0.04777199 -0.05491161 0.06127887 0.03062236 0.05826634 0.08419996 0.02946305 0.05734813 0.08432197 -0.0723015 0.01534402 0.07209897 -0.0712189 0.02000522 0.07293999 -0.06974059 0.02486693 0.07382398 -0.07009726 0.02508175 0.068672 -0.05206096 -0.05322086 0.04136484 -0.06593507 0.03386533 0.07599103 -0.003147304 -0.0734483 0.04584884 -0.06332725 0.03858685 0.07714599 -0.06084322 0.04251426 0.07772898 -0.05762815 0.04689693 0.078251 -0.0506708 0.05459284 0.07872599 0 -0.0744546 0.03390598 -0.006611943 -0.07416021 0.03400695 -0.04387778 -0.05813127 0.05971288 -0.05172801 -0.05148512 0.06024897 -0.05864179 -0.0458635 0.04392993 -0.04435276 -0.05783063 0.05922091 -0.0481379 -0.05478394 0.05973798 1.69299e-5 -0.07260912 0.05735486 -0.01167672 -0.07352441 0.03428596 -0.06196802 -0.04125636 0.04553699 -0.06481879 -0.03661781 0.04715389 -0.0168246 -0.07251971 0.03463596 -0.01665526 -0.07071942 0.05742084 -0.02682346 -0.0694499 0.03570598 -0.02709889 -0.06934732 0.03569185 -0.05513477 -0.04790467 0.06080389 -0.05938369 -0.04490149 0.04423981 -0.05833077 -0.0440678 0.06130486 -0.06503939 -0.03622949 0.04727095 -0.06749057 -0.03143161 0.04894793 -0.03142035 -0.06749433 0.03638792 -0.01160627 -0.07170033 0.05732583 -0.06129348 -0.03996932 0.06189799 -0.06603288 -0.03168839 0.06402498 -0.07063299 -0.02352851 0.05171895 -0.06777709 -0.02791398 0.06464999 -0.03625398 -0.065023 0.03724998 -0.0719555 -0.01909703 0.05326396 -0.0721628 -0.0137394 0.06747698 -0.06855779 -0.02631509 0.06351697 -0.07006275 -0.02199018 0.06504601 -0.06929939 -0.0237261 0.066459 -0.07391297 -0.008901178 0.05681997 -0.04095178 -0.06216979 0.03824496 -0.07161086 -0.01689136 0.065014 -0.0734356 -0.006374478 0.06703299 -0.07425701 -0.006207108 0.05753588 -0.07271397 -0.01166331 0.06600701 -0.07444757 6.31105e-4 0.06014388 -0.0744422 -0.001097083 0.05953997 -0.04482764 -0.05943667 0.03919684 -0.02162307 -0.06939059 0.05758082 -0.0737313 0.004276812 0.06897199 -0.02648067 -0.06771993 0.05781096 -0.02620315 -0.06779551 0.05814898 -0.07420426 0.006023168 0.06202501 -0.07378065 -0.001069188 0.06795299 -0.07381439 0.009682059 0.06330096 -0.07328301 0.009626984 0.07008999 -0.07295125 0.0150358 0.06492298 -0.0647574 0.002714753 0.07792997 -0.06914758 0.002870917 0.07789999 -0.06438177 0.003653705 0.07856398 -0.06298696 0.002620518 0.07843899 -0.06187158 1.09259e-4 0.07866501 -0.07290506 0.01837486 0.06078886 -0.009887635 0.07431995 0.08428901 -0.0484063 -0.05722743 0.03897899 -0.00978136 0.07454645 0.08027499 -0.004980266 0.07502603 0.08037203 -0.05236959 -0.05394721 0.03578299 -0.07269436 0.01832586 0.06491696 -0.0490545 -0.0566883 0.03894984 -0.07129549 0.02318555 0.06660699 -0.05594569 -0.04988241 0.04152095 -0.06955647 0.02797883 0.06827402 -0.0680086 0.03152877 0.069727 -0.06768715 0.03274005 0.06575602 -0.06749027 0.03264874 0.069898 -0.06518357 0.03743344 0.06722402 -0.06507879 0.03721874 0.071487 -0.0658701 0.03624403 0.06705898 -0.0625357 -0.04173147 0.04000699 -0.06324136 0.04065275 0.068587 -0.06232357 0.0416646 0.07303297 -0.05949825 0.04596459 0.07032799 -0.05932533 0.04583209 0.07448202 -0.05620515 0.04961407 0.07579702 -0.05686736 0.04917806 0.07154399 -0.06762075 -0.03234177 0.04759401 -0.06971949 -0.02752882 0.04925996 -0.04944956 0.05664235 0.07401996 -0.002745151 -0.07513421 0.02845698 -0.005547523 -0.07497882 0.02851086 -0.005468368 -0.07475 0.03301388 -0.04964172 0.05619007 0.07798498 -0.05286335 0.0531637 0.07703202 -0.04782325 0.05772477 0.07881402 -0.07159096 -0.02296322 0.04653799 -0.01052814 -0.07444041 0.02869784 -0.0713759 -0.02289122 0.05086588 -0.07153189 -0.0224272 0.05086398 -0.04622805 0.05915004 0.07706201 -0.0716598 -0.02259123 0.04645097 -0.01328843 -0.0738393 0.03100997 -0.04441154 0.06066155 0.07552498 -0.07286369 -0.01766347 0.05230587 -0.07295769 -0.01722133 0.05267399 -0.04168725 0.06229752 0.08038997 -0.01605165 -0.07344603 0.02904194 -0.07422369 -0.0119735 0.05029588 -0.03755825 0.06513494 0.07695502 -0.07397198 -0.01211798 0.05452299 -0.03581625 0.06610125 0.07741099 -0.02116245 -0.07190728 0.03378695 -0.02098846 -0.07219213 0.029477 -0.02602922 -0.07053428 0.03005182 -0.02607446 -0.07027482 0.03441095 -0.02624917 -0.0703895 0.02988398 -0.03744405 0.06494116 0.08124703 -0.07490336 -0.002878189 0.05779999 -0.07466715 -0.006667673 0.05634397 -0.07494771 -0.001674592 0.05807983 -0.03243952 0.06782585 0.078009 -0.07492518 0.002275288 0.05958497 -0.07471621 0.008396923 0.05733901 -0.02883297 0.06944406 0.078444 -0.07511299 0.00336194 0.05559897 -0.07515078 0.002276957 0.05528098 -0.04046911 -0.06337052 0.03241086 -0.07489126 0.003356039 0.05982995 -0.04023247 -0.06323599 0.03690081 -0.0742821 0.01159322 0.05851083 -0.04502975 -0.06015509 0.03346484 -0.07449507 0.008376002 0.06157588 -0.02014559 0.07243341 0.079607 -0.01951086 0.07239186 0.08361899 -0.04433912 -0.06071341 0.03345698 -0.01956743 0.07259821 0.07953602 -0.04488736 -0.06003803 0.03778499 -0.0737577 0.01338297 0.063317 -0.04888439 -0.05714386 0.034621 -0.01493906 0.07330071 0.084535 0.03224986 -0.05413073 0.07069998 0.02845919 -0.05566728 0.07095396 0.03142035 -0.05184412 0.071689 0.03253793 -0.05179148 0.07151401 0.0340175 -0.05212569 0.07172298 0.02985465 -0.05247581 0.07177901 0.0349791 -0.05312693 0.071307 0.03106427 -0.05459052 0.07069998 0.02875655 -0.05371707 0.07167196 0.03058975 -0.05297261 0.07083696 0.03373527 -0.03575831 0.07871699 0.04048085 -0.02885437 0.07932198 0.02023196 -0.008473038 0.08117598 0.02905565 0.05785316 0.079405 0.03255742 -0.05188632 0.067124 0.02839636 -0.05566871 0.06717699 0.03754127 -0.06281048 0.03599298 0.03332448 -0.06512999 0.03519499 0.07311201 0.009074628 0.06071084 0.03390878 -0.06486248 0.03347498 0.07233315 0.01448315 0.05923396 0.07315725 0.009229421 0.05741184 0.02899229 -0.06715768 0.03449785 0.07359278 0.004166305 0.05565685 0.07360678 -3.97282e-4 0.05739897 0.0248515 -0.0687893 0.03381699 0.02936094 -0.06703209 0.03272289 0.02409166 -0.06912028 0.03029698 0.07333886 -0.005815029 0.05552101 0.07364708 -0.00131464 0.05378186 0.07278275 -0.0109297 0.05042284 0.07335728 -0.006255984 0.05206996 0.0157094 -0.07140982 0.03290796 0.01994878 -0.07038474 0.03156083 0.01488739 -0.07164484 0.02942287 0.06889885 -0.02541249 0.04885184 0.07179266 -0.01601308 0.04866099 0.01031875 -0.07237708 0.03288394 0.0689547 -0.02543389 0.04542785 0.06857198 -0.02627182 0.04842984 0.06705939 -0.02999973 0.04384696 0.003917336 0.07401067 0.08303803 -5.58633e-5 0.07416933 0.079925 0.002954483 0.07411062 0.07990503 0.005954623 0.07387322 0.08298999 0.06675696 -0.03055948 0.04703295 0.01069635 0.07333052 0.08280199 0.006820917 0.07386612 0.07981997 0.06393951 -0.03596568 0.04519397 0.01542657 0.07247501 0.08250498 0.0131312 0.07298547 0.07951503 0.0182814 0.07180386 0.08227902 0.06481558 -0.03438562 0.04576689 0.0648685 -0.03441441 0.04231786 0.01829504 0.07185715 0.07912403 0.01543813 0.07252871 0.07935601 0.02055466 0.07118356 0.08205801 0.02335566 0.07036745 0.07860803 0.06399178 -0.0359959 0.04173398 0.03043025 0.06753104 0.080792 0.02556473 0.06953197 0.08148503 0.02828466 0.06852322 0.07796996 0.06238985 -0.03866767 0.04084485 0.03302288 0.06629115 0.08038902 0.03473198 0.06540441 0.08007103 0.0304532 0.06758093 0.07764101 0.03305989 0.06633472 0.07721197 0.06112653 -0.04050374 0.04362094 0.0383026 0.06338906 0.07778203 0.03475815 0.06545317 0.07690298 0.05962908 -0.04275357 0.03942883 0.06117647 -0.04053771 0.04015886 0.04241436 0.06075197 0.07527399 0.05828166 -0.04445558 0.04225099 0.04291802 0.06030339 0.07830297 0.0583294 -0.04449278 0.03878796 0.05655229 -0.04661339 0.04156196 0.04606086 0.0579909 0.07432198 0.04713022 0.05704694 0.077174 0.0565989 -0.04665261 0.03807896 0.05007988 0.05452996 0.07312303 0.05108857 0.05350083 0.07594501 0.05331099 0.05134457 0.07201999 0.05445915 0.05002987 0.07474201 0.05488145 -0.04855418 0.04083091 0.05326825 -0.05033308 0.03676295 0.05674469 0.04747831 0.07068103 0.05156779 -0.052028 0.03962683 0.05336076 -0.05029261 0.03681784 0.05759757 0.04633843 0.073462 0.05995857 0.04329609 0.06923198 0.04589104 -0.05704855 0.03797394 0.04765456 -0.05560052 0.03838795 0.06286859 0.03889662 0.06770902 0.06050938 0.04241687 0.07210302 0.04952424 -0.05394929 0.03925198 0.04958868 -0.05398041 0.03554087 0.0657041 0.03373652 0.06914699 0.06543958 0.03433811 0.06612998 0.04592907 -0.05709666 0.03446197 0.04347074 -0.05889809 0.03724497 0.06781721 0.02916932 0.06758302 0.04559993 -0.05729699 0.03434896 0.06787025 0.02918893 0.064332 0.04205775 -0.05997902 0.033463 0.06954461 0.02462553 0.06605798 0.06969618 0.02418965 0.06578499 0.06959891 0.02464395 0.06277197 0.0375728 -0.06286358 0.03246384 0.03904926 -0.0618931 0.03620696 0.07111245 0.01950055 0.06429499 0.02792978 -0.05827218 0.06619995 0.02534627 -0.0574882 0.06638801 -0.04466569 0.05663281 0.07424497 -0.07169145 -0.002009451 0.05560284 -0.03849697 0.06108963 0.078942 -0.07178258 0.00119549 0.05800396 -0.04041117 0.05982851 0.07840597 -0.07173967 0.001201391 0.05548083 -0.04271805 0.05813086 0.07497298 -0.07156068 0.005865454 0.05833399 -0.07106471 0.01062619 0.06121885 -0.036318 0.06235831 0.076276 -0.03846949 0.06104606 0.075984 -0.03580307 0.06272655 0.07948702 -0.06997185 0.01655477 0.063389 -0.03329128 0.06406015 0.07845199 -0.07088667 0.01152485 0.05906283 -0.06990337 0.01661133 0.06069284 -0.03010272 0.06567364 0.08054399 -0.03216236 0.06462085 0.07705301 -0.06693857 0.0264362 0.06682801 -0.03008568 0.06562536 0.077573 -0.06877499 0.02112251 0.064974 -0.06689417 0.02641892 0.06409597 -0.06494086 0.03110474 0.06845998 -0.02536463 0.06766241 0.08124101 -0.02749633 0.06675374 0.07763803 -0.06559449 0.02966964 0.06782197 -0.06302785 0.03515303 0.07015597 -0.02528166 0.06763762 0.07827103 -0.06420385 0.03252846 0.067577 -0.02094388 0.06916952 0.08178603 5.13618e-5 -0.07124072 0.03012686 -0.00976926 -0.07059133 0.03065186 -0.02301996 0.06843805 0.07818698 -0.01441687 -0.06981772 0.03318095 -0.01491367 -0.06968861 0.03090298 -0.02043837 -0.06831061 0.03369885 -0.02042585 -0.06827193 0.03136897 -0.02423107 -0.06704568 0.03297096 -0.02796947 -0.06561213 0.03465098 -0.03269428 -0.06340551 0.03542184 -0.06300699 0.03483682 0.06676602 -0.03681737 -0.06111919 0.03622198 -0.02012628 0.06935805 0.07871502 -0.03435188 -0.06248027 0.03333085 -0.04507899 -0.05536609 0.03823298 -0.05742377 0.04357177 0.07281899 -0.05107045 -0.04988712 0.03769195 -0.06017625 0.03954964 0.06865197 -0.05466336 -0.04602432 0.04143798 -0.05463469 -0.04598718 0.03892982 -0.0582512 -0.04140812 0.04069596 -0.05793309 -0.04188013 0.04287499 -0.0172466 0.07013267 0.07913702 -0.05450546 0.04723924 0.074126 -0.05763202 0.0432192 0.06962001 -0.06110888 -0.03709995 0.04207301 -0.06138116 -0.03671503 0.04475384 -0.06566327 -0.02854812 0.04763895 -0.06351888 -0.03287661 0.04357296 -0.01401543 0.07084912 0.07919502 -0.05051815 0.05150932 0.07559299 -0.0658276 -0.02802741 0.04533994 -0.0674771 -0.0240432 0.04921901 -0.06744158 -0.02387708 0.04643899 -0.05163645 0.05029726 0.07212901 -0.004911363 0.07206475 0.07954001 -0.06891047 -0.01959025 0.05060386 -0.05048227 0.05147296 0.07266199 -0.06761628 -0.02347648 0.04691886 -0.06908887 -0.01881623 0.048536 -0.0702188 -0.0144639 0.05258184 -0.04825294 0.05365717 0.07626599 -0.04672676 0.05493706 0.07386398 -0.04465985 0.05670624 0.07732397 -0.07024776 -0.01404666 0.05019098 -0.07101815 -0.009620368 0.05153 -0.0716499 -0.004076898 0.05618488 -0.07111686 -0.009255349 0.05435198 0.02219247 0.003681123 0.07269197 0.009350299 -0.0204643 0.07250398 0.01669085 -0.01552456 0.07249999 0.01473253 -0.01700073 0.07267796 0.01700085 -0.01473182 0.07270503 0.02046668 -0.009343147 0.07250899 0.02158772 -0.006338894 0.07250499 -0.02249485 0 0.07279497 -0.02226698 -0.003219068 0.07250601 -0.01892876 -0.01216089 0.07250696 -0.01648277 -0.01574552 0.07249999 -0.006340742 -0.02158439 0.07266998 -0.01133584 -0.02080959 0.07249897 -0.009344696 -0.0204637 0.07264298 -0.003203094 -0.02226781 0.07264196 8.14201e-4 -0.02273947 0.07249999 0.003206253 -0.02226591 0.07269996 -7.50312e-4 -0.0237466 0.07249897 0.02758717 0.05811131 0.079472 0.03019165 0.05104893 0.07997101 0.02655827 0.05534201 0.08006101 0.02733558 0.05366802 0.07989698 -0.03344696 0.05882674 0.08419996 -0.03433352 0.0582323 0.08419996 0.03447347 0.0580756 0.08419996 0.03179776 0.05835974 0.07969999 0.03140223 0.05777519 0.07969999 0.03337907 -0.05449718 0.06719994 0.03051465 -0.05591666 0.06719994 0.03055745 -0.0555948 0.07069998 0.06769359 2.23163e-4 0.07789999 0.06718575 0.001268029 0.07269996 0.06466621 0.001074612 0.07789999 0.0644024 6.17644e-4 0.07269996 -0.06450361 8.63903e-4 0.07269996 -0.06463688 0.00104624 0.07789999 -0.06555277 0.00166893 0.07789999 -0.06733375 0.001074612 0.07789999 -0.06764209 4.39959e-4 0.07789999 -0.03138583 -0.05436217 0.07069998 -0.02895057 0.05715751 0.07687199 -0.03196322 0.05328881 0.079405 -0.03300029 0.05303359 0.07259899 -0.03434318 0.05326014 0.07267898 0.03300064 0.05315589 0.07958602 0.03199267 0.05327934 0.079508 0.02945923 0.05920243 0.07474702 0.03827136 -0.05141448 0.07036399 0.0250324 -0.05349797 0.07140499 0.02526944 -0.0535736 0.06903994 0.03673398 -0.04993098 0.06994396 0.0346387 -0.0488094 0.07007598 0.03365588 -0.04891079 0.06659197 0.03174996 -0.0488168 0.06634497 0.02787655 -0.05009478 0.06749296 0.02627354 -0.0514425 0.06993299 -0.02498716 -0.05695331 0.06903994 -0.02485418 -0.05483812 0.07019197 -0.02665042 -0.05092102 0.07016497 -0.02545708 -0.0528022 0.07011598 -0.03006106 -0.04862385 0.071024 -0.02940219 -0.0485143 0.07407599 -3.06276e-5 -0.022435 0.07610398 -1.19548e-5 -0.02249592 0.07267999 -0.01216256 -0.01892459 0.07268297 -0.01473039 -0.01700222 0.072694 -0.01699978 -0.01473349 0.07269901 -0.02046293 -0.009345769 0.07269901 -0.02158838 -0.006338715 0.07250696 -0.02235299 -0.001628637 0.07751101 -0.02219218 0.003681838 0.07271099 -0.02095293 0.007915377 0.07830899 -0.01960057 0.01092314 0.07599902 0.04273957 0.05066037 0.05899 0.042638 0.05119901 0.07077598 -0.02956718 0.05024373 0.08281099 -0.02887767 0.05005556 0.08301299 -0.01868242 0.01085984 0.07955497 0.02981513 0.05032974 0.08256798 0.02947276 0.0502144 0.08286899 0.02997589 0.05058604 0.08239799 0.02919811 0.0501362 0.08297699 0.02822297 0.05175763 0.082376 0.04276865 0.05083346 0.05898702 0.04300504 0.05114442 0.05840599 0.04333955 0.05126023 0.05899 0.04292804 0.05167174 0.08256298 0.03717339 0.05104672 0.05836397 0.03850746 0.04897475 0.05898499 0.0388602 0.04886025 0.05899 0.03827685 0.04932004 0.05898702 0.03868687 0.04888874 0.05898702 0.03805088 0.04860454 0.05841702 0.04213964 0.04886025 0.05899 0.04227977 0.04887682 0.05898702 0.03771877 0.05125427 0.05898886 0.03788954 0.05121392 0.05898588 0.03809118 0.05162853 0.08009099 0.03831565 0.05158162 0.08107 0.03815335 0.05098801 0.05885189 0.03856074 0.05144101 0.08268499 0.03863966 0.05126422 0.082547 0.03826028 0.05066037 0.05899 0.04274457 0.04879665 0.05863487 0.01868909 0.01086151 0.079553 0.01894438 0.01092994 0.07946896 0.01944392 0.01109004 0.078974 0.01948326 0.01105326 0.07880097 0.01933515 0.01104885 0.079158 -0.01841968 0.01079416 0.07957798 -0.02464538 0.0307163 0.08075201 -0.02625221 0.05266356 0.08324098 -0.02595466 0.05508214 0.08287799 -0.02820128 0.05163854 0.08275496 -0.02559739 0.057415 0.083377 -0.02699679 0.05311936 0.08258199 -0.01698935 0.06657534 0.08445799 -0.02935212 0.05017906 0.08292698 -0.01893216 0.01092666 0.07947498 -0.01911926 0.01098001 0.07936698 -0.02451688 0.03066843 0.080935 -0.02999186 0.05057132 0.08233398 -0.02997696 0.05040764 0.08217102 0.0299865 0.05042082 0.08208101 0.02997696 0.05040764 0.08217102 0.01916962 0.01099532 0.07932901 0.02967816 0.05028027 0.082722 0.02903908 0.05009496 0.08300602 0.02887767 0.05005556 0.08301299 0.02586513 0.05905425 0.083184 0.02518326 0.05978059 0.08386403 0.02561098 0.05713051 0.08330798 0.0258767 0.05555891 0.082892 0.02542465 0.05391001 0.08335 0.02687406 0.05173033 0.08315998 0.02670806 0.05333054 0.08292996 0.02779597 0.06377524 0.08385503 -0.01952236 0.01113682 0.07864397 -0.03012079 0.05085766 0.08104699 -0.02000606 0.01255702 0.07294201 -0.02858334 0.05217057 0.079934 -0.02623015 0.05663347 0.08049201 -0.02646023 0.05829942 0.07982701 -0.0259633 0.05950063 0.08332598 -0.03133839 0.05095613 0.07969999 -0.02030915 0.01252985 0.07265198 0.03690224 0.05106455 0.05898499 0.03676337 0.05082833 0.05883389 0.03688359 0.04774993 0.05898499 0.03704583 0.04762095 0.05869787 0.02620989 0.05715751 0.08049201 0.02000606 0.01255702 0.07294201 0.02981156 0.04960757 0.07969999 0.03133839 0.05095613 0.07969999 0.02167654 0.01216346 0.07265198 0.03644496 0.05126565 0.07920598 0.03656893 0.05162167 0.07973599 0.0436604 0.05126023 0.05899 0.04426026 0.05066037 0.05899 0.0438894 0.04789412 0.05843389 0.03826028 0.04946017 0.05899 0.03871876 0.04964447 0.08263802 0.03887939 0.04941272 0.08240902 0.03836464 0.049129 0.05898588 0.03905946 0.04930764 0.08259999 0.04190415 0.04929792 0.08240801 0.04265958 0.04916894 0.05898588 0.04247075 0.04896461 0.05898588 0.04213517 0.04943156 0.08240902 0.04272806 0.04934293 0.05898797 -0.02751076 -0.05060505 0.06658899 -0.02837145 -0.04941731 0.06814897 0.01952236 0.01113682 0.07864397 0.01961255 0.01092463 0.07535898 0.01159077 -0.01920551 0.07634299 0.02065455 -0.008710265 0.07724201 0.007125735 -0.02025681 0.07686197 0.02224105 0.002644956 0.07836401 0.01966977 -0.009829878 0.07766401 0.009981095 -0.01961082 0.07682698 0.003541052 -0.022156 0.07606995 0.01518332 -0.01495891 0.07732498 0.01379168 -0.01768171 0.07670497 0.01994293 -0.0075652 0.07797199 0.02240037 -2.39494e-6 0.078143 0.006444275 -0.02148979 0.07617199 0.02079486 0.008300662 0.07884901 0.02220726 -0.002952218 0.07797402 0.008936703 -0.02057749 0.076222 0.01779967 -0.01363199 0.07697999 0.02075457 0.003673434 0.07895499 -0.0196259 0.01110953 0.07250297 -0.02128916 0.007265329 0.07279497 -0.01947426 0.01105862 0.07884901 -0.005111396 -0.0214219 0.076671 0.003236949 -0.02181679 0.07667297 -0.02054286 -0.008976161 0.07713598 -0.01917237 -0.01162678 0.07691299 -0.01969403 -0.01000112 0.07760101 -0.01636075 -0.01474428 0.07724702 -0.01764637 -0.01383978 0.07669401 -0.02175486 0.005346596 0.078094 -0.02046173 0.00910753 0.07893502 -0.01564079 -0.01607638 0.07651698 -0.02224344 0.002692997 0.07787197 -0.02157253 0.003957033 0.07888102 -0.01360428 -0.01783633 0.07634699 -0.0110991 -0.01949667 0.07622897 -0.02240145 -8.88909e-5 0.07821899 -0.0213145 -0.001519024 0.07850098 -0.01614725 -0.01411718 0.07739901 -0.01215034 -0.01840329 0.07691401 -0.02217328 -0.003250658 0.077618 -0.008665025 -0.02068871 0.07637399 -0.01995193 -0.007534623 0.07797402 -0.00587368 -0.02165478 0.07606101 -0.002971351 -0.02224481 0.07578802 -0.02154928 -0.006151676 0.07760798 4.5463e-4 -0.02620768 0.07247096 0.02836889 -0.04900246 0.06857097 0.02571058 0.05940192 0.08354997 0.02684605 0.06002724 0.08049201 -0.02731907 0.06370812 0.08412301 -0.02590095 0.06148654 0.08401399 -0.02020496 0.006810367 0.07922899 0.01841968 0.01079416 0.07957798 0 0.02373415 0.08070999 -4.766e-5 -0.02132546 0.07676798 -0.009631037 -0.01918601 0.07695597 0.01547986 0.06710255 0.08450502 -0.02771228 -0.04719418 0.06948095 -0.02701503 -0.04759669 0.06948095 -0.0301128 0.05052971 0.07969999 -0.02743875 0.05342721 0.08006602 -0.02671015 0.05497413 0.079903 -0.02687615 0.06002169 0.08035701 -0.02881789 0.06404483 0.08007401 -0.02187556 0.01209402 0.07273697 -0.03846555 0.05327755 0.08012402 -0.03588825 0.05116802 0.07987999 0.03698617 0.04777705 0.05851387 0.03733944 0.04753965 0.05899 0.03678166 0.047926 0.05876886 0.03612756 0.05086505 0.080307 0.03713357 0.05121862 0.05898588 0.03733944 0.05126023 0.05899 0.03234738 0.05033302 0.080271 0.02654099 0.05884283 0.07998198 0.03034526 0.05100762 0.07969999 0.02016395 0.01256126 0.07269102 0.03046298 0.05114686 0.07969999 0.03220582 0.05061882 0.07969999 0.03222417 0.0503636 0.07969999 0.03218632 0.04997414 0.07969999 0.03300017 0.05165952 0.07963603 0.03408336 0.05054903 0.07991498 0.03800165 0.05262959 0.08013498 0.0443052 0.04718685 0.08212602 0.04385626 0.04757142 0.05881202 0.04426026 0.04813957 0.05899 0.04173094 0.04926896 0.08240503 0.03926885 0.04926896 0.08240503 0.03866899 0.04986882 0.08240503 0.03914588 0.04963696 0.08297497 0.04208105 0.05185467 0.08319699 0.04233098 0.04986882 0.08240503 -8.41735e-4 -0.02415138 0.07581096 -9.79196e-4 -0.02449584 0.07605695 -0.001801431 -0.02507382 0.07643997 -0.01117545 -0.02214062 0.07658797 -0.01867258 -0.03391629 0.07463699 -0.02519565 -0.04683738 0.07451301 -0.01093167 -0.02263188 0.07665401 -0.02781099 -0.04953831 0.06876999 -0.02677685 -0.0477612 0.06969797 -0.02688866 -0.04767471 0.06951993 -0.01246857 -0.02302229 0.07262998 -0.02683776 -0.04770988 0.06956601 -0.01253229 -0.0229752 0.072546 -0.02694928 -0.04763609 0.06949096 -0.01134765 -0.02117168 0.07298702 -0.01131653 -0.02125233 0.075935 0.02701503 -0.04759669 0.06948095 0.02181816 0.01211804 0.07269102 0.02158474 0.01067203 0.07279896 0.03198605 0.04902482 0.07969999 0.03210717 0.04953014 0.07969999 0.02030915 0.01252985 0.07265198 0.02999716 0.05021822 0.07969999 0.02128905 0.007269561 0.07270699 0.02174443 0.005355775 0.07858997 0.02249485 0 0.07279497 0.02226775 -0.003201663 0.072667 0.02162384 -0.005878508 0.07764297 0.01946675 -0.01112419 0.07698798 0.01604425 -0.01567047 0.076622 0.01892578 -0.01216548 0.07250696 0.0121628 -0.01892513 0.07264 0.006336152 -0.02158552 0.07270097 0.00174278 -0.02237272 0.076065 -0.03219169 0.05068361 0.07969999 -0.02167654 0.01216346 0.07265198 -0.02156955 0.01063853 0.072483 -0.02181816 0.01211804 0.07269102 -0.02016395 0.01256126 0.07269102 -0.02995944 0.05010575 0.07969999 -0.03041839 0.05109781 0.07969999 -0.01268404 -0.02287948 0.07248395 -0.007880985 -0.02266788 0.07260501 -0.004505336 -0.02357268 0.07259601 -0.003869831 -0.0237388 0.07558995 -6.34026e-4 -0.02621692 0.07253301 -4.5463e-4 -0.02620768 0.07247096 -6.55048e-4 -0.04485625 0.069983 0.01258313 -0.02285581 0.07249999 0.0125578 -0.02295762 0.07252299 0.01244586 -0.02304399 0.07269996 0.02677685 -0.0477612 0.06969797 0.02679955 -0.04773926 0.06962698 0.02688866 -0.04767471 0.06951993 0.02677106 -0.04777365 0.06977397 0.01146733 -0.02147471 0.07572501 0.01251345 -0.0228812 0.07253098 0.02683776 -0.04770988 0.06956601 0.01257455 -0.02284342 0.072501 0.01246935 -0.02280491 0.07253599 0.01239216 -0.02297389 0.07278496 0.01239895 -0.02296322 0.07270896 0.0124228 -0.02294307 0.07263797 0.0124619 -0.0229153 0.07257699 0.01130145 -0.021259 0.07601201 0.01093167 -0.02263188 0.07665401 0.0110355 -0.02242588 0.076653 0.004516005 -0.02356982 0.07260197 0.001576066 -0.02492463 0.07642698 7.43259e-4 -0.02623867 0.07268697 6.88807e-4 -0.02622449 0.07258599 7.13068e-4 -0.02603441 0.07263696 7.54971e-4 -0.02604579 0.07278496 7.53841e-4 -0.02401381 0.07271796 6.03635e-4 -0.02590823 0.072537 6.65296e-4 -0.02602946 0.07257699 6.03131e-4 -0.02621358 0.07250994 6.03508e-4 -0.02602589 0.07252997 5.31823e-4 -0.02602362 0.072501 7.43766e-4 -0.02591532 0.07271599 7.43639e-4 -0.02603989 0.07270801 0.003096938 -0.0238552 0.07568901 -0.001404285 -0.06582033 0.06625896 0.0300976 0.06344854 0.07968598 0.030806 0.06424462 0.076487 0.02706974 0.06382322 0.08421701 0.0242272 0.06568217 0.08336597 0.02879506 0.06404334 0.08018499 0.02753186 0.06428992 0.08376103 -0.02800136 0.06408172 0.08360999 -0.02492368 0.06541556 0.08333802 6.73669e-4 0.06978982 0.08448702 0.01826095 0.06762516 0.08354103 0.02048796 0.06679672 0.08401298 0.01684796 0.06766462 0.084311 -0.003340125 0.06999266 0.08412402 -0.007464647 0.06942492 0.08436298 -0.005267441 0.06877297 0.08465099 -0.01661205 0.06804084 0.08377897 0.01125156 0.06852996 0.08455801 0.01030427 0.06931507 0.08376598 -0.01169955 0.06860995 0.08447402 0.001717031 0.06899851 0.08467 -0.0217787 0.06651902 0.08389002 0.006083965 0.06950771 0.08447098 0.005184531 0.06990677 0.083741 -0.01707506 0.06730371 0.08443599 -0.03005558 0.06346023 0.07963103 -0.02784293 -0.04712373 0.06951993 -0.01353919 -0.02238732 0.07249301 -0.02798765 -0.04707127 0.06977397 -0.02797389 -0.04706996 0.06969797 -0.01373368 -0.02230048 0.07269996 -0.02789908 -0.04709708 0.06956601 -0.0277794 -0.04715681 0.06949096 -0.02857547 -0.04811668 0.06912499 -0.0295704 -0.0493648 0.06620401 -0.01361227 -0.02215093 0.07264399 -0.01360285 -0.02235418 0.07252299 -0.01355898 -0.02227783 0.07253098 -0.01349568 -0.0223115 0.072501 -0.01361417 -0.02224987 0.07257699 -0.01345205 -0.02223587 0.07250696 -0.01369976 -0.02221888 0.07278496 -0.02794367 -0.04707878 0.06962698 -0.01368719 -0.02221947 0.07270902 -0.01365786 -0.02223008 0.07263797 -0.01353496 -0.02078831 0.07671201 -0.01379364 -0.02081006 0.07678699 -0.01406192 -0.02082753 0.07681196 -0.01310849 -0.02073591 0.07641899 -0.01286989 -0.02067333 0.07588201 -0.0291146 0.05327194 0.07940798 -0.02823996 0.05440402 0.07961601 -0.03688436 0.05326527 0.07959902 -0.02768689 0.05857402 0.07959699 -0.03574723 0.05239892 0.079405 -0.0344215 0.05184751 0.07954597 -0.03157597 0.05184572 0.07968598 -0.03027462 0.05088615 0.07969999 -0.03222393 0.0505042 0.07969999 -0.02981156 0.04960757 0.07969999 -0.03213053 0.04964667 0.07969999 -0.03221118 0.05017554 0.07969999 -0.03239119 0.05052417 0.07981997 -0.02166378 0.01046943 0.07599699 -0.03939133 0.05353695 0.082897 -0.02272444 0.01058524 0.07955998 -0.022026 0.01061624 0.07929795 -0.03198605 0.04902482 0.07969999 -0.03224498 0.04983693 0.08198499 -0.03246766 0.04955112 0.08265501 -0.03261905 0.04939132 0.08279502 -0.03306627 0.04895472 0.08291697 -0.03293329 0.04908037 0.082915 -0.03759646 0.05187904 0.08140701 -0.03432255 0.05057746 0.080141 0.0436604 0.04753965 0.05899 0.03673958 0.05066037 0.05899 0.03638327 0.05101662 0.079409 0.02214735 0.01060694 0.07938998 0.02235156 0.01059556 0.07949197 0.03019219 0.05071651 0.07969999 0.02929496 0.06347376 0.07971596 0.03574585 0.05239444 0.07955998 0.02750504 0.05715751 0.079405 0.03688538 0.05327194 0.079409 0.02824115 0.05441004 0.07940602 0.0291205 0.05325973 0.07957899 0.03025257 0.05239892 0.079405 0.04466527 0.05106544 0.08219701 0.03893506 0.05161005 0.08314597 0.03822505 0.052019 0.08259499 0.03867065 0.05107074 0.08250999 0.04232919 0.05107074 0.08250999 -6.63182e-4 -0.0635088 0.06752598 -5.31261e-4 -0.02607321 0.07249999 -3.63228e-4 -0.06346702 0.06723397 -7.43259e-4 -0.02623867 0.07268697 -7.91818e-4 -0.05219709 0.07331198 -0.001005828 -0.05223315 0.07371896 -0.001125693 -0.02460759 0.07621598 -0.001536965 -0.02489829 0.07641994 -0.001178026 -0.0522471 0.07387501 -0.009052038 -0.02230322 0.07618701 -0.005288243 -0.0239669 0.07633799 -0.006627857 -0.02312469 0.075661 -0.01142555 -0.02159571 0.07613199 -0.02578568 -0.04645222 0.07401597 -0.01137155 -0.02172046 0.07630497 -0.01128566 -0.0219087 0.076469 -0.02537173 -0.04673027 0.074454 -0.02750909 -0.04964572 0.073161 -0.02588325 -0.05133026 0.07344895 0.02794367 -0.04707878 0.06962698 0.02798765 -0.04707127 0.06977397 0.02771228 -0.04719418 0.06948095 0.02893143 -0.04873669 0.06889796 0.02789908 -0.04709708 0.06956601 0.02784293 -0.04712373 0.06951993 0.01354259 -0.0221256 0.07258498 0.0137186 -0.02230489 0.07266497 0.01348835 -0.02215528 0.07253801 0.01363074 -0.02234089 0.072546 0.01264226 -0.02039349 0.07261401 0.01342576 -0.02219039 0.07250899 0.0135324 -0.02223193 0.07253402 0.0134027 -0.02230387 0.07249498 0.01346945 -0.02226608 0.07250499 0.01347208 -0.02242451 0.07248401 0.01353919 -0.02238732 0.07249301 0.02282267 0.007440805 0.07279497 0.01977908 0.01102364 0.07250297 -0.02170449 0.006814122 0.07249999 -0.02282267 0.007440805 0.07279497 -0.02398246 -0.001042068 0.07279497 -0.02279758 -0.007502675 0.07260298 -0.01991397 -0.01339548 0.07260596 -0.01789587 -0.0159921 0.07260596 -0.01554149 -0.01828926 0.07261097 -0.01265364 -0.02041703 0.07250899 -0.01347208 -0.02242451 0.07248401 -0.01342862 -0.02234917 0.07249099 0.007882416 -0.02266871 0.07259798 0.0113517 -0.02116191 0.07249999 0.01246857 -0.02302229 0.07262998 0.0236147 -0.004312217 0.07279497 0.01991307 -0.0133993 0.072649 0.02155333 -0.01055228 0.07250201 0.01553815 -0.01829159 0.07262897 0.01286989 -0.02067333 0.07588201 0.01306676 -0.020729 0.07637101 0.02876156 -0.04827129 0.07359999 0.01341569 -0.02066743 0.07669901 0.01376605 -0.0208081 0.07678198 0.02586686 -0.05124783 0.07359898 0.02587658 -0.0463562 0.07354795 0.02501106 -0.0469439 0.07452696 0.02584439 -0.04792219 0.07441198 0.02552717 -0.04662948 0.07435595 0.02585721 -0.04638803 0.07378399 0.006494641 -0.02394038 0.07644498 0.00828725 -0.0225867 0.07586097 0.005347013 -0.02345961 0.07591098 7.91819e-4 -0.0521965 0.07331198 8.29952e-4 -0.02405601 0.07575899 8.84485e-4 -0.02441781 0.07589197 8.7539e-4 -0.05221551 0.07352799 0.001156449 -0.02447319 0.07625496 9.97237e-4 -0.02451002 0.07608097 0.001358211 -0.0247749 0.07636201 8.02692e-4 -0.02433019 0.07551896 6.63183e-4 -0.06350821 0.06752598 7.60168e-4 -0.05217617 0.07308298 5.42624e-4 -0.0634756 0.06729698 5.32472e-4 -0.0260719 0.07249999 6.36574e-4 -0.06349253 0.06741499 6.67995e-4 -0.06428742 0.06711798 0.02736759 -0.0487799 0.06913697 0.0297718 -0.0487464 0.07057595 0.02921748 -0.04912388 0.06771397 0.02823865 0.05990356 0.07949101 0.01496762 0.06844472 0.08363902 0.02153187 0.06663441 0.08345502 0.0258913 0.06503045 0.08329701 -0.02327108 0.06603443 0.08340102 -0.009249031 0.06983053 0.07921397 -0.01149988 0.06912654 0.08369898 -0.00680834 0.06976312 0.08372503 -1.93626e-7 0.07010453 0.08375501 -0.03039997 0.06355637 0.07686299 -0.02823877 0.0599054 0.079539 -0.02361387 -0.004281461 0.07250601 -0.02085578 -0.01203078 0.07692301 -0.02420866 -7.44055e-4 0.07271897 -0.02472895 -0.001471996 0.07840001 -0.02448987 -0.001084983 0.07812798 -0.02487218 -0.001696527 0.07845902 -0.02501958 -0.001923978 0.07846498 -0.02413564 -9.55347e-4 0.077941 -0.03803879 -0.05179202 0.066491 -0.03224468 -0.04876959 0.06660997 -0.03670305 -0.04987698 0.070109 -0.03894352 -0.0523321 0.069251 -0.03346616 -0.04832178 0.07179397 -0.02879577 -0.04833549 0.07362401 -0.01374757 -0.02230179 0.07277697 -0.0280385 -0.04548323 0.07463598 -0.01329296 -0.02076178 0.076581 -0.02770859 -0.04565769 0.07446098 -0.02756065 -0.04572916 0.07430601 -0.02013969 -0.03324878 0.07499498 -0.02147305 -0.0129472 0.07750099 -0.01656144 -0.01889562 0.076981 -0.01523429 -0.01863139 0.07637798 -0.02313315 -0.006682932 0.07703799 -0.01744109 -0.01658749 0.07649302 -0.01928395 -0.01440721 0.07660996 -0.02477097 -0.00403434 0.07828098 -0.02349936 -0.008838593 0.07785999 -0.0239343 -0.005352318 0.07798701 -0.02218377 -0.009370028 0.07718199 -0.03766715 0.05446237 0.07336997 -0.03526878 0.0522859 0.07249099 -0.0370754 0.05361431 0.07314097 -0.03320777 0.05179357 0.07217097 -0.03212606 0.05185306 0.072604 -0.03299999 0.05166274 0.079405 -0.03025364 0.05239677 0.07946503 -0.02768969 0.05573225 0.07958799 -0.02750504 0.05715751 0.079405 -0.03775852 0.05441004 0.079405 -0.0584712 4.55158e-4 0.07249999 -0.02378636 0.00390315 0.07844901 -0.02259266 0.008454442 0.078767 -0.02315926 0.006669223 0.07825499 -0.02324765 0.007410764 0.07903498 -0.02423435 0.006263971 0.07918196 -0.02169162 0.01068204 0.07855898 -0.02168411 0.01054602 0.078767 -0.03224956 0.04981416 0.082111 -0.03233617 0.04991984 0.082336 -0.02711927 0.03014814 0.08081501 -0.03280287 0.04920673 0.08288598 -0.02223545 0.01060146 0.07944101 -0.02247285 0.01059103 0.07953 -0.0362491 0.05064404 0.08258098 -0.03610193 0.04970556 0.082982 -0.03406608 0.04999971 0.08255898 -0.0406183 0.05364853 0.08332699 -0.05788564 0.001517534 0.07873702 -0.02424186 8.67084e-4 0.07800996 0.02159136 0.01049512 0.07277196 0.02278053 0.007883369 0.07864099 0.0239523 9.36194e-4 0.072501 0.02362018 0.004280865 0.07279497 0.03676503 0.04711097 0.08227497 0.03693598 0.04713594 0.08211302 0.03665888 0.04720342 0.082129 0.03720408 0.04755681 0.05885398 0.03633058 0.05003446 0.08241498 0.03673958 0.04813957 0.05899 0.04049998 0.04648381 0.08270096 0.03598177 0.05010831 0.08300995 0.03616958 0.04684603 0.08262902 0.03395545 0.0499897 0.08247601 0.03270983 0.04929912 0.08284902 0.03288447 0.04912716 0.08290702 0.03254973 0.04946345 0.08274 0.02197057 0.0106213 0.07924497 0.03241115 0.04961299 0.08257901 0.0218299 0.01063883 0.07906097 0.03227388 0.04977715 0.08225601 0.03231477 0.04972469 0.08239001 0.03224956 0.04981416 0.082111 0.02169585 0.01052463 0.07882797 0.03224498 0.04983693 0.08198499 0.02169162 0.01068204 0.07855898 0.02357012 0.005077779 0.078615 0.02333456 0.007815062 0.07919496 0.02502477 0.001755297 0.07878696 0.02461946 0.004785001 0.07905203 0.02272444 0.01058524 0.07955998 0.04422998 0.04796701 0.05892497 0.04466569 0.04773414 0.08221799 0.04406398 0.04713594 0.08211302 0.04408365 0.04772251 0.058568 0.02468246 0.0012483 0.07859498 0.0336079 0.06393754 0.07983201 0.06844937 0.02197152 0.06239098 0.06990337 0.01682317 0.06336897 0.02946466 0.06589025 0.07720601 0.06974768 0.01728338 0.06093299 0.03600704 0.06260734 0.07947403 0.03358328 0.06389087 0.07680201 0.07112079 0.009851992 0.05965298 0.07147181 0.007272064 0.06005698 0.03598117 0.06256252 0.076505 0.0717318 0.002247929 0.05564099 0.04011136 0.06003737 0.07857501 0.03905797 0.06069105 0.07728099 0.0442909 0.05699855 0.07751202 0.07171446 -0.00302428 0.05658888 0.0424242 0.0583437 0.07485902 0.04425925 0.05695784 0.07456099 0.04617649 0.05539953 0.07387399 0.04621016 0.05543982 0.07688599 0.04793667 0.05394357 0.07644498 0.07045656 -0.01302433 0.05038297 0.06933599 -0.01795446 0.04859596 0.04882276 0.05308717 0.07471299 -5.26754e-5 0.07229274 0.08286702 0.05049353 0.05147624 0.07252699 0.05126214 0.05076342 0.07533401 0.05337965 0.04850554 0.07448202 0.005935132 0.07210642 0.08304798 0.05439937 0.04745942 0.07435297 0.06769388 -0.02327197 0.04698985 -5.15747e-5 0.07223635 0.07963603 0.06615048 -0.02732032 0.04554599 0.06438368 -0.03115189 0.04412901 0.06584566 -0.0281009 0.047764 0.009370386 0.07162082 0.07945996 0.009374141 0.07167643 0.08264303 0.06361401 -0.03276705 0.04613184 0.05439502 0.04726409 0.070764 0.05524677 -0.04533529 0.04173994 0.05645287 -0.04376929 0.03979086 0.05722928 0.04404944 0.07330197 0.04801255 -0.05286806 0.03910595 0.05675077 0.04446464 0.07308101 0.04654484 -0.05409675 0.03624397 0.05643755 0.04477453 0.07011902 0.03975677 -0.05926722 0.03686887 0.04330605 -0.05671995 0.03535896 0.06007421 0.03982383 0.07153499 0.03848767 -0.06004309 0.03419786 0.01449257 0.07075345 0.079162 0.03529435 -0.06200748 0.03591096 0.03043752 -0.06447851 0.03269785 0.05748367 0.0434212 0.07000201 0.03020805 -0.06462031 0.03499299 0.02150887 -0.06773954 0.03098994 0.02542906 -0.06681853 0.03474295 0.02139705 -0.0680238 0.03379786 0.02110046 -0.06807321 0.03144699 0.01817584 0.06987154 0.07845097 0.06260246 0.03566807 0.070077 0.01726287 -0.0691055 0.03095901 0.06254148 0.03567063 0.06714999 0.0600301 0.03979796 0.068605 0.01768618 -0.06907552 0.03356498 0.01403772 -0.06985342 0.03082484 0.06460797 0.03180652 0.06869202 0.02360343 0.06825357 0.07817798 0.01910239 0.06971657 0.08198201 0.0236057 0.06831353 0.08148801 0.0649597 0.03095865 0.065678 0.06668019 0.02696806 0.06410902 0.0668593 0.02652037 0.06413799 0.02816557 0.06655687 0.08087903 1.17238e-5 -0.07052904 0.03125399 0.03776085 0.05440843 0.07955896 0.02830499 0.05986595 0.07513397 0.0276916 0.05818444 0.07438498 0.02768903 0.0557357 0.07958996 0.02760154 0.05689615 0.07425397 0.02958089 0.0530008 0.07295501 0.03157764 0.05185002 0.079405 0.03442203 0.05185002 0.079405 0.03353577 0.05180883 0.07258898 0.03687983 0.05342376 0.07311803 0.04497808 0.05193871 0.08271801 0.04416418 0.05098044 0.05866998 0.04415029 0.05166792 0.08244901 0.04396128 0.05116802 0.05846887 0.04472994 0.04697424 0.08252298 0.04319953 0.05124312 0.05898702 -0.001323342 -0.02463454 0.07635599 -0.001567125 -0.05226248 0.07404196 -8.75389e-4 -0.05221629 0.07352799 -8.02692e-4 -0.02433019 0.07551896 -0.001143932 -0.06164377 0.070526 -6.81246e-4 -0.06439173 0.06708896 -8.39153e-4 -0.05608099 0.07285499 -0.001759946 -0.05226451 0.07406097 -0.006306231 -0.02429151 0.07650798 -0.02585607 -0.0463919 0.073794 -0.02567929 -0.04652971 0.07419896 -0.02553761 -0.04662489 0.07434797 -0.02501177 -0.04694533 0.07452696 -0.0274083 -0.04900372 0.07339197 -0.02647668 -0.04873096 0.07423698 -0.02598166 -0.0597667 0.06770396 -0.02465873 -0.05836468 0.069884 -0.02477085 -0.05696833 0.07091701 0.001560986 -0.06974261 0.06120294 -0.002575874 -0.06911319 0.06149786 0.003668785 -0.0676769 0.06149995 -0.003664851 -0.06769382 0.06133186 -0.001438677 -0.06977611 0.05927801 0.0149011 -0.01889908 0.07632297 0.02279728 -0.007503688 0.072591 0.05873018 -5.94184e-4 0.07253402 0.02383399 -0.00102061 0.07249999 -0.01310688 -0.01859086 0.07249999 0.02207219 0.005693912 0.07249999 0.02130556 0.01096081 0.072501 0.01789319 -0.01599109 0.072501 -0.02361404 0.004279017 0.072501 -0.02428203 -4.55223e-4 0.07249999 -0.02155452 -0.0105493 0.072501 0.02470445 -0.001433253 0.07838398 0.02203518 -0.00970906 0.07706797 0.02306836 -0.0069229 0.077389 0.01890766 -0.01489943 0.07671397 0.02501958 -0.001923978 0.07846498 0.02064865 -0.01238709 0.07707196 0.02458459 -0.005056977 0.07819098 0.0168333 -0.01721173 0.07668799 0.02201277 -0.0120027 0.07758301 0.01282328 -0.02048021 0.07620996 0.01324808 -0.02075618 0.07654798 0.01406192 -0.02082753 0.07681196 0.02473556 -0.05677843 0.07107698 0.0250656 -0.05766528 0.06964296 0.02468425 -0.05585861 0.071141 0.0256775 -0.04652893 0.07419997 0.02739048 -0.04965049 0.07391697 0.01859349 -0.03405427 0.07511997 0.02534765 -0.04674333 0.07446497 0.02747064 -0.04911708 0.073502 0.01338547 -0.03744781 0.07535696 0.001801431 -0.02507382 0.07643997 7.41695e-4 -0.05682575 0.07202398 0.001005828 -0.0522325 0.07371896 0.001235425 -0.05450171 0.07372999 0.001178026 -0.05224651 0.07387501 0.001381695 -0.05225658 0.07398599 0.001759946 -0.05226367 0.07406097 0.001567125 -0.05226171 0.07404196 0.001070857 -0.06366842 0.06880998 9.25776e-4 -0.05820101 0.07216596 0.06945008 -0.01996618 0.05129086 0.0679298 -0.0227766 0.04970699 0.0667048 -0.02659147 0.04878383 0.06779426 -0.02587002 0.04924386 0.0644828 -0.0314626 0.04704797 0.06106185 -0.03724175 0.04456901 0.06024408 0.03980046 0.07187098 0.06214106 -0.03581398 0.04553699 0.05817925 -0.04154938 0.04306298 0.058465 -0.0416671 0.04361283 -0.01409935 0.07088893 0.082367 -0.00855261 0.07182234 0.08291602 0.06155616 0.03831857 0.07149702 0.06508177 0.03200805 0.06930899 0.06310647 0.03506505 0.07022398 0.05366617 -0.04754412 0.04153198 -0.006495893 0.07235252 0.08329498 0.06665849 0.02737873 0.067465 0.05174487 -0.0492593 0.04037886 -0.0047701 0.07213145 0.082892 0.05025374 -0.05112069 0.04029583 0.0469318 -0.0541467 0.03923285 0.06849318 0.02217185 0.06561499 -7.71362e-4 0.07249706 0.08334302 0.04400473 -0.0562148 0.03794986 0.04330193 -0.05708259 0.03820896 0.06918185 0.02094411 0.06546598 0.04111742 -0.05920851 0.03768599 0.03857249 -0.06027668 0.03701382 0.07078057 0.01248872 0.06195402 0.06993246 0.01694095 0.06379997 0.009719669 0.07173812 0.08296 0.03502029 -0.06243789 0.03633385 0.07263708 0.007711589 0.06088584 0.0127269 0.07151746 0.08300602 0.01426857 0.07086622 0.08246099 0.07087141 0.01229023 0.06215184 0.02843749 -0.06622093 0.03525495 0.07117187 0.01227927 0.062415 0.0715388 0.007515907 0.06058984 0.02260977 0.06878793 0.08192199 0.03063559 -0.0647521 0.03557497 0.07183969 0.002291381 0.05874896 0.0291031 0.06626051 0.08106297 0.03184998 0.06484127 0.080253 0.02213138 -0.0679785 0.034334 0.01404589 -0.06989324 0.03314983 0.03589344 0.06285846 0.079867 0.01562374 -0.06990092 0.03377598 0.07191836 -0.002995729 0.05703687 0.07225447 7.52523e-4 0.05847299 0.0707038 -0.0127322 0.05354899 0.07128357 -0.008075654 0.055031 0.07050216 -0.01303613 0.05306684 0.04415959 0.05734992 0.07801401 0.06943917 -0.01820409 0.05154883 -0.05248343 -0.0484746 0.04064285 -0.05210459 -0.04949676 0.04097282 -0.06140297 0.03796672 0.07122898 -0.04810035 -0.05455636 0.03929901 -0.06054472 0.03910017 0.071285 -0.0551691 -0.04580682 0.04212188 -0.05692785 -0.04432052 0.04284799 -0.05571985 -0.04475462 0.04194295 -0.06291669 0.03602284 0.070701 -0.05856674 0.04224091 0.07272499 -0.05776208 -0.04236489 0.04322898 -0.05416446 0.04791224 0.07471597 -0.06133836 -0.03719466 0.045111 -0.06348729 -0.03334575 0.04637485 -0.06376779 -0.03246235 0.04623997 0.009631037 -0.07077091 0.033351 0.004716813 -0.07108712 0.03040099 -0.04718369 0.05486202 0.07712399 0.004554927 -0.07151633 0.03329485 -0.04743456 0.05493885 0.07725799 -0.04675966 0.05497646 0.076806 -0.04274809 0.05817282 0.07792299 4.58502e-5 -0.07128548 0.03266698 0.004436433 -0.07117789 0.03294098 0.002149343 -0.07200181 0.03325086 -0.04206305 0.05884492 0.078498 -0.0664556 -0.02719873 0.04853987 -0.06913048 -0.01882737 0.05100584 -0.06869578 -0.02235043 0.05046486 -0.03464019 0.06355905 0.08011698 -0.06905817 -0.01963108 0.05114787 9.56458e-5 -0.07154518 0.03317898 -0.004845619 -0.07113182 0.03276085 -0.004400432 -0.071473 0.03327596 -0.07025396 -0.01492595 0.05279195 -0.0711953 -0.01246953 0.05388885 -0.00926429 -0.07068389 0.03287798 -0.01080656 -0.07068508 0.03339684 -0.07147938 -0.007416427 0.05539399 -0.02547425 0.06780624 0.081631 -0.03323239 0.06461375 0.080612 -0.01904708 -0.06870794 0.03356885 -0.01641988 -0.06949752 0.03360986 -0.02119028 -0.06831252 0.03422296 -0.01777887 0.07006555 0.08222204 -0.02397489 -0.06747078 0.03457695 -0.0719645 -9.57746e-5 0.05799686 -0.02493178 0.06888824 0.08209401 -0.02791225 -0.06643193 0.03518187 -0.07180726 0.005218446 0.05984497 -0.0301938 -0.06484252 0.03540599 -0.07154256 0.006400585 0.05982393 -0.07118296 0.01085907 0.06178086 -0.01428735 0.07097905 0.08272802 -0.07194167 0.007848799 0.06093388 -0.03709554 -0.0612663 0.03672099 -0.04127007 -0.05823075 0.03724586 -0.03725481 -0.06169718 0.03682386 -0.0701003 0.01817619 0.06451398 -0.04362088 -0.0567699 0.03824585 -0.06990545 0.01761239 0.06414401 -0.04761505 -0.05353146 0.03940486 -0.04902416 -0.05194038 0.03943085 -0.067088 0.02738553 0.06770598 -0.04588228 -0.05562007 0.03893101 -0.0671249 0.02652037 0.067276 -0.05103772 -0.0501582 0.04039198 -0.0365622 0.0551036 0.07331997 0.03765708 0.05445665 0.07310199 0.03144145 0.0520116 0.07265502 0.0278365 0.05903697 0.079405 0.03046375 0.05241376 0.07278698 0.03586632 0.05260515 0.072806 0.0288223 0.05375796 0.07302802 0.03469824 0.05206269 0.07227396 0.02824252 0.05462032 0.07350897 0.0278337 0.05559492 0.07382798 0.03246843 0.05181485 0.072187 0.01660341 0.06925165 0.07822 0.01991206 0.06785666 0.07798099 0.01694536 0.06836915 0.07870298 0.02143836 0.06709623 0.07825797 -0.006880939 0.07041436 0.07881301 -0.004620492 0.07029163 0.07937598 -0.02695459 0.06507551 0.077551 2.88427e-5 0.07047307 0.07906603 8.39461e-4 0.07120704 0.07890301 -0.02630579 0.06565934 0.07718801 -0.02376037 0.06630945 0.07798302 0.003646016 0.07034957 0.079396 -0.01977539 0.06841117 0.07792901 0.03017348 0.06412512 0.07653498 -0.01935315 0.06775301 0.07815599 0.007278919 0.07006454 0.07929599 0.02584075 0.06554377 0.077488 0.01172125 0.06977272 0.07854902 0.01238358 0.06934171 0.07904303 -0.0158562 0.06944036 0.07828599 -0.007001042 0.07089072 0.07878798 -0.0143162 0.06899327 0.078588 -0.03623002 0.05285894 0.072537 -0.02901577 0.05353647 0.07301902 -0.030025 0.05267453 0.072838 -0.02830499 0.05986481 0.07510602 -0.02781498 0.05871921 0.07485097 -0.03106886 0.05214291 0.07241803 -0.02765715 0.05800986 0.07432299 -0.02776324 0.05584383 0.07390999 -0.02824777 0.05461084 0.07350599 -0.03426235 0.05193132 0.07262903 -0.0263018 -0.05208522 0.06620198 -0.02519965 -0.0554226 0.06638199 -0.02531689 -0.0572021 0.06637299 -0.0255118 -0.0536648 0.06658196 -0.03545206 -0.04957866 0.06619995 -0.03052008 -0.04900527 0.06640195 -0.03459137 -0.04879277 0.069974 -0.03694158 -0.05056899 0.06642597 -0.05907815 -7.41433e-4 0.07270401 -0.05892539 -6.88561e-4 0.072609 -0.0584712 -4.55223e-4 0.07249999 -0.05869549 -5.76238e-4 0.07252496 -0.02371096 -0.004243195 0.07752102 -0.05815243 -0.00138545 0.07836198 -0.0244264 -0.001257658 0.07828402 -0.05844157 -0.001018226 0.07804399 -0.05831825 -0.001176834 0.07822 -0.05851233 -9.22118e-4 0.07787299 -0.02437639 -8.81709e-4 0.07776397 -0.02436035 -8.37075e-4 0.07748699 -0.0585711 -8.37075e-4 0.07748699 -0.03953707 -0.05399358 0.06620401 -0.03795313 -0.05081719 0.07096201 -0.03102058 -0.04805219 0.07401502 -0.03224986 -0.04804718 0.07347995 -0.03389924 -0.04816943 0.07358998 -0.03564035 -0.0488733 0.07290798 -0.01290392 -0.0205335 0.07631498 -0.02788156 -0.04556798 0.07457596 -0.04782176 -0.02879661 0.07611399 -0.03671824 -0.05570042 0.06634998 -0.0397917 0.05765235 0.07967996 -0.02400434 8.10573e-4 0.072591 -0.05873018 5.94119e-4 0.07253402 -0.05890417 6.77729e-4 0.07259899 -0.05906647 7.39233e-4 0.07269799 -0.06343746 0.02201592 0.079203 -0.05731076 0.03522419 0.08108299 -0.05659472 0.03474134 0.08138799 -0.05597364 0.04035723 0.08064502 -0.0625317 0.01627993 0.07967299 -0.05485737 0.03953319 0.08151197 -0.0648086 0.02223622 0.07805401 -0.06532758 0.01717883 0.07814699 -0.05423676 0.03904479 0.08178901 -0.05354726 0.03850096 0.08194398 -0.06365317 0.01662147 0.07923299 -0.06160295 0.02096045 0.08010798 -0.06039702 0.02082276 0.08037596 -0.05154275 0.04317259 0.08217298 -0.06120228 0.01644539 0.079988 -0.05288398 0.04442864 0.08126598 -0.05148327 0.04039925 0.08216798 -0.06685817 0.01254576 0.076882 -0.05207318 0.04365766 0.08192497 -0.06259727 0.02678102 0.07922697 -0.0605461 0.006050467 0.07916301 -0.0652222 0.01431167 0.07826596 -0.05120295 0.04226875 0.08227902 -0.06262409 0.0213555 0.07970201 -0.0648846 0.007840931 0.07823801 -0.0611819 0.02618092 0.08018201 -0.06432145 0.0119363 0.07876896 -0.04594475 0.05087661 0.08264899 -0.06026536 0.0257427 0.08055102 -0.0664699 0.01755464 0.076945 -0.0633412 0.00715363 0.07882899 -0.04409605 0.05099666 0.08305299 -0.06345075 0.02743226 0.07821601 -0.0484997 0.04713386 0.08254301 -0.04569637 0.04834336 0.08286297 -0.06310409 0.01165962 0.07924497 -0.04356926 0.05279797 0.08295702 -0.06073188 0.03144282 0.07971596 -0.05943948 0.03072232 0.08063298 -0.05862337 0.03025597 0.080971 -0.06187444 0.007764399 0.07921797 -0.05771356 0.02973735 0.08116698 -0.05837589 0.03592181 0.08031797 -0.02502477 0.001755297 0.07878696 -0.05819106 0.003342092 0.07892596 -0.04163485 0.02770197 0.08105701 -0.05714678 0.02814823 0.08109599 -0.05984956 0.01743322 0.080159 -0.0242266 9.26373e-4 0.07819199 -0.05834347 0.001013815 0.07835102 -0.02475959 0.001359403 0.07866698 -0.02462273 0.00116384 0.07852602 -0.05819165 0.0011729 0.078534 0.05869549 5.76173e-4 0.07252496 0.0584712 4.55158e-4 0.07249999 0.05892544 6.88501e-4 0.07260996 0.05907559 7.4079e-4 0.072703 0.02386426 0.003326714 0.07795 0.04163748 0.02558702 0.08087199 0.03306627 0.04895472 0.08291697 0.04498147 0.04985553 0.08298999 0.02436828 8.42628e-4 0.07780897 0.02409493 8.61184e-4 0.07799899 0.05832839 0.001029193 0.07837301 0.02446293 9.47675e-4 0.07823801 0.05816262 0.001204252 0.078561 0.0245527 0.001066625 0.078422 0.02469956 0.001478672 0.07872301 0 -0.07044869 0.03579092 -0.001615226 -0.0701366 0.03646296 0.001621425 -0.07021564 0.03190094 -0.002708435 -0.06899988 0.05911284 -0.001548588 -0.06997632 0.04640299 0.001550436 -0.06990283 0.05055499 0.002684414 -0.06927263 0.04814785 -0.002462506 -0.06946671 0.04622799 -0.003128886 -0.06887191 0.04734587 -0.002799332 -0.06910061 0.05133897 -0.001089513 -0.0703631 0.034105 0.002965867 -0.06911563 0.04422897 0.002976715 -0.06867998 0.06130999 0.001501977 -0.07006579 0.04173886 0.002719819 -0.06901252 0.05814188 0.001450896 -0.06977224 0.05911386 0.002699792 -0.0691446 0.05293798 0 -0.07001721 0.06097996 0.06943297 -0.01479458 0.04908388 0.05779999 0.04213482 0.06881999 -0.009419083 0.07160705 0.07928699 0.0592398 0.04080677 0.06847101 0.06803506 -0.02226787 0.04718196 0.0647397 -0.02988392 0.04397201 0.0678187 -0.02228212 0.04659187 0.06209743 0.03619587 0.06688201 -0.002702653 0.07202756 0.079185 0.004647076 0.07208704 0.07962 0.06456208 0.03178399 0.065764 0.06466031 0.03139954 0.06528902 0.0623821 -0.03346067 0.04261291 0.06330716 -0.03262078 0.04293197 0.06310689 0.03353881 0.06584 0.06716746 0.02525496 0.06312501 0.06211304 -0.03539669 0.04263097 0.01304948 0.07096034 0.07888901 0.0595985 -0.0394122 0.04099196 0.06883656 0.01735299 0.060229 0.06828486 0.02227085 0.06216084 0.05637896 -0.04356509 0.03931099 0.05337154 -0.04744333 0.03848296 0.07004398 0.01594692 0.06010699 0.05180418 -0.04880142 0.03752994 0.05008006 -0.05087441 0.03733396 0.04647809 -0.05388808 0.03571397 0.06996387 0.01499193 0.05950099 0.07100945 0.005081415 0.05597388 0.03947496 -0.05921185 0.03394198 0.03843808 -0.05918675 0.03369796 0.03440403 -0.06245571 0.03336799 0.0714246 0.007267296 0.05733597 0.07080626 0.01203662 0.05897498 0.02613067 0.06686115 0.07739198 0.0712679 0.007559657 0.05697101 0.02710837 -0.06568032 0.031681 0.03229963 -0.06334072 0.03250694 0.02761387 -0.06492263 0.03171187 0.02590405 -0.06640893 0.03198587 0.02071917 -0.06745553 0.03082698 0.03751015 0.0615853 0.07569998 0.0716381 0.001981258 0.05513387 0.0181967 -0.06865191 0.03065598 0.01409173 -0.06969672 0.03040885 0.07166355 -0.003041744 0.05374199 0.03492999 0.06203985 0.07572001 0.07123178 -0.008039951 0.05199599 0.04611796 0.05530107 0.07345402 0.070082 -0.01401787 0.04956185 -0.05054736 0.05134022 0.07212197 -0.07006037 0.0159024 0.06030386 -0.07090187 0.01064014 0.05809396 -0.04822415 0.05361145 0.07322299 0.009409725 -0.07062011 0.03056287 0.009338796 -0.07051122 0.03014695 0.004924118 -0.07098281 0.03001898 -0.0650261 -0.02952075 0.04423689 -0.06886065 0.02060115 0.06191986 4.60502e-4 -0.07096201 0.02975785 -0.00510019 -0.07106482 0.03040796 -0.005989909 -0.0702449 0.02987599 -0.06884276 0.02033877 0.06146699 -0.06449627 -0.02922779 0.04407984 -0.01292306 -0.06940329 0.030155 -0.005320191 -0.07081079 0.02986598 -0.06840258 0.02017152 0.06120598 -0.01528036 -0.06948733 0.03051698 -0.06863766 -0.02030372 0.04768395 -0.06758379 -0.02219748 0.04656589 -0.01005136 -0.07028853 0.03003185 -0.04036855 0.05978852 0.07524299 -0.06735509 0.02518522 0.06349599 -0.0385167 0.06040632 0.07515501 -0.02059894 -0.06803113 0.03092283 -0.0239591 -0.06635272 0.03121685 -0.01917809 -0.06788271 0.03068786 -0.06656926 0.02681761 0.06366598 -0.02487617 -0.06679278 0.03187698 -0.02509665 -0.06652158 0.03145295 -0.06985968 -0.01541727 0.04915398 -0.0296787 -0.06481689 0.03255587 -0.02976417 -0.0645864 0.03211498 -0.06407839 0.03181988 0.06526201 -0.03349179 -0.06269842 0.03272485 -0.03879505 -0.05962938 0.03379297 -0.06970906 -0.01398259 0.04938501 -0.06548905 0.02959072 0.06470102 -0.03851497 -0.05916339 0.03370589 -0.03886008 -0.05980098 0.034271 -0.04315376 -0.05660367 0.03487199 -0.0431903 -0.05678308 0.03531783 -0.07154065 -0.004811406 0.05318289 -0.07092911 -0.009679555 0.05114299 -0.0472579 -0.0534814 0.03645998 -0.04715818 -0.05334925 0.03599399 -0.05088394 -0.04984205 0.03720599 -0.01766026 0.06955665 0.078327 -0.05128246 -0.04865896 0.03734499 -0.0544849 -0.04507976 0.03858482 -0.07161468 -0.002070963 0.05368089 -0.07113528 0.001491904 0.05472987 -0.05469197 -0.04553598 0.03861695 -0.05453658 0.04711437 0.07089602 -0.07173717 6.10728e-6 0.05483984 -0.01433348 0.07067722 0.07879501 -0.06048935 -0.03674042 0.04147601 -0.07157289 0.004680097 0.05616086 -0.05786246 -0.04165083 0.04005783 -0.06087166 -0.03723448 0.04159796 -0.07101786 0.01061934 0.05848801 7.11364e-4 -0.07050019 0.02977287 0.06478667 0.01333689 0.07856798 0.05949646 0.03176903 0.08047801 0.05197405 0.04634892 0.08096802 0.064803 0.02308243 0.07791101 0.0506137 0.04186946 0.08229702 0.06558787 0.01743412 0.077955 0.06133556 0.01742804 0.08002799 0.04919457 0.04667943 0.08241301 0.06022197 0.02632457 0.08055502 0.05682605 0.03728914 0.08102101 0.06652146 0.01839292 0.07671201 0.06302875 0.02311486 0.07936799 0.04080718 0.05467641 0.08335298 0.05291014 0.03914844 0.08202999 0.05669206 0.03524971 0.08132302 0.05598127 0.03153926 0.081393 0.05859112 0.0368418 0.079813 0.06365948 0.01991355 0.07916998 0.05227547 0.0415489 0.08209097 0.06305247 0.01367855 0.07937198 0.04206866 0.049488 0.08296197 0.06715428 0.01245504 0.07663798 0.05514448 0.04241353 0.08041799 0.05857443 0.02797091 0.08096998 0.05871659 0.03093582 0.080913 0.05888146 0.02245414 0.080599 0.06439787 0.008132517 0.07850998 0.06280559 0.02773123 0.07888996 0.05081754 0.04642319 0.08185702 0.0617997 0.02029395 0.08001601 0.06614768 0.008180618 0.07747298 0.06796067 0.008174657 0.07567697 -0.02264183 -0.06092071 0.06807297 -0.027722 -0.06184279 0.063721 -0.02994775 -0.05950736 0.066356 -0.007141888 -0.0543856 0.07370096 -0.007577598 -0.05994051 0.07144099 -0.02337116 -0.06348639 0.06380397 -0.01614367 -0.05540299 0.07302498 -0.008335053 -0.06447321 0.06748598 -2.04835e-4 -0.06720441 0.064031 -0.02116096 -0.05822271 0.07084399 -0.01545774 -0.05188477 0.07402896 -0.01533079 -0.05335938 0.07371497 -0.0245918 -0.05546087 0.07192397 -0.01925563 -0.04918968 0.07432997 -0.02008473 -0.05540883 0.07264 -0.01773566 -0.06066709 0.069651 -0.00403583 -0.06028848 0.07138597 -0.007227659 -0.06128227 0.07055801 -0.007971525 -0.06257754 0.06945097 -0.01241827 -0.05951571 0.07129496 -0.001929283 -0.05724543 0.07293301 -0.007213056 -0.05709022 0.07286596 -0.01897299 -0.0647878 0.06390196 -0.01333725 -0.06358259 0.06765097 -0.02042448 -0.05411499 0.07314497 -0.001391649 -0.05432713 0.07379096 -0.01380938 -0.06587278 0.064067 -0.01731145 -0.05945187 0.07068896 -0.02494305 -0.05314409 0.07303798 -0.001479685 -0.05874317 0.07230198 -0.01292955 -0.06178313 0.069534 -0.01004815 -0.06692421 0.06293696 -0.004935026 -0.06747692 0.06298696 -0.004854619 -0.0662747 0.06540101 -0.004282057 -0.064471 0.06782901 -0.01173657 -0.05639332 0.07293301 -0.0119974 -0.0575217 0.07243299 -0.001457512 -0.06317991 0.06932699 -0.01263475 -0.0604887 0.07060599 -0.006206214 -0.05197519 0.07408601 -0.01829195 -0.06252419 0.06767696 -0.01699048 -0.05813568 0.07161998 -0.008526027 -0.0665878 0.06422197 -0.02319365 -0.06256181 0.06562995 0.02781027 -0.04560381 0.07453697 0.02926838 -0.04855018 0.07406497 0.02799558 -0.04550468 0.074624 0.02742588 -0.04578459 0.07407701 0.02752488 -0.04574352 0.07425796 0.02765578 -0.04568189 0.07441496 0.03489935 -0.04847604 0.07317298 0.03232616 -0.04832482 0.07094997 0.02376079 -0.003959715 0.07760298 0.02428203 -7.55178e-4 0.07279497 0.05908125 -7.4266e-4 0.07270699 0.0584712 -4.55223e-4 0.07249999 0.05793726 -0.001654088 0.07845199 0.024845 -0.001654088 0.07845199 0.02439719 -0.001224279 0.07826 0.05846965 -9.81095e-4 0.077986 0.05837255 -0.001107633 0.07815301 0.0244776 -0.001064062 0.07810395 0.0244174 -9.59806e-4 0.07794898 0.02414757 -8.78541e-4 0.07775497 0.05877459 -8.02811e-4 0.07552397 0.05772179 -0.001923978 0.07846498 0.04641407 -0.0305376 0.075962 0.02473723 -0.058281 0.06991696 0.01797205 -0.05311387 0.07365995 0.01989924 -0.05851668 0.07088994 0.01895856 -0.05590766 0.07254201 0.008207023 -0.06716173 0.06301099 0.02539473 -0.05933773 0.06874698 0.01193809 -0.06600743 0.06441301 0.022955 -0.06372851 0.06343299 0.001225352 -0.06568533 0.06645095 0.02475035 -0.05360502 0.07287597 0.01640439 -0.06511318 0.06447499 0.01870185 -0.05475771 0.073062 0.01445823 -0.06160897 0.06947201 0.02185726 -0.06363123 0.06449395 0.01471304 -0.05705797 0.07243096 0.009574592 -0.05806273 0.07233399 0.01515913 -0.06362158 0.06721395 0.01456403 -0.0584324 0.07173401 0.0139299 -0.05041509 0.07422298 0.00986278 -0.0610727 0.07050299 0.01704609 -0.05086714 0.07415795 0.01510429 -0.06009978 0.07058298 0.005695283 -0.05985552 0.07157796 0.007192313 -0.06602609 0.06553995 0.008791506 -0.05662399 0.072995 0.01208114 -0.0539565 0.07368201 0.001793742 -0.06181257 0.070441 -4.24156e-5 -0.06765592 0.062922 0.02136516 -0.06265622 0.06636101 0.001489818 -0.05930876 0.07201796 0.01944166 -0.0514518 0.07398396 0.002035737 -0.0538572 0.07388097 0.007203519 -0.06267762 0.06943696 0.001721203 -0.05653738 0.07321798 0.00711745 -0.0642603 0.06789797 0.0265854 -0.0606355 0.066715 0.02032715 -0.05964833 0.06994199 0.02073085 -0.06100755 0.06856995 0.03976047 -0.05431866 0.06620401 0.03536385 -0.0495156 0.06637799 0.02605658 -0.05244725 0.06642198 0.03687137 -0.05050998 0.06641799 0.03801923 -0.05178213 0.06642198 0.02743005 -0.05063539 0.06639498 0.02517616 -0.05584836 0.06646496 0.02539002 -0.05415922 0.06639295 0.02957129 -0.05975282 0.06619399 0.03957527 0.05997753 0.07500398 -0.06604361 -0.02932637 0.04804599 0.03328305 0.06529074 0.08084702 0.06350618 -0.03557986 0.04587501 0.06379908 -0.03380918 0.04649198 0.027493 -0.06733059 0.03487098 0.02606457 0.06845581 0.081945 -0.07228809 -0.002573907 0.05731987 0.01991122 -0.06926238 0.03420192 0.01188206 -0.07105511 0.03357994 0.07153666 -0.01048576 0.05457699 -0.0150941 0.07168763 0.08306497 -0.008223831 -0.0715655 0.03340286 -0.05420708 0.0482155 0.07492798 0.0618543 0.038998 0.07173198 0.05379009 0.04875183 0.07511401 0.03788119 0.06271892 0.07995599 -0.03170597 0.06292945 0.07644802 0.0514515 0.04913651 0.07124698 0.07072776 -0.001381278 0.05381596 0.0710988 -0.005838513 0.052266 0.07039576 -0.008272767 0.05134499 -0.06065464 0.03847706 0.06761097 -0.02615219 0.06624281 0.07717698 -0.03198188 0.06422191 0.07647699 -0.0679568 -0.02015 0.04722696 0.02012288 0.06819826 0.07786798 0.00577116 0.07162916 0.07904601 -0.03157711 -0.06000262 0.06400197 -0.02758795 -0.05636698 0.06620496 -0.02707606 -0.06088978 0.066199 -0.03623116 -0.05343461 0.06620299 -0.02428203 -7.55178e-4 0.07279497 -0.06040763 -0.004970908 0.07743197 -0.05910885 -0.004979968 0.07819801 -0.05772179 -0.001923978 0.07846498 -0.05929487 -0.003411412 0.07781797 -0.05796808 -0.001615822 0.078444 -0.05854797 -8.67164e-4 0.07770597 -0.06319636 -0.006824076 0.07616096 -0.0609011 -0.006492376 0.07788199 -0.03836178 -0.03852868 0.07526296 -0.05205214 -0.0425322 0.06844198 -0.0369578 -0.04974663 0.07205498 -0.04615426 -0.04586291 0.07041698 -0.05136579 -0.04469358 0.06633096 -0.06079757 -0.02375483 0.07359498 -0.06316119 -0.02256768 0.072088 -0.05687201 -0.02557706 0.07514399 -0.06104207 -0.01951271 0.07506901 -0.06461066 -0.01772308 0.07297199 -0.05892199 -0.021402 0.07555097 -0.0659576 -0.01792532 0.07114201 -0.04014092 -0.05452388 0.06555098 -0.05646145 -0.02418547 0.075706 -0.0662927 -0.01288306 0.07315498 -0.03976929 -0.04593229 0.07315999 -0.05522596 -0.0219267 0.076496 -0.05075109 -0.04190295 0.07032197 -0.06213325 -0.01653313 0.07537597 -0.05476385 -0.03804826 0.06994599 -0.0609588 -0.01627582 0.07605201 -0.0437451 -0.04456168 0.07249701 -0.05796939 -0.021097 0.07599699 -0.06324809 -0.01834172 0.07398802 -0.02865159 -0.04661601 0.07452601 -0.04785192 -0.04037648 0.07298099 -0.03895086 -0.04503887 0.07374101 -0.06488007 -0.013251 0.07444202 -0.06320506 -0.01300132 0.07573997 -0.05368876 -0.02112501 0.07676798 -0.05950731 -0.0163809 0.076568 -0.03670877 -0.04296398 0.07468599 -0.04784107 -0.04221868 0.07195597 -0.04063749 -0.04043591 0.07483994 -0.06086426 -0.01231896 0.07694602 -0.04244327 -0.04213804 0.07401502 -0.04455918 -0.03952181 0.074413 -0.04050678 -0.03780883 0.07530599 -0.06328976 -0.007541179 0.07695299 -0.05808538 -0.01561617 0.07706099 -0.04488396 -0.04146128 0.07359498 -0.04431158 -0.0513854 0.06571799 -0.05710268 -0.0119574 0.07758802 -0.05153954 -0.03799539 0.07240897 -0.05435401 -0.035012 0.07235801 -0.04850047 -0.03592848 0.07459998 -0.04539018 -0.03587162 0.07524895 -0.04774779 -0.03851717 0.07388198 -0.05762487 -0.03371948 0.07057595 -0.04384899 -0.05011981 0.06798899 -0.05210065 -0.035492 0.07342696 -0.06105756 -0.03035449 0.06908398 -0.06254768 -0.02767783 0.06893599 -0.06070023 -0.02824759 0.07128196 -0.04250377 -0.04842442 0.070571 -0.0282011 -0.04539179 0.07466197 -0.05391097 -0.03299558 0.07368296 -0.05747544 -0.03105247 0.07242798 -0.04770088 -0.04847329 0.06593096 -0.05175852 -0.03178608 0.07498997 -0.05030828 -0.02991539 0.07578098 -0.04754108 -0.04683738 0.06835496 -0.05795705 -0.02643454 0.07437497 -0.05450749 -0.02961802 0.07475 -0.05415827 -0.02740079 0.07554101 -0.04799139 -0.02979129 0.07600897 -0.059044 -0.02643603 0.07365298 -0.03246599 -0.05991941 0.06237787 -0.03484338 -0.05920177 0.05328196 -0.06808996 -0.007703244 0.07272398 -0.0674377 -0.01224881 0.07173597 -0.0445019 0.05376005 0.08175599 -0.04483586 0.05320119 0.082327 -0.04708397 0.05146044 0.08145999 -0.04873979 0.04939281 0.08182597 -0.05053573 0.04796165 0.08107101 -0.0535652 0.04445612 0.08062797 -0.05634826 0.04075402 0.07999998 -0.06147485 0.03219729 0.07871598 -0.06668436 0.01848042 0.07617598 -0.06532609 0.02290487 0.07713598 -0.04073494 0.05649381 0.08017998 -0.04050636 0.05713695 0.07974803 -0.03992128 0.0583226 0.074624 -0.05650293 -0.04215002 0.03970783 -0.06916916 0.01408815 0.05926299 -0.04743099 0.05204641 0.07299399 -0.05547773 -0.04312551 0.03971898 -0.001196265 -0.07039952 0.02986788 -0.07028967 -0.002505183 0.05391883 -0.0582754 -0.03927153 0.04106599 -0.06803488 0.02037835 0.06127697 -0.0076496 -0.06980842 0.03039383 -0.0624873 -0.03263521 0.04304099 -0.04505527 0.05574715 0.07353901 -0.04367268 0.05554831 0.07370698 -0.05649966 0.04247242 0.06910902 -0.06629675 -0.02391612 0.04607999 -0.0668407 -0.02179092 0.04717683 -0.0529319 0.04673981 0.07066702 -0.05347919 0.04723185 0.07058697 -0.01996994 -0.06732672 0.03131484 -0.06408047 0.03072667 0.06486499 -0.06837928 -0.01711523 0.048424 -0.07053101 0.001212537 0.05480599 -0.02998805 -0.0635153 0.03259295 -0.0307787 -0.06353402 0.03218394 -0.0702731 0.008181631 0.05713587 -0.06919419 -0.01250427 0.05042284 -0.07012325 -0.007478356 0.05177599 -0.0489521 0.05087345 0.072142 -0.05097818 0.04856175 0.07184898 -0.06283217 0.03172034 0.06588602 -0.04003727 0.05793607 0.07505601 -0.05978697 0.03847283 0.06755 -0.04589092 -0.05366837 0.03560888 -0.04204416 -0.05627501 0.03512299 -0.0707733 0.008072376 0.05701297 -0.06927239 0.01228195 0.059089 -0.04572916 -0.05333328 0.03615099 -0.0618571 0.0342459 0.06627202 -0.06022626 0.03644174 0.06750297 -0.05063056 -0.04887819 0.03744399 -0.06521248 -0.02625095 0.04561787 -0.07074069 -0.006801724 0.05185484 -0.002304494 -0.0698139 0.03537184 -0.003287792 -0.06873458 0.04824692 -0.06706756 0.008258342 0.07667797 -0.068502 0.007277846 0.07328099 -0.02436828 8.42628e-4 0.07780897 -0.02428203 7.55112e-4 0.07279497 -0.06527006 0.0074597 0.07760697 -0.05798035 0.001408219 0.07869201 -0.05844444 9.15565e-4 0.078166 -0.05850476 8.63091e-4 0.07799297 -0.05768835 0.001755297 0.07878696 0.02428203 7.55112e-4 0.07279497 0.06036067 0.01300382 0.07977098 0.06026959 0.004976153 0.07855802 0.05944186 0.00384289 0.07865101 0.05800396 0.002866864 0.07888501 0.05768835 0.001755297 0.07878696 0.05794787 0.001445651 0.078709 0.05886286 0.002390146 0.07797098 0.05850267 8.64854e-4 0.07800096 0.0584377 9.22017e-4 0.07818198 0.06533509 0.0074476 0.07744801 0.06349807 0.006526589 0.072896 0.06324285 0.007048547 0.07835501 0.06277459 0.00750941 0.07905101 0.06224155 0.006571114 0.07837498 0.003347754 -0.0684604 0.05263882 0.00238651 -0.06974524 0.03623795 0.003333628 -0.06869709 0.04870086 0.004218757 -0.06911689 0.04277098 0.001707017 -0.07021313 0.03022485 0.0560742 0.04372304 0.06936997 0.06590718 0.02466499 0.06347399 0.06845879 0.01722365 0.06038302 0.06916725 -0.01273488 0.05031085 0.06022346 -0.03622442 0.04213088 0.01394456 -0.06913399 0.03024196 0.05726957 -0.0409286 0.04019099 0.05416417 -0.04476165 0.0391469 0.008309483 -0.07001781 0.02994996 0.05459296 -0.045358 0.03851985 0.05664336 0.04224634 0.069067 0.07030647 -0.001961827 0.05412298 0.04481375 0.05528903 0.07337999 0.06552577 -0.02682667 0.04491299 0.06086719 0.03535187 0.067155 0.06030184 0.03765505 0.067267 0.05260586 -0.04673635 0.03816586 0.0656706 -0.02554368 0.04551982 0.04640448 -0.05322951 0.03576195 0.06099396 0.03556466 0.06676 0.06975769 -0.0105769 0.05070096 0.04375976 -0.05499488 0.035362 0.05164027 0.04821264 0.07113599 0.06508797 0.02859652 0.06413102 0.07016396 0.007576406 0.05697995 0.04931199 0.05028033 0.07225501 0.03914552 -0.05832386 0.03440696 0.06274801 -0.03181511 0.04340291 0.03254967 -0.0626406 0.03249597 0.03014045 -0.063443 0.03261786 0.06482815 0.02791494 0.06405997 0.02541416 -0.06547641 0.03190696 0.02051079 -0.06716972 0.03131598 0.03982788 0.05764073 0.07962799 0.04114705 0.05610853 0.08275598 0.06755119 0.01470184 0.075576 0.06646126 0.01931941 0.07630997 0.06489247 0.02431654 0.07710701 0.06295496 0.02915894 0.07788199 0.06070739 0.0324772 0.07948398 0.06089133 0.03339534 0.07855999 0.04897928 0.04944533 0.081586 0.04751324 -0.04853838 0.06609201 0.0496785 -0.04651451 0.06605499 0.01288616 -0.06643152 0.06316798 -0.001597881 -0.06777393 0.06152999 0.05273067 -0.0236625 0.07651799 0.05076158 -0.03263223 0.07501798 0.05243366 -0.0390731 0.07118397 0.04309415 -0.03850018 0.07497096 0.04049396 -0.0479778 0.071801 0.05111265 -0.04482078 0.06668496 0.03733527 -0.04971003 0.07198196 0.05450797 -0.04072958 0.067339 0.04176914 -0.03737223 0.07531398 0.04129189 -0.04271799 0.07407498 0.03929322 -0.04664522 0.07294297 0.03808099 -0.04528468 0.07383495 0.03676718 -0.04379463 0.07449597 0.04977315 -0.0439527 0.06929397 0.05089432 -0.038001 0.07279199 0.06662058 -0.008423209 0.07441997 0.05269593 -0.03377705 0.07391899 0.05098909 -0.02866208 0.07591301 0.06437951 -0.007865607 0.07629799 0.06301027 -0.01191598 0.07611995 0.038872 -0.03820091 0.07529199 0.04781872 -0.03085172 0.07588696 0.04983407 -0.03744471 0.07355099 0.06242913 -0.007436871 0.077335 0.03472036 -0.04300791 0.07481801 0.03141808 -0.04803282 0.07396799 0.02820014 -0.04539036 0.07466298 0.06560379 -0.0148254 0.073197 0.06450998 -0.01039206 0.07558995 0.06529206 -0.01998031 0.070894 0.04997438 -0.03215992 0.07533895 0.04645055 -0.0482456 0.06781196 0.04725188 -0.04197216 0.07241898 0.0448693 -0.04674828 0.07052999 0.06015592 -0.005696654 0.07808399 0.0639345 -0.0241068 0.06994301 0.04637587 -0.0412479 0.07316899 0.04454916 -0.03972786 0.07435399 0.06261986 -0.01673132 0.07499498 0.06140434 -0.01400262 0.07640397 0.04848527 -0.04282051 0.07119697 0.04401206 -0.05160778 0.06577396 0.05784696 -0.02068758 0.07613301 0.06051164 -0.01383441 0.07677501 0.04610085 -0.0351088 0.07531696 0.06190419 -0.02877968 0.06914496 0.06304675 -0.01993513 0.07354497 0.06226557 -0.02495509 0.07167398 0.05950433 -0.01168769 0.07739597 0.05996096 -0.03087693 0.070194 0.06110537 -0.0216872 0.07424199 0.06108075 -0.01810431 0.075495 0.05763757 -0.01721918 0.076882 0.05695647 -0.01506239 0.07727497 0.05970156 -0.0264486 0.07316398 0.05877137 -0.03028512 0.07186895 0.05856275 -0.03495568 0.068192 0.05343556 -0.02023077 0.076864 0.05803889 -0.0238077 0.07519197 0.04303514 -0.0444107 0.07285195 0.05775517 -0.02760672 0.073933 0.04222667 -0.0502848 0.06911098 0.05483305 -0.03065073 0.074234 0.05504524 -0.03520768 0.07183498 0.03929775 -0.05535811 0.06470996 0.05444502 -0.02632743 0.07573801 0.03966188 -0.05272507 0.068349 0.05575746 -0.02805858 0.07476896 0.04351335 -0.04550391 0.07207298 0.03598368 -0.0575093 0.064534 0.05377936 -0.04013431 0.06899595 0.05634027 -0.0314027 0.07304596 0.05673331 -0.03615021 0.06947499 0.05487239 -0.02278733 0.07639998 0.02436035 -8.37075e-4 0.07748699 0.06180983 -0.006136298 0.07704597 0.06448155 -0.007118642 0.07558298 0.05806398 -0.001496255 0.07840901 0.05822312 -0.001296937 0.07831096 0.05852687 -9.01162e-4 0.07782196 0.05855578 -8.53379e-4 0.07764995 0.05879926 -0.002025306 0.07752901 0.05915415 -0.003077626 0.07774698 0.05911082 -0.00498265 0.07819801 0.03413355 -0.05958729 0.05637097 0.03395915 -0.05970817 0.05324995 0.03269857 -0.05996638 0.058685 0.03203326 -0.05982643 0.06669497 0.03193277 -0.05108052 0.066199 0.03684175 -0.05574578 0.06621098 0.03655827 0.0551024 0.07345902 0.03166466 0.06296002 0.07639497 0.02906703 0.05826663 0.07472801 0.02899748 0.05715572 0.07957303 0.03144323 0.05333983 0.07270199 0.03065896 0.05377304 0.07308101 0.02990508 0.05445241 0.07333397 0.02900236 0.05625265 0.07400298 0.03472995 0.05342108 0.07295 0.03388488 0.0531333 0.07276099 0.03300005 0.05303359 0.07260102 -0.05925285 0.04416602 0.07277697 -0.06342285 -0.03685235 0.04491895 -0.06577408 -0.03254359 0.04640001 3.64303e-4 -0.07311153 0.03261995 -0.0611937 0.04107946 0.07221597 -0.06351768 -0.03644549 0.04544496 -0.06062787 -0.04103797 0.04383695 -0.06439548 0.03577202 0.070432 -0.0598579 0.0419079 0.07274103 -0.06299 -0.03594261 0.04575186 -0.06054657 -0.04135102 0.04337197 -0.0646227 0.03575474 0.06988501 -0.06680905 0.03103888 0.068753 -0.06864941 0.02654659 0.06727802 -0.05724477 -0.04500305 0.042611 -0.005662977 0.07366871 0.08349502 -0.06546837 0.03262925 0.06952399 -0.05398237 -0.04944258 0.04089695 -0.07037615 0.02172362 0.06548702 -0.07004028 0.02095299 0.06546801 -0.04459887 -0.05801761 0.03794986 -0.01243007 0.07276493 0.08324599 -0.07151865 0.01729536 0.06399196 -0.01497966 0.07257062 0.08257001 -0.07244288 0.01266205 0.06240797 -0.03786545 -0.06211102 0.03668087 -0.02419668 0.07002443 0.08170801 -0.07165747 0.0142697 0.06316 -0.07314079 0.007460892 0.06058585 -0.03341126 -0.06502759 0.03551083 -0.03301805 0.06606775 0.08083599 -0.0286867 0.06806552 0.08154797 -0.07295817 8.63965e-4 0.05851197 -0.07344508 0.002261221 0.05881196 -0.03803026 -0.06244963 0.036394 -0.03262084 0.06567794 0.08096998 -0.03089725 -0.06584763 0.03537982 -0.02862465 -0.06725072 0.03473883 -0.03963094 0.06122416 0.07943797 -0.03742265 0.06365096 0.08004498 -0.07335907 -0.003835976 0.05672198 -0.07212108 -0.0136103 0.05332183 -0.02369868 -0.06918179 0.03400588 -0.01851117 -0.06962132 0.03407686 -0.01939725 -0.07036912 0.03376394 -0.04558235 0.05830204 0.07763701 -0.04162967 0.06096023 0.079117 -0.04585731 0.05785197 0.07801198 -0.07099068 -0.01856517 0.05158096 -0.04926908 0.05519711 0.07656997 -0.07146346 -0.01441711 0.05321484 -0.007416367 -0.07265138 0.03301888 -0.04606318 0.05690312 0.07793897 -0.0494796 0.0547508 0.07696199 -0.0686143 -0.02449488 0.049721 -0.05279743 0.05153059 0.07581096 -0.06950449 -0.02335608 0.04995596 -0.05311256 0.05045247 0.07569301 -0.06552219 -0.03189963 0.04715389 -0.05653023 0.04732781 0.074386 0.05888921 -0.04349237 0.043042 0.05129778 -0.0516473 0.04030787 -4.48201e-4 0.07400095 0.08343201 -0.001973152 0.07352572 0.0837 0.06518948 0.03432852 0.069902 0.06663477 0.03003072 0.06862401 0.06282019 0.03886735 0.07095497 0.05657589 -0.04528999 0.04251194 0.06233906 -0.03863561 0.04430484 0.05958014 -0.04271787 0.04290199 0.0627225 0.03879946 0.07139301 0.06424379 -0.03524202 0.04585397 0.05971574 0.04317903 0.073044 0.05660438 0.04726654 0.07437801 0.05380713 0.04965674 0.07542699 0.005347907 -0.07290703 0.03272783 0.05299848 0.05133771 0.07575398 0.05035698 0.05391263 0.07667797 0.06848996 -0.02615183 0.04903692 0.07041555 -0.02096468 0.050502 0.05004185 0.05448889 0.07632601 0.04540759 0.05702817 0.077982 0.04633319 0.05745083 0.07787096 0.0143103 -0.07160371 0.03328382 0.01487505 -0.07158398 0.03297597 0.05022269 0.05351263 0.076743 0.04602605 0.05794775 0.077515 0.04174429 0.06112819 0.07860898 0.0712068 -0.01562398 0.05279588 0.01217669 -0.0715543 0.03340399 0.07173418 -0.01599681 0.05212897 0.0422157 0.06015563 0.07901901 0.04131138 0.06122446 0.07913398 0.01954549 -0.07046449 0.03336083 0.07329869 -0.006250202 0.05544185 0.07272189 -0.01092988 0.05385297 0.0716263 -0.0157637 0.05267685 0.07317548 -0.006166458 0.055893 0.03340107 0.06585913 0.08079999 0.0374493 0.06368052 0.079988 0.02407175 -0.06906181 0.03384298 0.02038085 -0.07015401 0.03380584 0.07293391 -0.003042519 0.05715799 0.07351189 -0.00128144 0.05740588 0.02825367 0.06847625 0.08113998 0.02870208 0.06804883 0.081559 0.03448039 -0.06444269 0.03578597 0.07353365 0.004165172 0.05903095 0.02381676 0.06991082 0.08221298 0.02333176 0.07031714 0.08177399 0.07340627 0.00243026 0.05887097 0.04196965 -0.05995792 0.03714895 0.07294458 0.009251058 0.061221 0.01908099 0.07077693 0.082749 0.01801186 0.07171773 0.08274602 0.03736335 -0.06235051 0.03659486 0.0722717 0.01447737 0.062716 0.07095187 0.01954853 0.06478202 0.01312112 0.07293134 0.08266699 0.04736566 -0.05475038 0.03923183 0.06937366 0.02467036 0.06655198 0.01288205 0.07279747 0.08316999 0.007917821 0.07368654 0.08292603 0.005499601 0.07348197 0.08362299 0.07051515 0.01872682 0.06470501 0.05319607 -0.05025053 0.04064583 0.006269395 0.07377052 0.08340203 0.06749838 0.02946704 0.06821501 0.00640738 0.07299804 0.08351904 -0.03873497 0.05892193 0.07485198 -0.03594803 0.05429202 0.07327401 -0.03399986 0.05327802 0.07953697 -0.03050446 0.05388486 0.07307702 -0.03211534 0.05313277 0.07272702 -0.03126966 0.05342108 0.07295 -0.02902323 0.05809915 0.07468998 -0.02893596 0.05661976 0.07406902 -0.02902954 0.0561034 0.07374501 -0.02936875 0.0552408 0.07362699 -0.02985179 0.05450552 0.07310599 -0.02946496 0.05920392 0.07489198 -0.03535956 0.0617839 0.07563197 -0.03668117 0.06011831 0.07581698 -0.03521066 0.06126672 0.07568901 -0.03487586 0.06118607 0.076191 -0.03405398 -0.05944859 0.06679898 -0.03506517 -0.05868548 0.06682097 -0.03535103 -0.05871069 0.05550098 -0.0354014 -0.05844771 0.06169599 -0.03430539 -0.05945581 0.05983895 -0.03409898 -0.05962181 0.05505287 -0.03391087 -0.05153912 0.06620401 -0.0283401 -0.05332082 0.06620198 -0.03101247 -0.0513677 0.06620401 -0.06179577 -0.005650341 0.07351702 -0.05886393 -0.002242565 0.077524 -0.06045866 -0.004303038 0.07324898 -0.05868339 -8.18208e-4 0.076406 -0.06670427 -0.007466316 0.07462 -0.0679115 -0.01038688 0.07161301 -0.05635964 -0.041956 0.04021996 -0.05920022 -0.03425037 0.066446 -0.06017577 -0.03233218 0.06822097 -0.06989365 -0.007762849 0.05208086 -0.06569409 0.02222144 0.07564401 -0.06788218 0.01287055 0.075562 -0.06653797 0.02297621 0.06273096 -0.03601509 -0.05745482 0.06466901 -0.002756059 -0.06939989 0.0401389 -0.06353169 -0.02552092 0.06889599 -0.06709879 0.01722085 0.07472497 -0.001714468 -0.0702129 0.03021496 -0.03694146 -0.0569694 0.06396901 -0.03823983 -0.05893248 0.03405886 -0.04144096 -0.05671077 0.03505897 -0.06144464 -0.03019911 0.06712496 -0.01189714 -0.06923043 0.03043586 -0.06475305 0.02761673 0.06433999 -0.06604969 -0.01838618 0.07036697 -0.01522219 -0.0685544 0.03086286 -0.03585726 -0.060229 0.03551197 -0.06996536 -0.00701797 0.05244696 -0.0702399 -0.003467917 0.05366587 -0.05913329 -0.03794652 0.04169785 -0.01468867 -0.06607651 0.06306898 -0.06203711 0.03134322 0.07706499 -0.04070276 -0.05441319 0.064282 -0.01183974 -0.06666803 0.06257498 -0.004808962 -0.07006961 0.03023296 -0.06188052 -0.03330677 0.04326498 -0.06533956 0.02302086 0.07690101 -0.06236463 0.03261417 0.066302 -0.06394058 0.02705997 0.076442 -0.06455487 -0.02286189 0.06967699 -0.06520056 -0.0212059 0.069301 -0.04430615 -0.05160868 0.06455695 -0.04386121 -0.05190372 0.06523597 -0.04530304 -0.05367881 0.03617686 -0.0636475 0.02751052 0.07771801 -0.03306365 -0.0599845 0.05550897 -0.05972379 0.0372262 0.06800699 -0.07032215 0.001454234 0.05534487 -0.01624667 -0.06576138 0.06259 -0.05956876 0.03593653 0.077932 -0.02458196 -0.0657854 0.031883 -0.06656539 -0.0167343 0.07003098 -0.06723916 -0.01380068 0.07107996 -0.06079965 -0.03525239 0.04247283 -0.05891609 0.03681272 0.07946097 -0.05419856 0.04493963 0.07050698 -0.01927548 -0.0649119 0.06324499 -0.07004266 0.006405055 0.05717384 -0.07028818 0.00275439 0.05575788 -0.02057915 -0.06453931 0.06305599 -0.02869516 -0.06409829 0.03250485 -0.0538975 -0.04183739 0.06562501 -0.05723387 0.04100453 0.06911998 -0.06624698 -0.02351522 0.04662698 -0.05090993 -0.04532361 0.06517297 -0.06897258 -0.002420008 0.07068496 -0.04895263 0.05009704 0.07665902 -0.03348028 -0.0617851 0.03301888 -0.06983059 0.008306503 0.05801486 -0.0416041 0.05605894 0.08247202 -0.04301989 0.05576217 0.07432198 -0.02373486 -0.06344801 0.06347298 -0.02482879 -0.06305253 0.06334596 -0.06771296 -0.0189011 0.04824286 -0.05244916 -0.0467568 0.03844994 -0.05470228 -0.04066181 0.06694298 -0.05296808 -0.04615211 0.0388 -0.06902128 0.0135824 0.05966198 -0.06886661 0.006331682 0.07214701 -0.04009288 -0.05479788 0.06479501 -0.05666553 -0.03813058 0.06613695 -0.0681613 -0.0172472 0.04876583 -0.06854808 0.008066713 0.07457798 -0.06901538 0.002060294 0.072209 -0.007380962 -0.06728762 0.06260401 -0.05744946 -0.03680688 0.06753295 -0.06780701 0.01875746 0.06147986 -0.0688619 -0.0141834 0.04989397 -0.02899616 -0.06134527 0.06290197 -0.06866848 -0.00245887 0.072537 -0.05873209 8.09944e-4 0.07593601 -0.05889475 0.002816021 0.07853198 -0.05950933 0.002643585 0.07317996 -0.05998724 0.004646599 0.07852697 -0.06153947 0.006073236 0.07778596 -0.0635392 0.006585717 0.07337498 0.06181818 0.005654335 0.07328701 0.05878067 8.01773e-4 0.075468 0.03788727 -0.05638182 0.06381297 0.06716436 0.01705223 0.074382 0.06476849 -0.02732378 0.04524296 0.06431239 0.02602905 0.07670801 0.06875109 0.01492697 0.06005901 0.01557815 -0.06847721 0.03085899 0.05283915 -0.04298901 0.06659197 0.03981757 0.05813884 0.074741 0.05574756 -0.03925943 0.06716197 0.0659312 0.02159792 0.07504701 0.04275763 0.05595463 0.07435697 0.01093137 -0.0693984 0.03052997 0.06937819 0.01162284 0.05894196 0.06921499 0.01260399 0.059201 0.06870007 0.007341504 0.07368898 0.06989139 0.008091509 0.05753797 0.04101419 0.05676394 0.07966899 0.04567295 0.05362284 0.073394 0.03265863 -0.0621286 0.03369385 0.02454328 -0.06316828 0.06325298 0.0649783 -0.02169382 0.06985598 0.006380081 -0.06993609 0.03034991 0.0702694 0.003004491 0.0559569 0.04533571 0.05300199 0.08201402 0.04199177 0.0557748 0.082107 0.07003217 0.00640434 0.05730599 0.04917293 -0.04714179 0.06501501 0.0525614 0.04685163 0.07117098 0.06812208 0.01144921 0.07506 0.04884636 0.04975384 0.08114403 0.05738157 -0.04056692 0.040609 0.07031017 0.00209552 0.05552697 0.06481438 -0.02227759 0.069233 0.0544697 -0.04437959 0.03936284 0.03177666 -0.05988901 0.06418997 0.02914369 -0.0612756 0.062927 0.06036597 -0.03597259 0.04230797 0.06992858 -0.007447898 0.05219084 0.05834227 0.03938943 0.06856799 0.05575436 0.04300862 0.06977802 0.05125606 -0.04805845 0.03800696 0.027224 -0.06204253 0.06386297 0.06319338 -0.02625769 0.06911796 0.06899535 0.002987027 0.07214701 0.06118327 -0.03054988 0.06797099 0.02829688 -0.0642752 0.03243982 0.04631078 -0.04978561 0.06555998 0.05901426 0.03836309 0.068268 0.06755226 -0.01224488 0.07155901 0.05801635 0.03836894 0.079207 0.01983499 -0.06476569 0.06303596 0.04781454 -0.05146908 0.03687596 0.04549205 -0.05058276 0.06480598 0.06901919 0.002094507 0.07192999 0.05068755 -0.04865801 0.03778588 0.06789606 -0.01858592 0.04802501 0.06362229 0.03014135 0.06531602 0.01504099 -0.0660417 0.06255 0.01795089 -0.06527221 0.063304 0.02377587 -0.0660808 0.03177487 0.0663948 -0.01718086 0.07055896 0.05866599 -0.03490316 0.06782799 0.06897729 -0.002585053 0.070486 0.0106073 -0.06686842 0.06258296 0.04757946 -0.05177599 0.03646183 0.004330754 -0.06750643 0.063066 0.066468 -0.02289849 0.04678994 0.03475672 -0.06103891 0.03345888 0.006157398 -0.0674259 0.06237083 0.06010615 0.03493696 0.07816803 0.04218786 -0.05325031 0.06500297 0.05259007 -0.04340046 0.06558996 0.05859196 -0.0351867 0.06669497 0.06495487 -0.02685928 0.045497 0.06247848 0.030326 0.07740998 0.06757408 0.01960897 0.06169599 0.01864236 -0.06770563 0.03117597 0.06553316 0.007022202 0.07323098 0.06841385 -0.006901741 0.07220995 0.06016129 -0.004786014 0.07760596 0.06293636 -0.00667721 0.07625597 0.03410625 -0.05942606 0.06620901 0.03542125 -0.05838871 0.06266099 0.03411269 -0.05950909 0.06056696 0.03497755 -0.05892312 0.06032395 0.03556984 -0.05839878 0.05751883 0.02773028 -0.05461043 0.066199 0.03516274 -0.05221599 0.06620496 0.02916383 -0.0524317 0.06620401 0.03869032 0.05886006 0.07529103 0.03560036 0.0607813 0.07587999 0.0345723 0.0611478 0.078462 0.03549748 0.05388534 0.07305401 0.03609508 0.05444425 0.073089 0.03403645 0.05328881 0.079405 -0.04666697 -0.05642426 0.03818899 -0.06213074 0.0400756 0.06811702 -0.06105095 0.04161882 0.07183301 -0.05617123 0.04806864 0.07411897 -0.04670566 -0.056472 0.03467798 -0.05929857 0.04419922 0.06954503 -0.05000746 -0.05347287 0.03945994 -0.05786848 0.04598754 0.07334703 -0.05621445 0.04810464 0.07089799 -0.05791229 0.04602164 0.070167 -0.05439335 0.05009675 0.07477098 -0.05443453 0.05013382 0.07159298 -0.05029988 -0.0533263 0.03576797 -0.05070906 0.05386537 0.07607698 -0.05277454 0.05189955 0.07221198 -0.05394726 -0.04957598 0.04054397 -0.04930675 0.05523866 0.07336896 -0.04675239 0.05736202 0.07728898 -0.05709558 -0.04583048 0.04215097 -0.04428344 0.05931299 0.07637298 -0.05399245 -0.04961687 0.03705185 -0.05153137 -0.05206358 0.03962188 -0.04167056 0.06117737 0.07862597 -0.05488497 -0.04854691 0.04084098 -0.05799525 -0.044824 0.04213088 -0.0385161 0.06329196 0.07615399 -0.04170233 0.06122297 0.075441 -0.0371865 0.06408923 0.07643401 -0.05741286 -0.04565566 0.03842395 -0.05325186 -0.0503472 0.03675884 -0.03715837 0.06404161 0.079611 -0.03388339 0.06584954 0.08023101 -0.03298556 0.06636965 0.07722401 -0.06086724 -0.04088419 0.04349684 -0.02912628 0.06810677 0.08099102 -0.02421897 0.07007604 0.07850599 -0.06059622 -0.04138571 0.03990298 -0.01975315 0.07116723 0.082681 -0.01965856 0.07149773 0.07899898 -0.01025992 0.07339167 0.08283102 -0.06347507 -0.03688275 0.04146283 -3.81833e-4 0.07412171 0.08308804 -0.06373256 -0.03632313 0.04507786 -0.005310237 0.07392054 0.08299803 -6.49424e-4 0.07416623 0.07992398 -0.06603628 -0.03208053 0.04483687 -0.06738388 -0.02887308 0.04803586 -0.003715515 -0.07290589 0.03293395 3.85099e-4 -0.07317709 0.02884495 -0.06790715 -0.02805972 0.04451888 -0.003673851 -0.07308852 0.02887499 -0.06965756 -0.02328526 0.04958397 -0.006598532 -0.07287502 0.02894896 -0.06971389 -0.02330487 0.04616487 -0.06846857 -0.02668762 0.04496085 -0.06841456 -0.02666568 0.04829901 -0.0700671 -0.02205377 0.04989784 -0.07115739 -0.01832038 0.05129086 -0.008300244 -0.07270008 0.02905786 -0.01133579 -0.07222861 0.03263199 -0.07121455 -0.0183357 0.04788595 -0.0135203 -0.07184529 0.03306895 -0.07310646 -0.008065104 0.05481797 -0.07153785 -0.01682412 0.05170989 -0.01606035 -0.07133078 0.03294295 -0.0724377 -0.01265048 0.05319088 -0.07236796 -0.01325505 0.04964596 -0.01352596 -0.07191252 0.02932995 -0.07316499 -0.008072316 0.05144101 -0.01864266 -0.07070261 0.03328198 -0.02117109 -0.06999582 0.0334059 -0.01866048 -0.07076209 0.02972882 -0.07325708 -0.006683111 0.05522495 -0.07359158 -0.002843201 0.05325198 -0.07357418 -0.001799166 0.05691796 -0.02118825 -0.07005411 0.02992695 -0.07354485 0.003609538 0.05879288 -0.07364356 0.002414703 0.05507296 -0.0261538 -0.06830573 0.03399199 -0.02369117 -0.06925433 0.03025084 -0.0333814 -0.0651009 0.03520482 -0.07329297 0.008045852 0.05700188 -0.07317638 0.008465409 0.060476 -0.02862125 -0.06738179 0.0309 -0.07156819 0.01770162 0.06367796 -0.03101718 -0.06625443 0.03470283 -0.07255077 0.01308727 0.06042397 -0.02860885 -0.0673362 0.03086882 -0.07004988 0.02329725 0.06228888 -0.07253819 0.01333034 0.05883389 -0.03530329 -0.06408739 0.03545397 -0.07135117 0.01859116 0.06385099 -0.07162439 0.01771461 0.06037199 -0.03340905 -0.06515592 0.03166997 -0.0704869 0.02190536 0.06182396 -0.07024139 0.02256882 0.06528598 -0.06855088 0.02736371 0.06696099 -0.03805178 -0.06257504 0.03256398 -0.03741997 -0.06290721 0.03240483 -0.03947526 -0.06162232 0.03630799 -0.06879979 0.02683413 0.06353098 -0.06683486 0.03135561 0.06837201 -0.04247152 -0.05961287 0.03709197 -0.06688678 0.03137934 0.06510502 -0.06616026 0.03278213 0.06876999 -0.04250776 -0.05966275 0.03357297 -0.0438748 -0.05860072 0.03735584 -0.0646727 0.03578174 0.06663 -0.06208276 0.04004532 0.07135999 -0.06364768 0.03748506 0.07039999 -0.03646677 0.05515587 0.07951098 -0.03155726 0.06259846 0.079557 -0.03535366 0.06061393 0.07945102 -0.03281915 -0.0598281 0.06670498 -0.0326727 -0.05753773 0.06721895 -0.0292809 -0.05824059 0.06719994 -0.02963536 -0.05301553 0.067182 -0.03247255 -0.05416476 0.06719994 -0.03348416 -0.05462414 0.06719994 -0.03474295 -0.05272191 0.067142 -0.03143256 -0.05739051 0.06719994 -0.06353616 -0.006575584 0.073183 -0.05954664 -7.55178e-4 0.07279497 -0.05900037 -7.64957e-4 0.073354 -0.06316268 -0.005896747 0.07270097 -0.0595144 -0.002619326 0.07306098 -0.06773567 -0.007043659 0.07272398 -0.068304 -0.001652598 0.07269996 -0.0687651 0.001892447 0.072555 -0.06838738 -0.007182121 0.07203799 -0.06878995 -0.003354072 0.07214701 -0.06826967 0.006294131 0.07269996 -0.06529086 0.006959855 0.07305401 -0.05954664 7.55112e-4 0.07279497 -0.05900609 7.63761e-4 0.07328999 -0.06041514 0.004325628 0.07356601 -0.06181144 0.005626082 0.07310199 0.05900305 7.64439e-4 0.07332897 0.05949831 0.002623081 0.07310396 0.06045895 0.004297435 0.07311499 0.06826967 0.006294131 0.07269996 0.06882816 0.0020594 0.07249999 0.06839877 0.001189649 0.07269996 0.06889027 -0.001881182 0.07214701 0.06871438 -0.00142467 0.07257497 0.0634886 -0.006518304 0.07297599 0.059237 -7.60316e-4 0.073089 0.06182736 -0.005649924 0.07323098 0.06045675 -0.004249215 0.07290697 0.05946767 -0.002617597 0.07331097 0.05890417 -6.77794e-4 0.07259899 0.03506869 -0.05870622 0.066693 0.03302335 -0.05979979 0.06417995 0.02955543 -0.05840408 0.067191 0.03646856 0.05515491 0.079405 0.03833097 0.05871862 0.07927101 0.03130519 0.0569055 0.079701 0.03818768 0.05834794 0.07969999 0.03168338 0.06265914 0.079553 0.02933257 0.05880653 0.07954996 0.02913737 0.05611616 0.07969897 0.0293408 0.05529606 0.07364702 0.03099739 0.05368906 0.079405 0.03017759 0.05432134 0.07969999 0.03498435 0.05368143 0.07969897 0.03582584 0.05432289 0.07958 0.03678762 -0.06326961 0.03227895 0.03892374 0.06304317 0.07606798 0.06346577 0.03790354 0.06735301 0.02730095 -0.06787562 0.03068184 0.02558398 0.06958341 0.07833498 0.01803714 -0.07088613 0.02963799 0.02056998 0.07123643 0.07890802 0.01559215 0.07341396 0.07966303 0.07116878 0.01951396 0.06097799 0.01050657 -0.07241171 0.02910995 0.01070433 0.07338482 0.07965302 0.005310118 -0.07297551 0.02891397 0.003537178 -0.07511204 0.02817291 0.07328218 0.01536196 0.05953788 -0.07347357 -0.004245281 0.05274099 0.07366478 -3.98401e-4 0.054075 0.07493746 -0.001172423 0.05380588 -0.07258868 -0.01204228 0.05003798 -0.01134544 -0.07228851 0.02915197 -0.01607376 -0.07139003 0.02946382 -0.02469193 -0.07073611 0.02968984 -0.07085847 -0.01945513 0.04746782 -0.06504219 -0.03398519 0.04243087 0.06648439 -0.03123432 0.04338485 -0.02864503 0.06837165 0.07791697 -0.05948019 -0.04289013 0.03934395 -0.04804527 -0.05534911 0.035025 -0.04298245 0.06032544 0.07512599 -0.04678767 0.05740463 0.07411301 -0.05074757 0.05390524 0.07289999 -0.0610972 0.04164958 0.06865102 -0.03582668 0.05432724 0.07969897 -0.03515964 0.05365329 0.07303696 -0.03299742 0.05315834 0.07969897 -0.03100329 0.05368643 0.07950001 -0.03017938 0.05431944 0.07969897 -0.0295313 0.05515491 0.079405 -0.02914005 0.05609816 0.07969897 -0.02905678 0.05784446 0.079593 -0.02934575 0.05882889 0.07951998 -0.03156429 0.06208121 0.07969999 -0.0383625 0.05862259 0.07969897 -0.03055888 -0.05546557 0.06719994 -0.03358727 -0.05693 0.06720596 -0.03137707 -0.05434691 0.06719994 -0.03068315 -0.05655092 0.06719994 -0.06840336 0.002317309 0.07269996 -0.06738299 0.001010537 0.07269996 -0.06560707 -0.001691043 0.07269996 -0.06277418 0.005145311 0.07269996 -0.06669259 0.001566529 0.07269996 -0.06769359 2.23163e-4 0.07269996 -0.06759756 -6.17709e-4 0.07269996 -0.06554818 0.001685917 0.07269996 -0.06691807 -0.001473546 0.07269996 0.06773519 5.81907e-5 0.07269996 0.0657314 -0.001691997 0.07269996 0.06047075 0.001331806 0.07269996 0.0666179 -0.001597464 0.07269996 0.06508189 0.001473546 0.07269996 0.06806397 -0.007725775 0.07269799 0.06430637 -2.23229e-4 0.07269996 0.06618338 0.001702964 0.07269996 0.0317983 -0.05417239 0.06719994 0.03086769 -0.05484682 0.06720197 0.03603595 -0.05513674 0.06719899 0.03304433 -0.05911803 0.06719797 0.03398507 -0.05591666 0.06719994 0.03118038 -0.05725282 0.06719994 0.0332638 0.05885004 0.07969999 0.03407478 0.05849123 0.07969999 0.0316661 0.05608296 0.07969999 0.03459745 0.05777519 0.07969999 0.03238219 0.05875509 0.07969999 0.03361797 0.05556005 0.07969999 -0.07472729 0.001379132 0.05469089 -0.07392859 0.01350224 0.05895888 -0.07150268 0.02324867 0.06247401 -0.06975901 0.02805626 0.06413698 -0.0708 0.02384459 0.06247884 -0.05606329 -0.050107 0.03702396 -0.05949467 -0.04596287 0.03856295 -0.06532698 -0.0371344 0.04141396 -0.0626409 0.04121172 0.06849902 -0.06533998 -0.03719198 0.04157584 -0.06250625 0.04178273 0.06888198 -0.06994718 -0.02757591 0.04490095 -0.06780797 -0.03248178 0.04320496 -0.06605195 -0.03542912 0.04192996 -0.05562537 0.05015903 0.07160198 -0.07262086 -0.01811438 0.04793298 -0.05637043 0.04975593 0.071639 -0.007749915 -0.07466393 0.02835798 -0.05301928 0.05331653 0.07287001 -0.07318788 -0.017021 0.04837387 4.67523e-5 -0.07519972 0.02814286 -0.04566437 0.05973273 0.07508802 -0.01755923 -0.0730521 0.02893495 -0.07486969 -0.006803274 0.05198788 -0.03094726 -0.06851941 0.03075093 -0.04174858 0.06254267 0.07589501 -0.07513928 -0.001445353 0.053801 -0.03614073 -0.06558704 0.03147482 -0.03572607 -0.06615072 0.03157198 -0.01486295 0.07371336 0.07976698 -0.02433508 0.07114076 0.07893997 -0.05251908 -0.05340081 0.03569996 0.07280355 -0.01864326 0.04784095 0.01474934 -0.07369512 0.02881085 0.01984202 -0.07204854 0.02923583 0.04935687 0.05636084 0.07375097 0.07335197 -0.01488119 0.04905295 0.05261796 0.05371606 0.07295501 0.05727547 0.04846853 0.071015 0.06704556 -0.03351682 0.042593 0.06247419 -0.04182368 0.03997486 0.06734907 0.0323342 0.06542199 0.05769819 -0.0481612 0.03757697 0.07000911 0.02742582 0.063919 0.05862069 -0.04641032 0.03812396 0.00356853 0.07511717 0.08025401 0.05163633 -0.0546016 0.03533196 0.01225829 0.07418197 0.08008301 0.07424837 0.01183032 0.05852586 0.02263617 0.07170295 0.07911098 0.04338747 -0.06108707 0.03303498 0.07490247 0.006538152 0.05669701 0.0751729 0.001524925 0.05496299 0.03201115 0.06803685 0.07795798 0.02757698 0.06996154 0.07846701 0.0368247 -0.06520593 0.03160798 0.03839927 0.06447643 0.07656496 0.0743252 -0.01131027 0.05035597 0.01949328 -0.07261461 0.02925497 0.04573446 0.05967789 0.07506901 -0.03457826 0.05781102 0.07969999 -0.0319252 0.05849123 0.07969999 -0.03433376 0.05608296 0.07969999 -0.03140223 0.05777519 0.07969999 -0.03294163 0.05889272 0.07969999 -0.03404635 0.05852073 0.07969999 -0.02114129 0.04088515 0.08557397 -0.02622628 0.04100525 0.08549201 0.0133816 -0.01567542 0.08047497 0.001002073 -0.02206707 0.07991498 -0.004811942 -0.02696913 0.07948696 -0.01241546 -0.0199052 0.08010399 -0.004336297 0.02418613 0.08396202 -0.03110939 0.04086941 0.08542197 -0.03378176 -0.05667573 0.07069998 -0.03398507 -0.05591666 0.06719994 -0.03080677 -0.0568813 0.07069998 -0.02918255 -0.05309867 0.07161098 -0.03104794 -0.05188661 0.07177698 -0.03167665 -0.05251032 0.07084196 -0.03478479 -0.05266851 0.07150799 -0.03408825 -0.05302166 0.07083898 -0.03656125 -0.05397093 0.07289099 -0.03264266 -0.05416738 0.07069998 -0.02882742 -0.05567806 0.07077997 -0.02829569 -0.05473148 0.07162398 -0.06770366 -2.24532e-4 0.07789999 -0.06466621 -0.001074731 0.07269996 -0.06508189 -0.001473546 0.07789999 -0.0642963 -2.24532e-4 0.07269996 -0.06430637 2.23163e-4 0.07789999 -0.03454715 0.06650996 0.08219403 0.02780157 -0.06960743 0.03469598 0.07051628 0.02535653 0.067721 0.03266245 -0.0674616 0.03543794 0.06950539 0.0270062 0.06904703 0.06853818 0.03015786 0.06961202 -0.06954467 -0.02687478 0.05018395 -0.07142448 -0.02248251 0.05137795 0.06641536 0.03393805 0.07134401 0.03687822 -0.06511062 0.03661286 0.06378185 0.03937685 0.07244896 0.06385511 0.03852546 0.072968 -0.06756341 -0.03228241 0.04797101 0.04128068 -0.06244719 0.03749996 -0.06515038 -0.03703731 0.04621487 0.04582017 -0.05930632 0.03846585 0.04897075 -0.05623048 0.03994184 0.05405437 0.05192923 0.07689797 -0.06227195 -0.04158872 0.04473888 0.04975754 -0.05604571 0.03957498 -0.05589985 -0.0498082 0.04188996 0.05081999 0.05500823 0.07822299 -0.05926197 -0.04587239 0.04314684 0.04642623 0.05824637 0.07998698 -0.05230236 -0.05369198 0.04020196 -0.04835516 -0.05714899 0.03935384 0.05660265 -0.04912132 0.04199987 0.06254297 -0.04117769 0.04485398 -0.04406046 -0.06056416 0.03807896 -0.04958593 0.05615276 0.078538 0.0648694 -0.0373184 0.04625087 -0.05337476 0.05263555 0.077048 -0.05411016 0.05169713 0.07718503 0.06539636 -0.03662508 0.04611086 -0.05991476 0.04491645 0.07474398 -0.05988615 0.04507887 0.074427 -0.03584027 -0.06583309 0.03589898 -0.04030019 -0.06309908 0.037256 -0.03568583 -0.06548291 0.03671598 -0.06305021 0.04053699 0.072851 0.0686832 -0.02921003 0.04930597 -0.03097075 -0.0682491 0.035362 -0.02601987 -0.0699675 0.03510385 0.07104039 -0.02336812 0.05117899 -0.06563198 0.0360431 0.07166802 -0.06567257 0.03613924 0.071325 -0.02090787 -0.07194983 0.03404498 0.07149547 -0.02251487 0.05099684 -0.06596016 0.03456521 0.07178097 0.07228636 -0.01844 0.05309587 -0.07002669 0.02674669 0.06806802 0.07284438 -0.01745086 0.05301886 -0.01587235 -0.0732426 0.03363382 -0.06806337 0.03117257 0.07001399 -0.02120399 -0.07149261 0.03462696 -0.06999576 0.02660501 0.06839102 0.07385897 -0.012766 0.05437386 -0.01078945 -0.07378304 0.03383499 -0.0717318 0.02175503 0.066338 -0.07300931 0.01665329 0.06493097 -0.07303845 0.01684683 0.06463599 -0.01049548 -0.07421171 0.03310394 0.07485097 -0.002717018 0.05819696 -0.07401829 0.01150453 0.063039 8.81222e-5 -0.0749405 0.033077 -0.07467526 0.005535185 0.06113696 -0.074665 0.006631255 0.06109488 0.07415628 0.01067119 0.06270998 0.07312208 0.01542794 0.06470698 0.007257044 -0.0745877 0.03312093 0.07464575 0.005600273 0.06111294 0.07391315 0.009855329 0.06297498 0.07299208 0.01709264 0.06450897 0.01254338 -0.0738911 0.03321397 0.01773154 -0.07281523 0.03369885 0.07210397 0.0204795 0.06589698 -0.074602 -0.007282078 0.05627387 -0.07456499 -0.006769478 0.05687797 -0.0736131 -0.01171833 0.05551087 0.02281945 -0.0713877 0.03418684 0.06473201 -0.001185894 0.07269996 0.06736308 -0.00104624 0.07269996 0.03163188 -0.05745619 0.07069998 0.03264302 -0.05754971 0.06719994 0.03275537 -0.05751961 0.07069998 0.03358376 -0.05693328 0.06719994 0.03398507 -0.05580031 0.07069998 0.03161686 0.05614691 0.08419996 0.03255254 0.05548852 0.07969999 0.03469234 0.05689382 0.07969999 0.03433376 0.05608296 0.07969999 0.03192037 0.06784331 0.08203697 0.03467065 0.06645816 0.082134 0.0694608 -0.02819555 0.04888397 0.0680862 -0.0318892 0.04343599 0.06735205 -0.0329129 0.04724287 0.05086904 0.05536282 0.07368999 0.06505507 -0.03768628 0.04140585 0.05621278 0.04993581 0.07170099 0.05246275 0.05355745 0.07718902 0.06486529 -0.03757292 0.04562294 0.0542351 0.05207043 0.07254898 0.0574519 0.04814887 0.07549202 0.05955839 0.04588931 0.070302 0.05604708 0.04979264 0.07587999 0.06257897 -0.0412786 0.04421198 0.06229245 -0.04169797 0.04418885 0.05982714 -0.0455324 0.03869295 0.06094306 0.04402303 0.06975799 0.05938309 0.04575854 0.07447701 0.0607602 0.04389542 0.07401597 0.05972898 -0.04528176 0.04311388 0.06265765 0.04155564 0.06880402 0.06247365 0.0414375 0.07297497 0.0654515 0.03694725 0.06705999 0.06525766 0.03690242 0.07139801 0.066666 0.03476005 0.06654697 0.06790369 0.0322889 0.06559997 0.05674535 -0.04932278 0.03738182 0.0664677 0.03465753 0.07080996 0.06770521 0.03219842 0.06976199 0.06881618 0.03028476 0.06499499 0.07072597 0.02550375 0.06333696 0.05326366 -0.05272072 0.04077684 0.0698049 0.02734982 0.06807601 0.07178658 0.0223506 0.06216388 0.07157766 0.02228975 0.066316 0.07354605 0.01558995 0.05989897 0.07315987 0.01716035 0.06022799 0.0419901 -0.06209594 0.03709596 0.04582548 -0.05959987 0.03384298 7.45588e-7 0.07519984 0.08028203 0.04150575 -0.06240826 0.03718495 0.07440698 0.01076471 0.05822587 0.04162842 -0.06260228 0.03280186 0.03704518 -0.06542032 0.03182488 0.0378769 -0.06468808 0.03619384 0.003648698 0.07488876 0.08449101 0.07403308 0.01180022 0.06266897 0.00730437 0.07482826 0.08043998 0.01261734 0.07411462 0.08019202 0.01222252 0.07397085 0.08416801 0.07468485 0.006538212 0.06083887 0.02887845 -0.06935143 0.03026998 0.03276205 -0.06766921 0.03104597 0.0749762 0.005590498 0.05643188 0.07475191 0.005577743 0.06072998 0.03693246 -0.06521993 0.03621399 0.03316622 -0.06722754 0.03531098 0.01786446 0.07302755 0.07981503 0.07514375 0.002493858 0.05535888 0.02875512 -0.06923109 0.03461498 0.02788555 -0.06982201 0.03029888 0.07513517 -0.002723038 0.05355 0.02422446 -0.07094311 0.03401899 0.07493728 0.002006709 0.05937701 0.07488936 -0.003492057 0.05735099 0.02290529 -0.07160943 0.02967894 0.07469445 -0.008570849 0.05147188 0.0745477 -0.007815599 0.05608898 0.07393878 -0.01362133 0.04972589 0.01945835 -0.07239294 0.03351485 0.01471549 -0.07350194 0.03312885 0.01780962 -0.07304048 0.02918297 0.07408148 -0.01280868 0.05005186 0.07447385 -0.008541405 0.05571883 0.01247966 -0.07414472 0.02864396 0.07372075 -0.01357722 0.05396687 0.07310545 -0.0175448 0.04840987 0.009871125 -0.07430684 0.03284984 0.007307529 -0.07482349 0.02856487 0.07262629 -0.0185666 0.05223184 0.03589928 0.06605857 0.077407 0.03674215 0.06559926 0.07705301 0.071711 -0.02258682 0.04666197 0.004962623 -0.07479763 0.03267896 0.00364089 -0.07486289 0.03287798 0.06981909 -0.02727562 0.04934787 0.06966459 -0.02828252 0.04465699 0.03748178 0.06517314 0.07709199 0.04021358 0.06352204 0.07651901 0.07120668 -0.02343636 0.05053883 0.04134607 0.06279772 0.07614797 0.04122334 0.06261545 0.08033901 0.0427379 0.06158155 0.080145 0.04935038 0.05643641 0.07819098 0.04626667 0.05898922 0.079364 -0.03234386 0.06762963 0.08217602 -0.03470367 0.05693316 0.07969999 -0.03344738 0.05548852 0.07969999 -0.03369253 0.05559092 0.08419996 -0.03238195 0.05556005 0.07969999 -0.0316661 0.05608296 0.07969999 -0.03130733 0.05689382 0.07969999 0.00291121 -0.002113521 0.08166098 0.003043532 -0.002236962 0.08170098 -0.01711934 -0.02854758 0.07934796 -0.02123087 -0.02807021 0.079553 -0.01262235 -0.02891999 0.07948195 -0.02954709 -0.0223 0.08019602 -0.03023028 -0.01997911 0.08009797 -0.03110939 -0.01495832 0.08053696 -0.03111046 -0.01727712 0.08040601 -0.03118515 -0.01495981 0.080554 -0.03131145 0.04088515 0.08557397 -0.02126735 -0.01398879 0.080639 -0.02134329 0.04086941 0.08542197 -0.02134329 -0.0139873 0.08062201 -0.02069646 -0.01710379 0.08034902 -0.01651787 -0.0199359 0.08010196 -0.007027089 -0.01399642 0.08072602 -0.007176399 -0.01557701 0.08048301 -0.006855368 -0.002906084 0.08159202 -0.006934523 0.01625585 0.08328497 -0.0067842 0.01769894 0.08339399 -0.001532614 0.02739787 0.084544 0.003894746 0.02968603 0.08462703 0.01011759 0.03000396 0.08447098 0.01585388 0.03025341 0.08479398 0.02823168 0.0203728 0.08362799 0.02740246 0.01011204 0.082731 0.02869528 0.01599705 0.08324497 0.02045857 -4.81824e-4 0.08180397 0.0265153 0.007780671 0.08265799 0.01842612 -0.005569875 0.08135896 0.0187028 -0.005020737 0.081707 0.0201264 -0.008577406 0.08109599 0.04061979 -0.02887278 0.07933801 0.04084044 -0.02887231 0.07955801 0.02857685 -0.03081089 0.07918196 0.01211315 -0.01467686 0.08066397 0.0123021 -0.01451081 0.080576 0.008749663 -0.004162073 0.08148199 0.0178436 0.01168429 0.08286798 0.01198548 0.003685355 0.08230799 0.0134204 0.02133435 0.08371198 0.006696999 0.02082693 0.08366799 -0.03310465 -0.05180472 0.07183098 -0.03021389 -0.05916911 0.07073301 -0.03198617 -0.05755096 0.07069998 -0.03290766 -0.05744618 0.07069998 -0.03058886 -0.0553534 0.07069998 -0.03385716 -0.05512017 0.07069998 -0.06322145 -6.82958e-4 0.07791602 -0.06530427 -0.002751946 0.077919 -0.06306457 -0.002709627 0.07850801 -0.06739586 -0.004037678 0.07847297 -0.06843119 -0.002833366 0.07789999 -0.0644024 -6.1771e-4 0.07789999 -0.0666179 0.001597464 0.07789999 -0.06733375 -0.001074731 0.07789999 -0.06639289 -0.001691043 0.07789999 -0.0685575 0.003669619 0.07817101 -0.04073876 0.06234282 0.08143502 -0.04560893 0.05938094 0.07981801 -0.07282108 -0.01745522 0.05311298 0.03174936 -0.06776911 0.03573983 0.02379459 -0.07058149 0.03505796 -0.07102447 -0.02231919 0.05213087 -0.07132458 -0.0216431 0.05207985 0.03264069 -0.0670349 0.03618186 0.06170386 0.04188555 0.07415097 -0.0676307 -0.03135478 0.04862588 -0.06508648 -0.03624188 0.04704385 0.03798377 -0.0641337 0.03726989 0.05766505 0.04734665 0.07598298 0.03727293 -0.06444782 0.03740698 -0.06234186 -0.0406925 0.04571199 0.04141277 -0.06186789 0.03830885 0.04533559 -0.05919808 0.03892385 0.05566591 0.04945546 0.07709097 0.0455597 -0.05888175 0.03935295 -0.06242406 -0.04074281 0.04538285 0.05410629 0.05150592 0.07729101 0.05147618 0.05385261 0.07858997 -0.05618137 -0.04885292 0.04285997 0.04928636 -0.05580252 0.04042989 -0.059448 -0.04499572 0.04387485 0.05278664 -0.0525195 0.04146397 -0.05259478 -0.05288296 0.04109185 -0.0560863 -0.04910826 0.04247587 0.05634397 -0.04868412 0.04282397 -0.04895287 -0.0561161 0.04020595 -0.05422955 0.05103904 0.07756996 -0.0452829 -0.0590946 0.03927898 -0.05716079 0.04787009 0.07622402 -0.04116314 -0.06203413 0.03825098 0.06214827 -0.04121971 0.04516589 -0.0601114 0.04391932 0.07523703 -0.04249888 -0.06133759 0.03814995 0.0653485 -0.03602749 0.04694396 -0.03662943 -0.06481629 0.03727787 -0.06016707 0.0440889 0.07484298 -0.06324809 0.03926742 0.07361698 0.06771481 -0.031964 0.048105 -0.03192585 -0.06726038 0.03642284 -0.06301045 0.03984093 0.07344901 -0.03111255 -0.0677604 0.03595197 0.0698145 -0.02591627 0.05078798 -0.06818699 0.03008031 0.07016199 0.07118189 -0.0218116 0.05230885 -0.02192908 -0.07114481 0.03511494 0.0732339 -0.01403278 0.05465996 -0.07001549 0.02563601 0.068475 -0.01705574 -0.0724703 0.03460097 -0.01619738 -0.07278293 0.03418684 -0.07166159 0.02025175 0.06676697 0.07401376 -0.009496629 0.05617398 -0.01188856 -0.07349491 0.03424197 0.0745083 -0.003972232 0.05811101 0.07440817 -0.002495706 0.05905199 -0.005195856 -0.07440358 0.03361797 0.07454705 0.00258851 0.06046599 -0.07383346 0.01040512 0.06315499 -0.07440537 0.004392504 0.061176 -0.003312647 -0.07438009 0.03393197 0.002506971 -0.07451421 0.03358799 0.009799718 -0.07392191 0.03375995 -0.07486605 0.001313686 0.05965387 0.07314735 0.01384294 0.06475198 0.07175689 0.0203123 0.06662499 -0.07245266 -0.0171141 0.05394887 0.04216289 0.06189936 0.08064097 0.06753176 -8.17181e-4 0.07789999 0.06842535 -0.0028373 0.07789999 0.06736308 0.00104624 0.07789999 0.06669259 -0.001566529 0.07789999 0.06665366 0.001578152 0.07789999 0.06452059 -9.1147e-4 0.07781296 0.06480747 0.0032745 0.07811099 0.0676943 -0.003554642 0.07811301 0.0642963 2.24466e-4 0.07789999 0.06425011 -0.003180146 0.07817 0.06560707 -0.001691043 0.07789999 0.06466871 0.003927767 0.07907897 0.0627858 0.001161277 0.07807201 0.06560707 0.001691043 0.07789999 0.03677392 -0.05486321 0.07193696 0.03640115 -0.05363333 0.07316297 0.03380078 -0.05328309 0.07071995 0.0334357 -0.0545904 0.07069998 0.03073829 -0.05673128 0.07069998 0.03363299 -0.05686908 0.07069998 0.03126478 0.05721575 0.08419996 0.03361767 0.05875509 0.08419996 0.0324949 0.05881863 0.08419996 0.03470283 0.05697405 0.08419996 0.03438287 0.05614691 0.08419996 0.03164494 0.05819773 0.08419996 0.03345197 0.05547142 0.08419996 0.03271049 0.06429445 0.08433097 0.02894008 0.05785912 0.08484399 0.02746742 0.06975567 0.08287 0.02245289 0.07152813 0.08343899 0.01777786 0.07269924 0.08428299 0.02762722 0.06958204 0.08325201 0.01761513 0.07287245 0.083826 0.01232832 0.07376015 0.08469498 0.007935345 0.07431226 0.08493798 0.00672847 0.07459014 0.08470702 1.56056e-5 0.07497197 0.08462798 -0.001581192 0.07470005 0.08508503 -0.004882216 0.07481837 0.084504 -0.009655296 0.07416915 0.084818 -0.02015936 0.07206195 0.08410203 -0.02444738 0.07086676 0.08345699 -0.02863496 0.06927382 0.08295899 -0.03208899 0.06714177 0.08301401 -0.03277558 0.05545383 0.08419996 -0.03438287 0.05614691 0.08419996 -0.03897166 0.06083065 0.08419996 -0.03166615 0.0582323 0.08419996 -0.0347352 0.05721575 0.08419996 -0.03238219 0.05875509 0.08419996 -0.03173208 0.05597162 0.08419996 -0.03041768 0.05836164 0.08423 -0.02946728 0.05788815 0.08445203 -0.03182077 0.06421285 0.08492898 -0.03534168 0.05466026 0.08434599 -0.03635644 0.05505883 0.08484101 0.003276944 0.0162661 0.08333802 -0.008428037 -0.02843743 0.07965898 -0.03125077 -0.01496326 0.08059597 -0.0313735 0.04097193 0.08567398 -0.03143274 0.04108667 0.085725 -0.02120172 -0.0139923 0.08068102 -0.008471786 -0.0185523 0.08051598 0.02613461 0.02495974 0.08433097 0.04104846 -0.02887678 0.07962095 0.03374058 -0.03635627 0.07896596 0.00955075 -0.01127606 0.08116102 0.009325146 -7.97087e-4 0.08207696 0.01796126 0.01783233 0.08340597 -0.02876389 -0.05297863 0.07546198 -0.03456676 -0.05206739 0.07431 -0.02793228 -0.05479431 0.07440197 -0.03272306 -0.05136913 0.07516497 -0.03353756 -0.0500828 0.07673197 -0.03586947 -0.05141723 0.07563197 -0.03027147 -0.05084007 0.076765 -0.0303483 -0.05172491 0.07551699 -0.02654755 -0.05793356 0.07417398 -0.02818256 -0.05790627 0.07249701 -0.03083026 -0.05986326 0.07058697 -0.0370689 -0.05797082 0.06990796 -0.03590017 -0.05657535 0.07069998 -0.03043454 -0.06101548 0.07013696 -0.03832769 -0.05372291 0.07331496 -0.03676909 -0.05593317 0.07107597 -0.03613466 -0.05724549 0.07058197 -0.0647822 -0.004447162 0.07974201 -0.0682258 -0.004952609 0.07845997 -0.06955039 -0.002859294 0.07762998 -0.0699225 0.003006279 0.07778298 -0.070831 0.003396451 0.07729697 -0.06804907 0.00560981 0.08018398 -0.06710517 0.004517555 0.07970297 -0.06379097 0.003991842 0.081173 -0.04109477 0.05815196 0.08612197 -0.03924667 0.05455905 0.08691996 -0.03962177 0.05982506 0.084746 -0.03338205 0.06526345 0.083929 -0.03945547 0.06283295 0.08281099 -0.04595309 0.05863374 0.080244 0.06175076 0.001295924 0.080908 0.05046749 0.05472981 0.07957798 0.0587365 -0.04353559 0.06141698 0.05831885 -0.04409366 0.06121885 0.01595586 -0.07085728 0.05764997 0.01431 -0.07122933 0.05725598 0.04895138 -0.05609238 0.04036384 0.05895519 0.04546064 0.07577502 0.05812823 0.04630357 0.077986 0.05523365 -0.0477662 0.06104683 0.00534296 -0.0724141 0.05737584 0.06620037 0.03340393 0.075405 0.0646333 0.03694462 0.07280701 0.06694489 0.03261762 0.07125997 0.06416547 0.03755813 0.07422602 0.009617865 -0.07201409 0.05697482 0.004722833 -0.07248109 0.05706685 0.06865435 0.02879953 0.06996899 0.06683689 0.03225082 0.07423299 0.04484355 -0.05942469 0.03920197 0.06919848 0.02746641 0.069503 0.07090616 0.02269065 0.06783801 0.068919 0.02736324 0.07328903 0.04061919 -0.0623877 0.03816884 0.07045608 0.02405363 0.06831401 0.07206785 0.01866793 0.06643497 0.07056188 0.02259021 0.07239401 0.07183635 0.01787096 0.07153099 0.03590339 -0.06521749 0.03718197 0.07402318 0.007936716 0.06269198 0.07433229 0.004170656 0.06137901 0.07278907 0.01307654 0.07075798 0.07389128 0.008893013 0.06331199 0.05127137 -0.05178213 0.06154286 0.07444596 -7.99005e-4 0.05964601 0.07404947 -0.007692277 0.05723798 0.03097385 -0.06770062 0.03631585 0.032992 -0.06674343 0.03660386 0.07378339 0.00253725 0.06872797 0.07357835 -0.01133358 0.05597198 0.04414802 -0.05795979 0.05965685 0.07372158 -0.002453804 0.06779199 0.07330948 -0.01296103 0.055399 0.02858376 -0.06874841 0.03590196 0.07329881 -0.007593512 0.06685698 0.07271897 -0.0159412 0.05436486 0.07141178 -0.0210421 0.05258584 0.0263893 -0.0696159 0.03564786 0.07258027 -0.01122027 0.06843996 0.07156479 -0.01630347 0.06742995 0.07249689 -0.01279461 0.06593596 0.04053294 -0.06050992 0.05886685 0.0676406 -0.0311076 0.049061 0.02167618 -0.07122212 0.035088 0.03648197 -0.06303632 0.05796688 0.07142299 -0.0175246 0.06522899 0.06892317 -0.02507197 0.06511998 0.06633448 -0.0337978 0.04813796 0.01634377 -0.07262951 0.03459799 0.01922667 -0.07197493 0.03453898 0.07026076 -0.02150273 0.06465101 0.0649684 -0.03637582 0.04720497 0.01460593 -0.0730673 0.03413587 0.03026807 -0.06611001 0.05842489 0.06260007 -0.04029417 0.04585182 0.06868869 -0.0258913 0.064022 0.03227305 -0.06524384 0.05773586 0.01090466 -0.07364308 0.03424394 0.009848952 -0.07379651 0.03413695 0.05922037 -0.04523658 0.04387295 0.02794498 -0.06716662 0.05758082 0.0666446 -0.03061902 0.06315696 0.005162835 -0.07429349 0.03383994 0.06414675 -0.03529846 0.06302899 0.02350038 -0.0688064 0.05746299 0.06371515 -0.03598612 0.06330996 0.06154537 -0.0395804 0.06203585 0.01894515 -0.07016521 0.05733484 0.03656184 0.06496936 0.08216899 0.04208016 0.06149911 0.081164 0.03365135 0.06492996 0.08403599 0.03328955 0.06398606 0.08419996 0.03811442 0.06354004 0.08306902 0.03955197 0.05691826 0.08667999 0.03916436 0.05938839 0.08461397 0.0400598 0.06139332 0.08381301 0.06296455 0.00274384 0.07868397 0.06286656 0.005461037 0.08244901 0.06962507 0.003118932 0.07788997 0.06601309 0.004641175 0.080576 0.06850379 0.004268288 0.07874798 0.070535 0.00262773 0.07744598 0.06934267 -0.00396198 0.07766896 0.0648927 -0.004648208 0.07992398 0.06233566 -0.003500461 0.08109498 0.06233572 -0.001626372 0.07846498 0.02937334 -0.05237829 0.07538998 0.03131824 -0.0514236 0.07530796 0.03192293 -0.05045652 0.07673901 0.02794265 -0.05512613 0.07401996 0.02673494 -0.05825638 0.07392901 0.02833873 -0.05367672 0.07488501 0.03379195 -0.05166393 0.07477897 0.0270251 -0.0546301 0.07574295 0.03562915 -0.05219471 0.07479196 0.02906328 -0.05160999 0.07667499 0.02597618 -0.05508869 0.07588797 0.03653049 -0.05673182 0.07067596 0.03263527 -0.05982547 0.07022601 0.03064 -0.05963921 0.07069796 0.0283159 -0.05813288 0.07232797 0.03911525 0.05466949 0.08692997 0.03788965 0.05570811 0.08639097 0.03258585 0.05342525 0.08432698 0.03586465 0.05438911 0.08482497 0.0350492 0.05461645 0.08428502 0.03904318 0.06060576 0.08420097 0.03230714 0.05559092 0.08419996 0.03241169 0.05253964 0.08585202 0.02946043 0.06039994 0.08637201 0.03310734 0.06649714 0.08319401 0.02940446 0.06387925 0.08737802 0.02991199 0.06314182 0.08728098 0.03169596 0.06421214 0.08513998 0.03108757 0.06613796 0.085635 0.02755117 0.06899422 0.083866 0.02283477 0.07069343 0.08429998 0.01814705 0.07192301 0.08482301 0.00339663 0.07410603 0.08538502 0.008111894 0.07376945 0.08525604 -0.004723191 0.07397615 0.08548802 0 0.07404774 0.08559101 -0.007200062 0.07410866 0.08512401 -0.01215195 0.07341766 0.08494204 -0.01743876 0.07238984 0.08458298 -0.01970726 0.07153272 0.084697 -0.02259707 0.07099932 0.08410501 -0.02431792 0.07017672 0.08420801 -0.02757579 0.06925857 0.08351701 -0.02874296 0.06825602 0.08442497 -0.03342968 0.06617516 0.083211 -0.03328907 0.06398516 0.08419996 -0.03266209 0.06469774 0.08442199 -0.03089535 0.06456756 0.08630102 -0.0269314 0.05934453 0.08733898 -0.02761673 0.05886185 0.087134 -0.03143745 0.05391913 0.084275 -0.03478378 0.05369114 0.08460402 -0.02947413 0.05556136 0.08457303 -0.03126478 0.05721575 0.08419996 -0.03683096 0.05258464 0.08665001 -0.03846687 0.05647099 0.086443 -0.03656226 0.05390435 0.08619898 0.003620624 0.01774376 0.08339798 0.003517687 0.01531469 0.08348697 0.003204286 -0.002404153 0.08190196 0.003239393 -0.01498287 0.08081901 0.002736032 -0.0187276 0.08036398 -0.002894163 -0.02646958 0.079831 -0.02816158 -0.02343767 0.07979595 -0.02560019 -0.02602601 0.07986998 -0.0298326 -0.02153474 0.08019101 -0.03044778 -0.02007383 0.08028995 -0.03046643 -0.02008301 0.08031797 -0.03049045 -0.02009361 0.080343 -0.03151345 -0.01498436 0.08083599 -0.03135603 -0.01497763 0.08075797 -0.03141349 -0.0149821 0.08080899 -0.02103048 0.04114902 0.085725 -0.02108055 -0.0140084 0.08086299 -0.02093917 -0.0140134 0.08092099 -0.01915127 -0.01868426 0.08051198 -0.007475376 -0.01582932 0.08076298 -0.007060706 0.01624423 0.08341801 -0.007138729 -0.01400983 0.08087897 -0.007186889 0.01623255 0.08354997 -0.005911171 0.02206784 0.08401501 0.009453475 0.03046447 0.084665 0.0204789 0.02882862 0.08452302 0.02322626 0.02709627 0.08421701 0.02835059 0.02088212 0.083974 0.02893239 0.01350325 0.083328 0.02423417 0.004143476 0.08250999 0.0280441 0.01035469 0.08305299 0.04087889 -0.02881991 0.07960897 0.01309657 -0.0159868 0.08074802 0.01319926 -0.01587861 0.08069097 0.03374403 -0.03617388 0.07894599 0.01313143 -0.01595151 0.08074498 0.008253276 -0.005172193 0.08169502 0.01663488 0.01968485 0.08388197 0.01844388 0.01449656 0.08320796 0.01637095 0.009650647 0.082991 0.01601976 0.008699417 0.08274298 0.004374444 0.01869934 0.08378201 -0.05971777 -0.00219357 0.08195501 -0.06239593 -0.002691388 0.08053696 -0.06034499 -6.92624e-4 0.081909 -0.05976223 0.002028763 0.08232498 -0.06118178 0.002163231 0.081999 -0.0616557 8.50888e-4 0.080666 0.06305086 0.003477454 0.08149498 0.06053006 0.001435637 0.082116 0.06179314 -0.001749873 0.08074498 0.0229367 -0.06270051 0.07128995 0.05270344 -0.04031473 0.07496297 0.07130038 -0.006889343 0.07446998 0.07005417 0.02392804 0.07365602 0.06756377 -0.006221652 0.07880997 0.07117891 -0.0162748 0.06941998 0.01064383 -0.07183152 0.05748498 0.01865983 -0.06426471 0.07091599 0.02176487 -0.05991983 0.07388597 0.01920068 -0.06693309 0.06729102 0.04676067 0.05760663 0.08171898 0.07114779 -0.01152062 0.07282698 0.05209928 -0.04595798 0.07104599 0.05105847 -0.05155795 0.063425 0.06997978 0.02383041 0.07448798 0.04180353 0.06059771 0.08372998 0.0704196 -0.02072143 0.06584995 0.07014799 -0.0114395 0.07457399 0.04794543 -0.05489492 0.06023997 0.01111048 -0.07091873 0.06221085 0.04566335 0.05658912 0.08438098 0.04148638 0.05861437 0.08568799 0.06967985 0.02335 0.07643502 0.05116838 -0.03973329 0.07604497 0.05246758 0.0381987 0.08548903 0.01971787 -0.0585463 0.07508498 0.05346316 -0.04707866 0.067999 0.04858762 -0.03745222 0.077883 0.07167351 0.01812905 0.07289701 0.0708571 0.02108687 0.07362598 0.0566892 0.03360801 0.08494001 0.04713356 -0.05547231 0.06116586 0.06935006 -0.02147692 0.06974595 0.01656335 -0.06877529 0.06506699 0.06698578 0.0276544 0.0794 0.05291128 -0.04665368 0.06931996 0.04984128 -0.05084711 0.06741994 0.05221116 -0.02407681 0.08002799 0.061526 -0.004511058 0.08161896 0.01108479 -0.06983381 0.06494998 0.05038964 -0.05133199 0.06561994 0.04910027 -0.05027562 0.06915795 0.04881834 0.05495142 0.08277601 0.06355297 0.03188616 0.081918 0.005789816 -0.07225799 0.05869299 0.07008558 -0.0214405 0.06686198 0.05016177 0.0545051 0.08137601 0.06997287 -0.01644748 0.07237899 0.06477797 -0.00587362 0.08046901 0.01522988 -0.06801372 0.06697499 0.06855458 -0.01150459 0.07649999 0.06358426 0.02733147 0.08271801 0.01936519 -0.0523349 0.077389 0.05913513 0.03321301 0.08432596 0.0686199 -0.01609098 0.07466799 0.01147305 -0.07161539 0.05862885 0.04686266 -0.05513679 0.063452 0.04916507 -0.03337538 0.07885199 0.0627529 -0.01068359 0.08037596 0.06109279 0.02741903 0.083938 0.0674104 -0.01126945 0.077627 0.06875336 -0.02511239 0.066325 0.04817575 -0.0319314 0.079243 0.05387765 0.05100482 0.080154 0.0688889 0.02316236 0.078103 0.04775619 -0.04895889 0.07183796 0.04639369 -0.05463278 0.06564599 0.05281656 0.05083262 0.082345 0.04544603 -0.0538274 0.06826996 0.05502545 0.05001205 0.07856899 0.0679863 -0.02507323 0.069247 0.01534014 -0.065714 0.070167 0.04737067 -0.04230052 0.07638698 0.06725305 -0.02887988 0.06556296 0.01641684 -0.06273943 0.07281196 0.00580579 -0.07148319 0.06224799 0.06996798 0.01821947 0.07793796 0.06807357 -0.02107769 0.07262498 0.06519156 -0.01101487 0.07921397 0.01046419 -0.0593115 0.07560998 0.05717074 0.02808237 0.08460402 0.0498442 -0.04429531 0.074162 0.01385629 -0.06046253 0.07469695 0.04121577 0.05635142 0.08657503 0.02048927 -0.05528789 0.07645398 0.04273808 -0.05883955 0.06080883 0.04293966 -0.05779862 0.064148 0.005535304 -0.07049751 0.06480497 0.066603 -0.01592218 0.07685297 0.07063657 0.01797592 0.076698 0.04729574 0.05343544 0.08526802 0.07122761 0.01827192 0.07493799 0.01243203 -0.05205762 0.077578 0.03973478 -0.06098377 0.05925285 0.06215626 -0.01479083 0.079764 0.06304228 0.0244522 0.08326202 0.06483757 0.02265644 0.08241599 0.01095545 -0.06876671 0.06697696 0.07259887 0.0137543 0.07181996 0.06507277 -0.03322893 0.06503796 0.04003018 0.05561852 0.08688402 0.005458474 -0.06969493 0.066374 0.0457648 -0.04671591 0.07475596 0.06022685 0.02580291 0.08416801 0.06828427 0.0184623 0.08002603 0.06653219 0.0239759 0.08101499 0.009812176 -0.06282448 0.07357597 0.06637239 -0.02474528 0.072609 0.06727916 -0.02495032 0.070957 0.06702977 -0.02959829 0.064372 0.0437324 -0.0559169 0.06682199 0.06503027 -0.02182739 0.07588201 0.06121999 -0.01045233 0.08084601 0.005328953 -0.06870609 0.068017 0.01381325 -0.05588769 0.07673597 0.07297849 0.01021075 0.07235699 0.05668658 0.04741322 0.08033001 0.04462116 -0.03410649 0.07914996 0.01037263 -0.06588721 0.07089799 0.0636357 -0.03470772 0.06737798 0.00501883 -0.06587922 0.07140898 0.06429916 -0.01598048 0.07845503 0.06672418 -0.02899187 0.06765496 0.005499184 -0.06735318 0.06977796 -7.72285e-4 -0.06567722 0.07173901 0.04455053 -0.05262988 0.07068097 0.03510212 -0.06371551 0.05881083 0.03727775 -0.06241172 0.05994695 0.06117826 -0.01970893 0.07898402 0.06019109 0.04334235 0.07833403 0.00467509 -0.06355792 0.073394 0.04520708 -0.037795 0.07845497 0.06248736 -0.02758759 0.07524299 0.06496441 -0.02854472 0.07182598 0.04260927 -0.0514189 0.072995 0.06595247 -0.02887052 0.06975895 0.01296216 -0.05403131 0.07723796 0.07192647 0.01430493 0.07493799 6.90988e-5 -0.0633949 0.07358098 0.04549986 -0.04072523 0.07757896 0.005117297 -0.06002992 0.07550197 0.06505995 0.01871073 0.08243 0.07337045 0.007882595 0.07086998 0.07356029 0.003165841 0.07095396 0.04131466 -0.05658042 0.06841099 0.006375372 -0.0570802 0.07662302 0.06211405 0.02136182 0.08358496 0.05624145 0.04656994 0.082008 0.004381954 -0.05581706 0.07701796 1.18968e-5 -0.0525943 0.077546 0.05826795 -0.02221161 0.0794 0.05855643 -0.01822811 0.080145 0.0613718 0.04192817 0.07630097 0.07364505 0.004150986 0.07011699 0.04319512 -0.03735888 0.07876396 0.03500056 -0.06277412 0.06364101 0.06907975 0.01236045 0.07968497 0.06359589 -0.03599733 0.064233 0.06144875 -0.03950458 0.06361895 0.04095 -0.04802078 0.07582497 0.07203918 0.009818196 0.07551801 0.07120007 0.009581327 0.077057 0.05313485 0.04810082 0.08402603 0.03997927 -0.05953669 0.06472897 0.04675906 0.05070084 0.08614802 0.05995714 -0.02356863 0.07839095 0.06241798 -0.02220851 0.07756799 0.03831118 -0.05479508 0.07242697 0.04979789 0.04876816 0.08552402 0.07297086 0.005266368 0.07370597 0.06698197 0.01802426 0.08121401 0.06633239 0.01500165 0.08173096 0.03823655 -0.05725246 0.07003396 0.03829729 -0.03889977 0.07874399 0.0631268 -0.03387618 0.069624 0.03257769 -0.06498521 0.05922091 0.03738737 -0.05230277 0.07463997 0.06069856 -0.03957742 0.066468 0.04215794 -0.04380208 0.07729196 0.06566745 0.03422105 0.07670402 0.04071408 -0.04202067 0.078076 0.0571444 -0.01389729 0.08091896 0.06303781 0.03892093 0.07768702 0.03860259 -0.04663378 0.07702898 0.0598883 0.02355116 0.08411997 0.04834765 0.04694992 0.08612799 0.06365489 0.03814291 0.07629501 0.05785053 -0.03042227 0.07698899 0.05498445 -0.02496922 0.079589 0.05947077 0.01941037 0.08384501 0.03485745 -0.05044358 0.07633996 0.05985099 0.04224592 0.08076798 0.06112688 -0.0401104 0.06263297 0.05300909 0.04603981 0.08485001 0.03552424 -0.0604496 0.06767696 0.06067377 -0.02694082 0.07681798 0.06295728 0.01558715 0.08315896 0.06217706 -0.03370118 0.07130497 0.0514363 -0.02334111 0.080105 0.05836838 -0.04384326 0.06269699 0.07236361 0.002799451 0.07478201 0.03684115 -0.04338252 0.07818198 0.03436297 -0.04672199 0.07765299 0.05997335 -0.03891628 0.06899797 0.02577829 -0.06795603 0.05812788 0.05769008 0.04226166 0.08320599 0.05807757 -0.0437231 0.06461197 0.02526766 -0.0680086 0.05946195 0.05914026 -0.03861469 0.07058596 0.05782139 -0.01612019 0.080594 0.062249 0.03814744 0.08046001 0.06052237 -0.03261899 0.074014 0.06732058 0.03089863 0.07533699 0.03016787 -0.06540399 0.06246685 0.07329577 5.56042e-4 0.07193297 0.05564177 0.03921669 0.08501398 0.02990925 -0.06312531 0.06765997 0.05580496 0.04224652 0.08441996 0.06690877 0.01032292 0.08125299 0.06129413 0.01636821 0.083503 0.07368826 -7.70061e-4 0.06926101 0.05749619 -0.04339939 0.06685501 0.02116799 -0.0695036 0.05787098 0.06854528 0.005787909 0.07991099 0.02899932 -0.06077677 0.07109701 0.02964282 -0.06432652 0.06571298 0.0566377 -0.02604138 0.07894498 0.0726009 3.90835e-4 0.073888 0.06539297 0.005846381 0.08174496 0.06487369 0.01328653 0.08239698 0.07339119 -0.005804002 0.06838899 0.05260086 -0.0260086 0.07974296 0.02024269 -0.06956702 0.05969399 0.02796757 -0.05098778 0.07714599 0.02437096 -0.06624382 0.06587898 0.05191087 0.04394572 0.08562099 0.07190567 -4.73319e-4 0.07513898 0.0712853 0.003337085 0.076756 0.05635148 -0.0328201 0.07674598 0.02488029 -0.06738972 0.062756 0.05952256 3.38854e-4 0.08217597 0.0579741 0.03797709 0.08422398 0.05756884 -0.0380643 0.07288801 0.02232569 -0.0609709 0.07299596 0.02375245 -0.0647186 0.06866997 0.05492377 -0.04788208 0.06258499 0.07080209 -0.001741409 0.07669401 0.06199938 0.01016736 0.08293998 0.05604565 -0.04255199 0.07031297 0.05685228 -0.04310083 0.06852495 0.05731749 -0.03464341 0.07534599 0.05247783 -0.02923911 0.07917296 0.06470477 0.03391414 0.07955801 0.02723658 -0.04916 0.07767099 0.06144577 0.03164362 0.08343797 0.07322669 -0.006622672 0.06902098 0.01628029 -0.07069832 0.05860996 0.0207476 -0.0685414 0.06306701 0.05386465 -0.03636568 0.07638597 0.03342562 -0.04350328 0.078327 0.06037229 0.03734195 0.08291 0.06764936 0.02959406 0.07625299 0.05351305 0.03977215 0.085473 0.06831979 0.02867013 0.07452499 0.06705808 0.02916336 0.078336 0.01634287 -0.06990021 0.06237083 0.05425947 -0.0476467 0.06558996 0.05445104 -0.04173737 0.07272297 0.02001345 -0.05404353 0.07691299 0.02043753 -0.06770449 0.06527501 0.07278299 -0.005993902 0.07174497 0.05241554 -0.03546452 0.07738196 0.05587345 -0.02950328 0.07818496 0.07189965 -0.0115289 0.07105201 0.0511865 -0.03385078 0.07830601 0.02648049 0.05781573 0.08720499 0.03086435 0.05164301 0.08654302 0.03641659 0.05380421 0.08613097 0.0296489 0.05410993 0.085675 0.02893048 0.05437213 0.08628702 0.03650146 0.05166566 0.08666801 0.03464376 0.0522226 0.08639001 0.03429317 0.05072963 0.08658498 0.02815389 0.05657851 0.086416 0.02808737 0.06218522 0.08758801 -0.02753067 0.06782752 0.085734 -0.02425777 0.06911176 0.08591103 -0.02865445 0.06230872 0.08759099 -0.02399355 0.06828147 0.08672797 -0.02320545 0.06596446 0.08775401 -0.01961338 0.07116836 0.08541899 -0.02500265 0.06700885 0.08719301 -0.01866286 0.06757211 0.08789998 -0.01443397 0.07192152 0.08629798 -0.03068637 0.06596773 0.08597397 -0.01925837 0.06977975 0.08698403 -0.01426917 0.07112592 0.08705902 -0.01888519 0.06847095 0.08767902 -0.009471595 0.07352834 0.08538401 -0.02050364 0.06551367 0.087879 -0.01420956 0.07281947 0.085119 -0.02719515 0.06700414 0.08657097 -0.01403725 0.06997615 0.08774501 -0.01050955 0.06946194 0.08810502 -0.004739046 0.07326197 0.08648502 -0.009443402 0.07278794 0.08641701 0 0.07338964 0.08653402 -0.009350717 0.07199198 0.08716601 -0.009087502 0.07089245 0.08781999 -0.0234552 0.06667786 0.08757698 -0.02950298 0.06436896 0.08722698 -0.004704177 0.07247304 0.08722299 0.001862764 0.07148987 0.08787703 -0.004472494 0.07135993 0.087875 -0.02442497 0.06963253 0.08515602 -0.03026616 0.06240671 0.086757 -0.03268069 0.05134642 0.08651101 -0.03414589 0.05243474 0.08600097 -0.02978676 0.05154001 0.08665597 -0.02728337 0.0561788 0.08693301 -0.02846336 0.05829423 0.086281 -0.02857553 0.05554121 0.08614903 -0.03512376 0.05095273 0.08660501 -0.02978825 0.05299836 0.08645099 -0.0267353 0.05519032 0.08697599 -0.03151345 0.04124581 0.085756 -0.06056475 0.008986234 0.082933 -0.007262766 0.01623106 0.08356702 -0.007261157 -0.00869137 0.08138698 0.01471877 0.02064836 0.08395296 0.02175962 -0.04860299 0.07789498 0.02040535 -0.008312165 0.08142 0.02716267 0.05435085 0.08690202 0.05739927 -0.0111199 0.08117401 0.006748855 0.03002274 0.08477401 -0.008278489 -0.05192029 0.077605 0.008325695 -0.007124483 0.08152401 0.001880407 -0.01995909 0.080401 -0.01648527 -0.02891731 0.07961696 0.02206999 0.06492382 0.08782702 -0.003729641 0.02546834 0.08437502 -0.007741749 0.06857633 0.08814698 0.003317356 -0.002510726 0.081927 -0.02093917 0.04124581 0.085756 -0.01293987 -0.01965242 0.080428 0.1155291 0.3246383 0.9387562 0.2973741 0.5826956 0.7563297 0.06719589 0.1591982 0.9849573 0.2975795 0.5693182 0.7663702 0.0667808 0.05980467 0.9959738 0.2156583 0.3598166 0.9077575 0.0732339 0.0664162 0.9951009 0.2064326 0.6017552 0.7715415 0.202301 0.5967529 0.7765052 0.01955109 0.8300994 0.5572727 0.01348179 0.8199784 0.5722357 0.1318708 0.3429309 0.9300584 0.01055711 0.6662653 0.7456401 0.3381545 0.8211594 0.4597269 0.02102828 0.6410987 0.7671703 0.007671892 0.4916651 0.8707506 0.07465511 0.8398521 0.5376572 0.313449 0.8074795 0.4997269 0.06855869 0.8311015 0.5518788 0.007479727 0.2201401 0.9754396 0.05710405 0.6429442 0.7637814 0.06953585 0.09784537 0.9927695 0.05980491 0.6360973 0.7692879 -0.00131607 -0.01904451 0.9998177 0.04255568 0.4286592 0.9024636 0.1477875 0.3177058 0.9366014 0.04145175 0.4377379 0.8981465 0.1349589 0.8206871 0.5552107 0.02285623 0.2549481 0.9666846 0.1291319 0.8345878 0.5355262 0.2607417 0.5941188 0.7609446 0.249731 0.5828045 0.7732874 0.06352102 0.06546121 0.9958313 0.1037201 0.6315091 0.7683999 0.1082991 0.640098 0.7606222 0.07816731 0.4284438 0.9001811 0.1682828 0.3361892 0.9266379 0.0814433 0.4350924 0.8966948 0.2075104 0.8138435 0.5427691 0.4060885 0.771977 0.4890234 0.1966119 0.8476642 0.4927568 0.03884458 0.2283916 0.9727942 0.1563437 0.6295927 0.7610321 0.0109049 -0.02651983 0.9995888 0.006989955 -0.05804383 0.9982897 0.04143339 0.1129978 0.992731 0.1594499 0.6112185 0.775234 0.1097348 0.4206378 0.9005678 0.270402 0.808709 0.5223723 0.3988895 0.8002285 0.4477966 0.2616516 0.8316181 0.4898467 0.1765186 0.3216366 0.930264 -0.6665901 0.7408841 0.08214807 -0.1785426 -0.223572 0.9581952 -0.5981787 -0.7823182 0.1736682 -0.8750771 0.1614015 0.4562779 -0.4161057 0.2065981 0.8855356 -0.5832697 -0.7866742 0.2023372 -0.08180296 -0.292815 0.9526635 -0.4241136 -0.7066952 0.5663123 -0.07818406 -0.04638248 0.9958595 -0.9150832 -0.3365752 0.2221263 -0.688404 0.5957823 0.4136947 -0.7797873 -0.3919364 0.4881779 -0.5627728 0.3895412 0.729071 -0.6103164 -0.008856534 0.7921082 -0.3934018 -0.3315446 0.8575041 -0.8480236 0.1081226 0.5188117 -0.06358528 -0.2383711 0.9690904 -0.6059874 -0.01277422 0.7953718 -0.1003541 -0.1531543 0.9830936 -0.003491163 -0.9918255 0.1275546 -0.05487889 -0.9951884 0.08117002 -0.02612233 -0.9964637 0.07986027 -0.6599092 -0.7442099 0.1033033 -0.5726303 -0.7659479 0.2922642 -0.9365659 0.3305595 0.1165109 -0.01208734 -0.9178584 0.3967241 -0.6664076 0.6901179 0.2822026 -0.653419 0.5482392 0.5219936 -0.713317 -0.382893 0.5870024 -0.6570165 0.5653595 0.4986963 -0.612309 -0.7338923 0.2940744 -0.06183212 -0.2264921 0.9720485 -0.01571267 -0.8794441 0.4757429 -0.05814903 -0.981972 0.1798604 -0.05783712 -0.9821772 0.1788377 -0.7958837 0.08649927 0.5992388 -0.2819219 0.08329117 0.9558152 -0.04839557 -0.9646764 0.2589544 -0.3702795 -0.2985077 0.8796513 -0.05678999 -0.9584529 0.2795408 -0.3617114 -0.07284855 0.9294397 -0.6937038 -0.3677226 0.6193183 -0.04747802 -0.9253358 0.3761643 -0.6606225 0.6892493 0.2975121 -0.1229416 -0.9885923 0.0870096 -0.9115315 -0.3240386 0.2531986 -0.5390767 -0.7464741 0.3900933 -0.8704826 0.1806445 0.4578512 -0.8704202 0.1803878 0.4580712 -0.6502966 -0.7411504 0.1667644 -0.05431836 -0.9205727 0.3867757 -0.6410683 -0.7448359 0.1850703 -0.7103471 -0.7021307 0.04918968 -0.8537623 -0.3401597 0.3941845 -0.02101904 -0.1384716 0.9901434 -0.2383224 0.06918984 0.9687184 -0.1886869 -0.9801832 0.06031847 -0.5034365 -0.7156985 0.4840738 -0.5061982 -0.716156 0.480504 -0.1190759 -0.9778223 0.1722916 -0.1200063 -0.9772664 0.1747822 -0.622286 -0.7312447 0.279359 -0.2780305 -0.06187444 0.9585774 -0.001883387 -0.6276196 0.7785179 -0.07101565 -0.2244973 0.9718837 -0.0428723 -0.8711852 0.4890791 -0.6263787 -0.3723212 0.6848552 -0.4546878 -0.02751624 0.8902258 -0.0506649 -0.87585 0.4799167 -0.4698405 -0.6806277 0.5621351 -0.9227335 0.3026863 0.2386293 -0.4552757 -0.6770995 0.5781526 -0.1166948 -0.954799 0.2733884 -0.5851563 -0.712141 0.3878755 -0.1112197 -0.9524004 0.2838375 -0.0462318 -0.807276 0.5883607 -0.9226148 0.3088829 0.2310269 -0.8544479 -0.3409562 0.3920047 -0.3003346 -0.2576824 0.9183676 -0.2337937 -0.2387446 0.9425187 -0.8023694 -0.3369007 0.4926475 -0.06106698 -0.7930164 0.6061319 0.002160489 -0.5224546 0.8526644 -0.3525878 0.226976 0.9078348 -0.7115969 -0.6903082 0.1307845 -0.5757798 0.7694875 0.2763456 -0.3780731 0.1971929 0.9045307 -0.1240045 -0.2809327 0.9516826 -0.2148219 -0.2134801 0.9530361 -0.5489768 0.4301335 0.7166656 -0.5451371 0.4091399 0.7317309 -0.7771584 0.1040592 0.620642 -0.617109 0.6045452 0.5036881 -0.5400227 -0.3257756 0.7760449 -0.1979436 -0.9738639 0.1113893 -0.1044866 -0.1645219 0.9808238 -0.8040071 -0.3387724 0.4886777 -0.9543785 -0.2496853 0.1637654 -0.5851444 -0.7122887 0.3876221 -0.1067903 -0.9160496 0.3865865 -0.1084713 -0.9172518 0.3832537 -0.6725302 -0.6761415 0.3008916 0.9643912 -0.1067786 0.2419672 -0.01098406 -0.4369253 0.8994307 -0.01634603 -0.3748847 0.9269274 -0.6054204 0.598908 0.5241901 -0.1884543 -0.9669765 0.1715855 -0.006252765 -0.537447 0.8432744 -0.5953345 0.757609 0.2675921 -0.2032344 -0.9547477 0.217147 -0.01893258 -0.7133627 0.7005392 -0.1341369 -0.2814939 0.9501414 -0.3943536 -0.6039852 0.6925945 -0.4020922 -0.6053888 0.6868962 -0.6882608 -0.7029095 0.1794863 -0.5977852 0.7467764 0.2915098 -0.01877468 -0.1247324 0.9920129 -0.09906864 -0.8713531 0.480551 -0.01527756 -0.09871798 0.9949982 -0.7307175 -0.3268271 0.599363 -0.7051421 -0.3377624 0.623451 -0.1001848 -0.8722485 0.4786916 -0.23245 0.07824647 0.9694558 -0.2539362 -0.9656666 0.0548129 0.007681906 -0.2816967 0.9594728 -0.5378735 -0.3227117 0.7788128 -0.08799034 -0.1467881 0.9852467 -0.7175607 0.1064779 0.6883089 -0.9236919 -0.2815181 0.2598864 -0.1702843 -0.9463522 0.2746285 -0.03955566 -0.6454153 0.762807 -0.2351568 -0.9654678 0.1121307 -0.5463204 -0.6834435 0.484189 -0.5485994 -0.6834232 0.4816343 -0.2002009 0.06700789 0.9774608 -0.8598609 0.2188933 0.4612212 -0.05527889 -0.6348835 0.7706277 -0.0901243 -0.7996499 0.5936646 -0.2055037 -0.3832025 0.9005132 -0.4048222 -0.2738803 0.8724154 -0.7538272 -0.6545275 0.05777984 -0.731252 -0.6696985 0.1295166 -0.6614579 -0.6829018 0.3100302 -0.07580775 -0.7913905 0.6065923 -0.5098314 -0.6487489 0.5649751 -0.6322748 0.07370978 0.7712299 -0.4114885 -0.2864624 0.8652264 -0.6151639 -0.6850375 0.3902526 -0.9136237 -0.2726228 0.3016103 -0.4342209 -0.009022891 0.9007612 -0.02965462 -0.397027 0.9173278 -0.04759395 -0.5683094 0.8214372 -0.598459 0.03925418 0.8001913 -0.1723592 -0.9067865 0.3847476 -0.04952973 -0.1171805 0.9918748 -0.5880131 -0.6530941 0.4771885 -0.2156596 -0.2141841 0.952689 -0.2872266 -0.04620271 0.9567478 -0.1531693 -0.07989621 0.9849649 -0.5340935 0.4473506 0.7173714 -0.8491777 0.222448 0.478972 -0.9021009 0.3763917 0.2110526 -0.2339426 -0.9475567 0.217732 -0.9033737 0.3449781 0.2547671 -0.3816599 0.2354319 0.8938163 -0.2977852 -0.2235254 0.9280951 -0.7655344 0.1911517 0.6143438 -0.8733369 -0.2803103 0.3983829 -0.8760387 -0.2786052 0.3936181 -0.2338603 -0.9476005 0.2176299 -0.6373371 -0.296687 0.7111809 -0.05643755 -0.1739894 0.983129 -0.3014372 -0.4965834 0.8139659 -0.1716456 -0.9074183 0.3835752 -0.5095181 -0.6065085 0.6103596 -0.3010823 -0.520527 0.7990002 -0.579082 -0.6556049 0.4846094 -0.5512064 -0.2965865 0.7798769 -0.5425033 0.6678492 0.509576 -0.06659746 -0.04434949 0.9967939 -0.9670045 -0.1958002 0.1629867 -0.01732307 -0.1234404 0.9922008 -0.5511923 0.6916165 0.4667479 -0.5155127 -0.2585677 0.8169392 -0.9669659 -0.1957273 0.1633036 -0.1231319 -0.07757246 0.9893538 -0.4233223 -0.5871976 0.6899256 -0.8223389 -0.2817062 0.4943689 -0.8249568 -0.280313 0.4907859 -0.1991127 -0.1872043 0.9619297 -0.443786 0.002238452 0.8961301 -0.2562942 -0.418173 0.8714612 -0.1578968 -0.8618395 0.4819765 -0.1595803 -0.8626821 0.4799101 -0.006659388 -0.3959352 0.9182544 -0.07708936 -0.6546418 0.7519983 -0.6044819 -0.2907199 0.7416762 -0.7346677 -0.6418422 0.2197775 -0.03557789 -0.08521366 0.9957274 -0.4738531 -0.6267684 0.6185667 -0.7472215 -0.2794671 0.6029579 -0.6684689 -0.6746053 0.3131405 -0.1463479 -0.1600334 0.9762027 -0.2249761 0.1057454 0.9686092 -0.3093082 -0.9453105 0.1035217 -0.3179923 -0.9451821 0.07424122 -0.004838705 -0.1461242 0.9892545 -0.4029277 0.3682271 0.837889 -0.1394283 -0.7907665 0.596027 -0.2112852 -0.8986592 0.3844088 -0.145586 -0.7848039 0.6024014 -0.4223505 -0.258162 0.8688916 -0.3879827 -0.5151663 0.7642469 -0.4336731 0.5930883 0.6783613 -0.4199403 -0.2555695 0.870824 -0.8270919 0.3113735 0.4679375 -0.233504 -0.8739588 0.42623 -0.9294033 -0.1858044 0.3188828 -0.9890407 -0.1199219 0.08612459 -0.4287738 0.01431697 0.9032985 -0.7265897 -0.2610975 0.6355278 -0.1750259 -0.3267203 0.9287733 -0.7989145 -0.5995194 0.04808539 -0.7856584 -0.6103129 0.1012872 -0.9164894 -0.1999554 0.346504 -0.3535817 0.2592366 0.8987638 -0.3377861 -0.4909954 0.8030095 -0.1381797 0.01972502 0.9902108 -0.5794922 0.09215855 0.8097503 -0.8216862 0.2886493 0.49144 -0.06754666 -0.4884814 0.869956 -0.8753169 0.4287763 0.2235423 -0.6596589 -0.6153883 0.4314479 -0.1953522 -0.8546651 0.4810251 -0.9276218 -0.1887215 0.3223387 -0.01651465 -0.08501321 0.996243 -0.2555232 0.1854165 0.9488566 -0.8904004 -0.2184777 0.3993176 -0.3042103 -0.9374265 0.1693751 -0.2527514 -0.03671687 0.9668344 -0.2825412 -0.9343434 0.2171933 -0.9797336 -0.07595664 0.1853448 -0.2657637 0.2164704 0.9394202 -0.6480621 -0.2696652 0.7122474 -0.4201316 0.3622039 0.8320444 -0.1334688 -0.2787448 0.9510455 -0.2881945 -0.2055729 0.9352453 -0.3034916 -0.2067422 0.9301348 -0.1901207 -0.8128539 0.550566 -0.2099145 0.1214776 0.970144 -0.7471598 0.2076311 0.6313807 -0.8643904 0.4297327 0.2610727 -0.3264843 -0.1964807 0.9245558 -0.04757863 -0.1086387 0.992942 -0.1482715 -0.7843019 0.6024003 -0.7729455 -0.6105399 0.1726159 -0.6140965 -0.6231095 0.484376 -0.7509087 -0.6222298 0.2212833 -0.9874553 -0.07713758 0.1377745 -0.2345268 -0.1852675 0.954292 -0.2993218 -0.9125927 0.2785337 -0.4556978 0.0498569 0.8887373 -0.1090314 -0.6571524 0.7458304 -0.06100219 -0.1761527 0.9824709 -0.8446994 -0.2218766 0.4870872 -0.02142852 -0.06938588 0.9973598 -0.8387449 -0.2253565 0.4957032 -0.5983694 -0.2490273 0.7615376 -0.5982196 -0.2081818 0.7738177 -0.5852138 -0.583113 0.563475 -0.1385855 -0.06268662 0.9883645 -0.08189046 -0.004367113 0.9966318 -0.07257747 -0.01209264 0.9972895 -0.9371253 -0.114209 0.3297768 -0.7467191 -0.6018961 0.2830757 -0.2217409 -0.1737763 0.9594961 -0.5162917 -0.2409898 0.821807 -0.2837343 -0.4121552 0.8658077 -0.3832688 -0.918306 0.09909158 -0.7652527 -0.2158922 0.6064478 -0.4252427 0.05624586 0.90333 -0.2722628 -0.4058113 0.8724622 -0.8098047 0.3502553 0.470678 -0.9390988 -0.1104509 0.325414 -0.1179328 -0.1365325 0.9835907 -0.3807162 -0.92107 0.08176404 -0.8032684 0.3524338 0.4801565 -0.7391905 -0.2273188 0.6339744 -0.08789819 -0.06204164 0.9941955 -0.2724592 -0.8812931 0.38612 -0.6438897 0.1813733 0.74331 -0.5083946 -0.5635316 0.6511276 -0.2426375 -0.8712922 0.4265876 -0.4209359 -0.2177496 0.880567 -0.7785205 0.3939654 0.4885665 -0.8560599 -0.1518524 0.4940671 -0.2155296 -0.3295186 0.9192195 -0.4322732 -0.2151126 0.8757092 -0.1386334 -0.2601408 0.9555667 -0.07543814 -0.4844929 0.8715365 -0.2736969 -0.02184861 0.9615679 -0.4880585 -0.5248698 0.6973597 -0.4283713 -0.1872823 0.8839817 -0.1147253 -0.6530627 0.7485635 -0.7038359 -0.59471 0.3885036 -0.6705312 -0.602581 0.4327635 -0.03596067 -0.1059445 0.9937216 -0.1817461 -0.7031091 0.6874635 -0.01763528 -0.2125524 0.9769906 -0.366064 -0.9138653 0.1756344 -0.4161446 -0.4948033 0.762885 -0.858354 -0.1567909 0.4885131 -0.8330361 -0.5444946 0.09786009 -0.2540116 -0.8400182 0.4794242 -0.833733 0.4893327 0.2558179 -0.100468 -0.2047916 0.9736359 -0.3711519 -0.8999735 0.2286789 -0.9388075 -0.1177002 0.3237084 -0.8351545 0.4745348 0.2780893 -0.06220388 -0.1752344 0.9825598 -0.2345507 -0.8021805 0.5490832 -0.6156725 -0.5559874 0.5584134 -0.6589954 -0.5756996 0.4840403 -0.3427748 -0.8970755 0.2788567 -0.4505958 -0.891204 0.05214309 -0.2393688 -0.7877396 0.5675991 -0.01321458 -0.102563 0.9946388 -0.3855071 -0.44072 0.8106481 -0.8186484 -0.5481569 0.1712857 -0.8150557 -0.5506132 0.1803037 -0.2115617 -0.3182148 0.9241109 -0.8503832 -0.4948264 0.1788721 -0.805869 -0.1615843 0.569619 -0.8593325 -0.155839 0.4870955 -0.6069392 -0.531539 0.5908394 -0.1762925 -0.2781108 0.9442328 -0.9727215 -0.01982164 0.2311279 -0.4126348 -0.8771857 0.2455154 -0.7933362 -0.5470636 0.2670941 -0.04223001 -0.3219508 0.9458141 -0.3198277 -0.4020495 0.8579432 -0.7831024 -0.5517983 0.2868262 -0.4663494 0.1231771 0.8759827 -0.7763509 0.3789589 0.5036561 -0.802957 0.4360236 0.4063785 0.002168297 -0.1295943 0.9915648 -0.1259771 -0.5765411 0.8072981 -0.3333494 0.02902859 0.9423565 -0.613761 0.2012092 0.7634216 -0.04407566 -0.1307057 0.9904411 -0.5239788 -0.2083652 0.8258513 -0.2645217 -0.008614599 0.9643414 -0.5195237 -0.4923329 0.6983578 -0.3301458 -0.8605003 0.387999 -0.7461039 -0.5372043 0.3933709 -0.1964361 -0.6990876 0.6875241 -0.1943776 -0.6983339 0.6888739 -0.8015876 0.5646216 0.1966208 -0.3419815 -0.8352158 0.4306546 -0.08937245 -0.4935295 0.865125 -0.5904754 0.2004104 0.7817766 -0.07774794 -0.5033214 0.8605946 -0.5078877 -0.4943159 0.7054799 -0.6138013 -0.2012679 0.7633735 -0.309343 -0.3658095 0.8777758 -0.01535779 -0.2095417 0.9776791 -0.917163 -0.08492112 0.3893589 -0.8670238 -0.482739 0.1234214 -0.02415221 -0.2038376 0.9787068 -0.1913813 -0.02299755 0.9812463 -0.581559 0.2206045 0.7830217 -0.7471671 -0.5397438 0.3878378 -0.9727904 -0.004186451 0.2316495 -0.4734854 -0.870209 0.1361902 -0.260511 -0.3286293 0.9078199 -0.8037624 0.5345398 0.261215 -0.05183857 -0.3293111 0.9427974 -0.4385831 -0.1939399 0.8775148 -0.3069111 -0.8210441 0.4813442 -0.1750099 -0.2746309 0.9454891 -0.1411795 -0.1373023 0.9804165 -0.9703719 -0.01202255 0.2413172 -0.5036925 -0.8583138 0.09793645 -0.4132685 -0.4222147 0.8068109 -0.4024621 -0.4207574 0.8130114 -0.8305343 -0.487284 0.2697538 -0.6975793 -0.5233125 0.4894153 -0.7013797 -0.5226538 0.4846644 -0.7625298 -0.1138085 0.6368641 -0.08826291 -0.06343704 0.9940752 -0.8642163 -0.09756046 0.4935711 -0.4559457 -0.8656561 0.2067683 -0.2828055 -0.7701828 0.5716988 -0.7696634 0.504998 0.3906348 -0.8306414 -0.4879971 0.2681303 -0.1547777 -0.5715318 0.8058508 -0.1004312 -0.1873985 0.9771363 -0.3670293 -0.8242557 0.4311521 -0.3016866 -0.1691733 0.938278 -0.3352257 -0.1672045 0.927182 -0.3323515 -0.3628905 0.8705476 -0.09249335 -0.1718572 0.9807702 -0.8942309 -0.4334043 0.1118567 -0.8960041 -0.4314514 0.1050066 -0.6337963 0.3128473 0.70741 -0.6335397 -0.4950934 0.594567 -0.383615 -0.8281871 0.4085898 -0.4006479 0.1135072 0.9091741 -0.2681509 -0.1626197 0.9495525 -0.06068617 -0.3196588 0.9455873 -0.1620737 -0.5554811 0.8155813 -0.9182904 -0.019028 0.3954499 -0.8953599 -0.04706496 0.4428495 -0.6270539 -0.4813163 0.6124852 -0.766577 0.4926227 0.4119255 -0.8402567 -0.07432734 0.53707 -0.767066 0.6128757 0.1897187 -0.9678215 0.04494118 0.2475925 -0.3127039 0.05577361 0.9482118 -0.2874945 -0.7534111 0.5913701 -0.03354686 -0.1249133 0.9916003 -0.04507684 -0.06874769 0.9966153 -0.3651821 0.09582799 0.9259909 -0.9376718 0.04896992 0.3440545 -0.1343197 -0.1267786 0.9827947 -0.2705901 -0.7190012 0.6401705 -0.05143332 -0.1601563 0.9857509 -0.3308095 -0.3412003 0.8798565 -0.7308773 0.4439234 0.5184116 -0.9775652 0.07780337 0.1957373 -0.05415886 -0.1020927 0.9932996 -0.03823155 -0.2310773 0.972184 -0.8511698 -0.05528002 0.5219714 -0.2241736 -0.6891577 0.6890631 -0.97462 0.1211496 0.1882516 -0.7796483 -0.4870091 0.3936629 -0.54135 -0.4629754 0.7018505 -0.138422 -0.4856331 0.8631339 -0.1455472 -0.4900656 0.8594485 -0.8980793 0.01681226 0.4395124 -0.4782237 0.1884761 0.8577756 -0.7659123 -0.4656794 0.4433071 -0.1790198 -0.1294828 0.9752877 -0.2530316 -0.3031578 0.9187331 -0.8807945 -0.4061563 0.243389 -0.6639342 0.4163707 0.6211497 -0.2235199 0.01764512 0.9745396 -0.1900532 -0.2567369 0.9476107 -0.04796856 -0.09917098 0.9939135 -0.8246898 -0.02886688 0.5648483 -0.9799966 0.1426461 0.1387758 -0.747617 0.6068096 0.2699093 -0.1069331 -0.4332359 0.8949146 -0.2815902 -0.3024891 0.9106082 -0.706575 0.6529967 0.2726668 -0.7791565 -0.4368935 0.4494879 -0.8575243 -0.4339704 0.2762644 -0.363426 -0.7517843 0.55022 -0.4993461 -0.4033926 0.7667645 -0.5411436 -0.8316565 0.1245439 -0.1990771 0.00359106 0.9799773 -0.7305477 -0.4757997 0.489811 -0.9180701 -0.3810038 0.1094694 -0.005267024 -0.1087839 0.9940515 -0.01658231 -0.1370509 0.9904252 -0.9705644 0.1935683 0.1433043 -0.9722989 0.2058396 0.110748 -0.4821404 -0.8305374 0.2788338 -0.6921382 -0.4579845 0.5578485 -0.5634648 -0.8238031 0.06209617 -0.173697 -0.2303963 0.9574691 -0.8827837 0.02112007 0.4693046 -0.9539325 0.131238 0.269795 -0.08759278 -0.3471713 0.9337022 -0.7304595 0.547455 0.4083161 -0.9303991 0.08576762 0.3563726 -0.4337468 -0.3929459 0.8108375 -0.08210587 -0.3341512 0.9389364 -0.6403316 -0.4604056 0.6148189 -0.820172 -0.01664 0.5718752 -0.9498634 0.1813137 0.2547252 -0.6593495 -0.04703414 0.7503639 -0.7274118 -0.4231066 0.5402342 -0.6259885 0.3271513 0.7078915 -0.5117855 -0.8179866 0.2626286 -0.8843201 -0.3979858 0.2441009 -0.882289 -0.4230985 0.206286 -0.3637758 -0.7512308 0.5507446 -0.4492943 -0.8091836 0.3786247 -0.7301345 0.5472949 0.4091114 -0.7716489 -0.002250969 0.6360449 -0.7871779 -0.4299517 0.4421457 -0.9471323 0.168332 0.2731387 -0.3065375 0.0968163 0.9469221 -0.8817802 0.08010393 0.4648089 -0.383076 -0.3489955 0.8552514 -0.8891587 0.07330787 0.4516888 -0.5427285 -0.8157246 0.2000984 -0.1568275 -0.4593962 0.8742771 -0.5998038 -0.4139369 0.6847567 -0.4598242 -0.779753 0.4249084 -0.9566674 0.2911786 -0.001598834 -0.6243214 -0.03395414 0.7804294 -0.6904539 0.5051629 0.5177682 -0.8881503 0.08035612 0.4524731 -0.9600697 0.2549746 0.1151273 -0.7075062 0.649702 0.2780691 -0.452542 0.2244756 0.8630276 -0.4469054 0.2238681 0.8661171 -0.7680484 0.03023576 0.6396777 -0.500513 -0.3909012 0.7724527 -0.2766311 -0.2942901 0.9148052 -0.5311506 -0.7635138 0.3673223 -0.5075695 -0.3898925 0.768347 -0.4444664 -0.7553222 0.4815995 -0.7759363 0.02385067 0.6303604 -0.6577394 0.4241436 0.6224799 -0.6101452 -0.786087 0.09894549 -0.9135991 0.1447448 0.379981 -0.4255072 -0.7454332 0.5131013 -0.1071122 -0.03177565 0.9937391 -0.9453574 0.239511 0.2212101 -0.8324003 -0.3956403 0.3880449 -0.7946169 -0.4142457 0.4438292 -0.5388471 -0.7700701 0.3415198 -0.1909095 -0.227744 0.9548227 -0.9430741 0.2105174 0.257476 -0.9740717 0.2119632 0.07909488 -0.9854561 0.1483615 0.08285707 -0.5629444 -0.8227664 0.07841479 -0.9738849 0.2130944 0.07835149 -0.9576445 0.2766963 0.07972675 -0.9570556 0.279243 0.07789635 -0.5897784 -0.803746 0.07844656 -0.9364467 0.3417814 0.07907623 -0.9389268 0.3332744 0.08570027 -0.5615314 -0.8236982 0.07876336 -0.9047218 0.4158601 0.09240674 -0.4483963 -0.8903326 0.07904791 -0.9114497 0.4049288 0.07274788 -0.8807827 0.4662328 0.08275693 -0.8759392 0.476968 0.07233417 -0.02180308 -0.9966967 0.07823377 -0.8372483 0.5434629 0.06052708 -0.8477542 0.5240063 0.08203798 -0.808099 0.5820032 0.09082025 -0.5899526 -0.8036175 0.07845419 -0.8087196 0.5808859 0.09243565 -0.7552075 0.6416531 0.1339517 -0.7631329 0.6403953 0.08673119 -0.7182303 0.6897807 0.09136658 -0.7184233 0.6894894 0.09204638 -0.6659234 0.7389316 0.1025972 -0.1272124 -0.9905353 -0.0515446 -0.7114235 -0.6985597 0.07675403 -0.7120817 -0.6979132 0.07653021 -0.6162101 -0.7836689 0.07841241 -0.6636917 -0.7438831 0.07842987 -0.02095949 -0.9966971 0.07845741 -0.06451106 -0.9948106 0.07867884 -0.1986517 -0.9776966 -0.06816768 -0.559179 -0.1698271 -0.8114663 -0.6635416 -0.7440217 0.07838618 -0.6884859 -0.7209985 0.07841223 -0.6079385 -0.7868686 0.1060597 -0.1199175 -0.9896818 0.07842129 -0.6191861 -0.7812245 0.07935267 -0.6595938 -0.7456136 0.09485059 -0.6090557 -0.7891594 0.07923835 -0.6829718 -0.2384981 -0.6904116 -0.1534917 -0.9850307 0.07845395 -0.7336782 -0.6749522 0.07846045 -0.219699 -0.9724072 0.07846438 -0.06427091 -0.9948315 0.07861119 -0.6999576 -0.7037344 0.1217265 -0.7557725 -0.6501199 0.07843691 -0.7559983 -0.6498476 0.07851636 -0.7690627 -0.2608067 -0.5835432 -0.7770318 -0.6245427 0.07853752 -0.3391833 -0.930001 -0.1416087 -0.7431779 -0.6567886 0.127732 -0.7762638 -0.6255074 0.07845354 -0.1871964 -0.9791879 0.07841253 -0.253938 -0.9640361 0.07842308 -0.8767784 -0.4747411 0.07668417 -0.7998775 -0.5950177 0.07842153 -0.7865911 -0.6036915 0.1297349 -0.8195074 -0.5676739 0.07844895 -0.8544864 -0.5119491 0.0880987 -0.3177524 -0.9449238 0.07843846 -0.1213104 -0.9894891 0.07870924 -0.3514552 -0.9329092 0.07848393 -0.7999306 -0.5949428 0.07844811 -0.4164887 -0.8527939 -0.315087 -0.8393301 -0.5379385 0.07840377 -0.319233 -0.9444531 0.07809466 -0.9154419 -0.3959577 0.07199752 -0.9222906 -0.3807908 0.06617003 -0.8315259 -0.545735 0.1036251 -0.819038 -0.5683462 0.07848244 -0.1872218 -0.9791826 0.07841825 -0.2201268 -0.9723109 0.07845944 0.9388285 0.3333719 -0.08639574 -0.8403761 -0.5362086 0.07904738 -0.8700142 -0.4865655 0.07955724 -0.8997824 -0.4290009 0.07968699 0.1531367 -0.2881664 0.9452562 -0.4816485 -0.8421251 -0.2425698 -0.2826846 -0.9559955 0.07849794 -0.254076 -0.963997 0.07845616 -0.9403146 -0.3311393 0.0784564 -0.3497999 -0.9335331 0.07846111 -0.250617 -0.9619784 0.1085763 -0.9203112 -0.378012 0.10067 -0.9697207 -0.2312669 0.07846873 -0.9500457 -0.3020934 0.07844084 -0.960262 -0.2678495 0.07844561 -0.9762852 -0.1992419 0.08467501 -0.9307698 -0.3568739 0.07942688 -0.9403763 -0.3202478 0.1146028 -0.9348165 -0.3460173 0.07993847 -0.9697623 -0.2311058 0.07842862 -0.3821775 -0.920755 0.07842528 -0.9825704 -0.1533922 0.1050058 -0.9858253 -0.1445288 0.08520579 -0.538457 -0.833035 -0.1269516 -0.9521558 -0.2948021 0.08056849 -0.9855561 -0.1478182 0.08263683 -0.9562571 -0.2811416 0.08082115 -0.9617503 -0.2521166 0.1071155 -0.3157395 -0.94325 0.1028995 -0.4142045 -0.9067969 0.07844889 -0.9911758 -0.1026069 0.08391946 -0.9771378 -0.184012 0.1064963 -0.9951021 -0.06269609 0.07642704 -0.9946781 -0.06680756 0.0784372 0.9803757 -0.06230354 0.187034 -0.9964501 -0.03052926 0.07845622 -0.983824 -0.1601608 0.08024305 -0.5977216 -0.7032784 -0.3848744 -0.47632 -0.8757646 0.07845795 -0.9871127 -0.1383798 0.08037358 -0.9868475 -0.1198869 0.1084399 -0.445933 -0.8916255 0.07840847 -0.9962221 -0.0124067 0.08595263 -0.3830035 -0.9172481 0.1093816 -0.9932124 0.07717746 0.08702206 -0.9926877 0.07201468 0.09687757 -0.5077447 -0.857932 0.07840949 -0.5370441 -0.8398986 0.07844883 -0.9926586 -0.05464839 0.1079007 -0.9965267 -0.02200496 0.08031541 -0.9877651 0.1362738 0.07582747 -0.9967777 0.001641869 0.08019787 -0.9932901 0.01184028 0.1150421 -0.9958497 0.04500269 0.07910907 -0.9938209 0.07854741 0.07842433 -0.9894756 0.1215997 0.07843357 -0.5077219 -0.857945 0.07841521 -0.3854843 -0.9193089 0.07920336 -0.9902245 0.09199035 0.104848 -0.9936278 0.08275693 0.07652002 -0.08116298 -0.5352522 0.840784 -0.2675516 -0.4633946 0.8447968 -0.2803637 -0.1306458 0.9509617 -0.4315485 -0.1110911 0.8952233 -0.1933092 0.9777443 0.08153307 -0.1665482 0.9847054 0.05115568 -0.6398972 -0.7664529 0.05551254 -0.1702371 0.9254134 0.3385698 0.4398763 0.8959019 0.06219744 -0.6354531 -0.7554605 0.1596215 -0.0339092 0.9981245 0.05096751 -0.9640242 0.2621217 0.04415667 -0.9748021 0.2168468 0.05232942 -0.1287486 0.9879739 0.08562374 -0.1003058 0.9935806 0.05231124 -0.9610539 0.1844948 0.2057597 -0.6659826 -0.7440738 0.05311787 -0.6655123 -0.7445591 0.05220395 -0.9669042 0.2497185 0.0523166 -0.9578903 0.2823258 0.05233067 -0.9472997 0.3160483 0.05231499 -0.943992 0.3269355 0.04463481 -0.7114415 -0.7008695 0.05131202 -0.9441975 0.2470216 0.2178795 -0.935884 0.3483994 0.05233538 -0.9249414 0.3182139 0.2079021 -0.7123305 -0.6997964 0.053577 -0.9192511 0.3911039 0.04489099 -0.7597621 -0.6481698 0.05135834 -0.9230622 0.3810761 0.05231857 -0.7590479 -0.648934 0.05225968 -0.9092961 0.4128471 0.0523265 -0.9000014 0.3854197 0.2035911 -0.8339256 0.5104665 -0.209743 -0.8679181 0.4945509 0.04623407 -0.8943378 0.4443227 0.05231857 -0.8782932 0.4752496 0.05233436 -0.8943306 0.4443381 0.05231106 -0.8414054 -0.5378651 0.05232942 -0.8703474 0.4480295 0.2043653 -0.8639032 0.5015081 0.04648631 -0.7875318 0.5721171 -0.2290758 -0.8023504 -0.5945433 0.05246204 -0.8614532 0.5051343 0.05232363 -0.8054949 -0.5892783 0.06268143 -0.8432878 0.5349092 0.05232584 -0.8432709 0.5349367 0.0523175 -0.8341264 -0.5479603 0.0630303 -0.8339086 0.4994541 0.234824 -0.8184533 0.572705 0.046296 -0.8771123 -0.4774866 0.05177521 -0.8227444 0.5659997 0.05230671 -0.8028357 0.5938976 0.05235183 -0.7884046 0.5700728 0.2311609 -0.7389029 0.6587026 -0.1418927 -0.7823112 0.620684 0.05235189 -0.7798067 0.6233732 0.05751055 -0.9087035 -0.4141978 0.05194485 -0.7614405 0.6461198 0.05232107 -0.9086877 -0.4142286 0.05197632 -0.7395291 0.6710896 0.05230188 -0.7355239 0.6758272 0.04756343 -0.8773098 -0.477029 0.05264097 -0.7166664 0.6954474 0.05236822 -0.7054657 0.6881773 0.1694997 -0.6892609 0.7228517 0.04904013 -0.03262734 0.9980908 0.05244398 -0.01751202 -0.998462 0.0526033 -0.639248 0.767737 0.04406553 -0.05429965 -0.9971892 0.05162775 -0.6388136 0.7681438 0.04327189 -0.6919087 0.720085 0.05234426 -0.034913 -0.9973882 0.0632289 -0.6982125 0.706089 0.1180589 -0.6582144 0.7509098 0.05374479 -0.9330382 -0.2361621 -0.2714172 -0.6711547 0.7320126 0.1170855 -0.9841321 -0.1739534 0.03499084 -0.9346754 -0.3516349 0.0522952 -0.9345176 -0.3520058 0.05262047 -0.1053848 -0.9930546 0.05231529 -0.9466426 -0.3180082 0.05233085 -0.1053964 -0.9930527 0.05232733 -0.5588258 0.8179795 -0.1364673 -0.1704406 -0.983977 0.05233931 -0.6184705 0.7823753 0.07337093 -0.9362266 -0.3140415 0.1576642 -0.959309 -0.2797623 0.03820419 -0.1719207 -0.9836085 0.05438506 -0.9601847 -0.2766993 0.03851026 -0.9867174 -0.1365255 -0.0880323 -0.174087 -0.9835656 0.04788166 -0.175833 -0.983128 0.05041939 -0.6079368 0.7917093 0.06007891 -0.9444197 -0.3027935 0.1280142 -0.6030266 0.794743 0.06886583 -0.5810035 0.8122241 0.05222254 -0.954426 -0.2491967 0.1642323 -0.9425971 -0.3079047 0.1292498 -0.9754343 -0.2162218 0.04214423 -0.9758638 -0.2123767 0.05085361 -0.5357741 0.8426795 0.05326914 -0.6018113 0.7956116 0.06946521 -0.2415008 -0.9689871 0.05235791 -0.2440143 -0.9681731 0.05565959 -0.3483396 0.8319205 -0.4319353 -0.2191067 -0.9741194 0.05553221 -0.2967103 -0.9451008 0.1369217 -0.3068306 -0.9504275 0.05042433 -0.3286519 -0.8841773 -0.3319916 -0.9892454 -0.1387493 0.04628545 -0.987509 -0.1481142 0.05374294 -0.5720148 -0.8191249 0.04281836 -0.3088248 -0.9496232 0.05331957 -0.9925928 -0.110901 0.04960542 -0.5349215 0.8432807 0.05231511 -0.4948438 0.8674046 0.05233532 -0.4608821 0.8862394 0.04655742 -0.9955341 -0.08080852 0.04880517 -0.3635672 -0.9308086 0.03760647 -0.9798051 -0.1219657 0.1584501 -0.3730686 -0.9263225 0.05240917 -0.9970918 -0.06411021 0.04120582 -0.998859 -0.0420624 0.02262014 -0.4894341 0.8702726 0.05549776 -0.4365733 -0.8982535 0.05044203 -0.9988391 -0.02043008 0.04362428 -0.9318972 0.1315037 -0.3380452 -0.9566002 -0.09898036 0.2740786 -0.452079 0.8904426 0.05231404 -0.998528 -0.01424777 0.05233454 -0.4389904 -0.8968527 0.05424594 -0.4140468 0.9087518 0.05230212 -0.9970318 0.06354683 0.04346811 -0.9984396 0.01943683 0.05235159 -0.4139442 0.9087917 0.05242311 -0.4984034 -0.8654806 0.0503751 -0.9974415 0.04869168 0.05234056 -0.9984395 0.01946407 0.05234313 -0.4994231 -0.8647825 0.05222821 -0.9729752 -0.02158433 0.2298989 -0.3573905 0.9325704 0.05083894 -0.360077 0.9317094 0.04756242 -0.9074189 0.2477 -0.3394641 -0.9952147 0.08250606 0.05235117 -0.5602074 -0.8265719 0.05428385 -0.2822768 0.9560117 -0.07976013 -0.5929192 -0.7286509 -0.3428044 -0.3017555 0.9520603 0.05024951 -0.6278136 -0.7772426 0.0417639 -0.9611682 0.01770049 0.2753953 -0.278574 0.9572218 0.07825005 -0.9841319 0.1720744 0.04330056 -0.991924 0.1155303 0.05234247 -0.4078777 0.9100365 0.07395529 -0.5538843 -0.830415 0.06019282 -0.2437456 0.9215582 0.30219 -0.267799 0.9620527 0.05233097 -0.9906029 0.122848 0.06011927 -0.9808816 0.1896458 0.04365438 -0.5737738 -0.7975353 0.1863363 -0.5510289 -0.8323204 0.06008356 -0.9812773 0.1882413 0.04074466 -0.2099243 0.9743587 0.08097523 -0.2327634 0.9711259 0.05230402 -0.6073205 -0.7933711 0.04152232 -0.9549176 0.0906313 0.2826987 -0.612387 -0.7888419 0.05206578 -0.9898915 0.1284238 0.06018662 0.1172053 -0.04960525 0.9918681 0.1462199 -0.662383 0.7347575 -0.01437461 -0.4793305 0.8775168 0.028934 -0.07641053 0.9966565 0.3224313 -0.7099977 0.6260522 0.02938216 -0.07576829 0.9966925 -0.2943766 -0.5938127 0.7488183 0.5198222 -0.5183749 0.6790231 0.9550685 -0.219133 0.1995615 -0.005344152 -0.01378029 0.9998909 0.5046094 -0.3650165 0.7823889 -0.2440847 -0.6085982 0.755004 0.5550248 0.5057543 0.6604242 0.4819107 0.4307448 0.7630341 -0.4840415 0.4144524 0.7706705 -0.1116582 0.02205008 0.9935021 -0.5590579 -0.6093377 0.5622828 0.8697388 -0.1829792 -0.4583373 0.04262304 -0.1145005 -0.9925085 0.05268371 -0.02464622 -0.9983071 -3.95631e-4 -1.69454e-4 -1 -0.007953703 -0.03157329 -0.9994698 -0.3497 -0.3938449 -0.8500565 -0.006382167 -0.03107947 -0.9994966 0.02878159 -0.04273009 -0.998672 0.02326387 -0.03958779 -0.9989452 0.2965071 -0.338703 -0.8929523 0.01064795 -0.001610577 -0.9999421 0.5455476 -0.357224 -0.7581351 -0.9754287 -0.2196232 -0.01745396 -0.9754179 -0.219661 -0.01757937 -0.4678872 0.8824298 0.04898244 -0.4825559 0.8756908 -0.01747792 -0.9890372 -0.1462476 -0.02042502 -0.9886866 -0.1489487 -0.01769596 -0.4484266 0.8878574 0.1030674 -0.4207267 0.9068898 -0.02323591 -0.4298229 0.9028961 -0.005577325 -0.9966714 -0.0796439 -0.01740431 -0.4392735 0.8953562 0.07332259 -0.3288879 0.9212524 -0.20767 -0.9966677 -0.07967168 -0.01749485 -0.9428884 0.09868973 -0.3181536 -0.9997988 -0.009756088 -0.0175262 -0.9936026 0.106334 -0.03803956 -0.9998006 -0.009722232 -0.01744079 -0.3651609 0.9308909 -0.00998938 -0.3703891 -0.2765882 0.8867417 -0.3661822 0.9304234 -0.01493412 -0.9995384 0.02487546 -0.01744788 -0.07448691 -0.1143052 0.9906494 -0.9660372 0.1297671 -0.2234565 -0.9978411 0.05376601 -0.03771615 -0.3303771 0.9436885 -0.01741337 -0.3605408 0.9326912 -0.009874403 -0.2936491 0.9556257 -0.0234456 -0.2967298 0.9548028 -0.01741373 -0.9964178 0.07944518 0.0289883 -0.9953217 0.09502667 -0.01745557 0.991331 -0.1295596 0.02184504 -0.2830036 0.9586051 0.0313912 -0.2150823 0.9596264 -0.1812646 -0.9915933 0.128193 -0.01758611 -0.9916152 0.1280422 -0.01744776 -0.2395082 0.9707781 -0.01502621 -0.238111 0.9711832 -0.0103203 -0.2334885 0.9723029 -0.01049596 -0.2056641 0.9784668 -0.01747125 -0.07951229 0.04789179 0.9956828 -0.9802604 0.1969454 -0.01738142 -0.9802445 0.1970187 -0.01745396 -0.9649476 0.2618586 -0.01750016 -0.1711218 0.985095 -0.01747328 -0.1711868 0.9850822 -0.01755589 -0.1073127 0.9941202 -0.01446312 -0.8686609 0.4411376 -0.2254459 -0.06886297 -0.3052083 0.9497925 -0.9599297 0.2785156 0.03105074 -0.9443448 0.3284939 -0.01745682 -0.9316358 0.3629738 -0.01746404 -0.9190947 0.393971 -0.007215142 -0.1054733 0.9942802 -0.01680415 0.003313064 -0.2927587 0.9561807 -0.004060864 0.2758516 -0.9611917 -0.01960945 -0.9996621 -0.01706898 0.8955146 -0.4405753 0.06282675 -0.9186406 0.3946906 -0.01785916 -0.02609044 -0.9993747 -0.02386677 -0.06337571 -0.9979133 -0.01235622 -0.0676862 -0.997574 -0.01626622 -0.04061281 0.9990204 -0.01757621 -0.9048503 0.4255846 -0.01112198 -0.04064452 0.9990199 -0.01753437 -0.09540665 -0.9951528 -0.02384459 0.005694806 -0.2894176 0.957186 -0.1235638 -0.9922417 -0.01373338 -0.127839 -0.9916419 -0.01742482 -0.4702919 0.5193901 -0.7134841 0.02238893 -0.2323033 0.9723858 -0.1633145 -0.9864202 -0.01742607 -0.1633569 -0.9864127 -0.01745802 -0.002412796 -0.33963 0.9405561 -0.1953482 -0.9805786 -0.01746249 -0.1953184 -0.9805849 -0.0174427 0.06128561 -0.1278339 0.9899004 -0.8888717 0.4577916 -0.01827269 -0.2301363 -0.9730021 -0.01743781 -0.2301294 -0.9730039 -0.0174337 -0.8892943 0.457002 -0.0174604 -0.2647805 -0.9641511 -0.0174334 -0.004175961 -0.34211 0.9396507 -0.5890654 0.028301 0.8075897 -0.2994129 -0.9539637 -0.01747101 -0.2647314 -0.9641639 -0.01747655 0.1373554 0.02613466 0.990177 -0.2994625 -0.9539491 -0.01741987 -0.8717365 0.4896646 -0.0174452 -0.007709205 -0.3486497 0.9372215 -0.871717 0.4896983 -0.01746308 -0.3337629 -0.9424947 -0.01749712 -0.3336719 -0.9425281 -0.0174303 -0.8550117 0.5183154 -0.01744186 -0.5275217 0.5722437 -0.6278997 -0.1985732 -0.09827202 -0.9751468 -0.3673938 -0.9299021 -0.0174328 -0.01816862 -0.3665117 0.9302361 -0.3673328 -0.9299273 -0.01737689 -0.4007074 -0.9160391 -0.01749879 -0.4005992 -0.916088 -0.01741224 -0.5675567 0.06722211 0.8205856 -0.854973 0.5183824 -0.01734876 0.4665182 0.7099282 0.5276011 -0.4335808 -0.9009466 -0.01740336 -0.03468281 -0.3895548 0.92035 -0.4335899 -0.9009421 -0.01741278 -0.4630271 -0.8861721 -0.01746523 -0.4631597 -0.8861013 -0.01754629 -0.5353078 -0.8391616 -0.0961948 -0.4999237 -0.8658935 -0.01745629 -0.8170568 0.576293 -0.01745426 -0.8357311 0.5488615 -0.01745641 -0.8358443 0.5486925 -0.01734745 -0.5307241 -0.8423159 -0.09400022 -0.5586522 0.622729 -0.5478289 -0.5018644 -0.8649151 -0.007344782 -0.8170416 0.5763151 -0.0174328 -0.6140729 0.1752374 0.7695495 -0.3295859 -0.121599 -0.9362622 -0.5463918 -0.8373899 -0.01530873 -0.5790458 -0.8149297 -0.024405 -0.5536329 -0.8321179 -0.03272098 -0.7978219 0.6026413 -0.01743257 -0.7977912 0.6026811 -0.0174632 -0.6066582 -0.7947848 -0.01682758 -0.6533103 0.6843757 -0.3237525 -0.4591475 -0.2391239 -0.8555719 -0.606442 -0.7949379 -0.01737654 -0.6352186 -0.7721368 -0.01738393 0.6089277 0.7924976 0.03398114 -0.7774806 0.6286646 -0.01746165 -0.7774583 0.6286915 -0.01748293 -0.7544633 0.6561095 -0.01748508 -0.2705358 -0.0976606 0.9577436 -0.7067942 0.706388 -0.03818279 -0.6998596 -0.5812582 -0.4151329 -0.6248939 -0.7801432 0.02973675 -0.6627919 -0.7486003 -0.01745367 -0.6897062 -0.7238789 -0.01745831 -0.7508694 -0.6573384 -0.06404203 -0.7415943 0.6708483 7.2964e-4 -0.6387373 0.7279682 -0.2491527 -0.7494575 -0.5294643 -0.3974684 -0.7256482 0.6869723 -0.03878045 -0.6882626 -0.723065 -0.05892133 -0.703378 -0.7107734 -0.007789611 0.7085735 0.7044245 0.04134863 -0.7923673 -0.60659 -0.06482768 0.2145028 -0.9232583 0.3187206 -0.7932688 -0.4827257 -0.3710802 -0.7134932 -0.6974474 -0.06704115 -0.7214934 -0.69183 0.02861231 0.7346884 0.6764151 0.05191981 -0.4927038 0.8450798 0.2075645 -0.8328295 -0.5490956 -0.06992304 -0.5898638 0.7790724 -0.2123842 -0.8315854 -0.4262 -0.3561174 -0.7315529 -0.6784407 -0.06744283 -0.008494555 -0.3339543 0.942551 -0.7362968 -0.6727231 0.07287389 -0.839175 -0.5438331 -0.005574703 -0.1220759 -0.2024105 0.9716622 -0.7043156 0.7098844 0.001932382 -0.6831377 0.7300685 -0.01797127 -0.875557 -0.4823101 -0.02787756 -0.8595869 -0.5106925 -0.01742321 -0.8404914 -0.541545 -0.01741492 0.8654608 0.4964858 0.06693023 -0.6398162 0.7685235 0.002676844 -0.657233 0.7534791 -0.0177192 -0.5569691 0.8067116 -0.1974892 -0.8774282 -0.4793452 -0.01865124 0.4870823 -0.8656433 0.1158134 -0.6320587 0.7747242 -0.01744484 -0.6037507 0.7969825 -0.01744014 -0.07021516 -0.3582422 0.9309846 -0.9050324 -0.4253076 -0.00545752 -0.5362161 0.8411087 0.07077157 -0.9096822 -0.4149428 -0.01734364 -0.1026279 -0.1871817 0.9769496 -0.6032347 0.7975597 -0.002576351 -0.5758955 0.8173156 -0.01843297 -0.4763389 0.8420367 -0.2531316 -0.959173 -0.2446192 -0.1419456 -0.5482752 0.8361068 -0.01788115 -0.9365034 -0.3502269 -0.01739138 -0.9365183 -0.3501849 -0.01743859 -0.9477295 -0.3185977 -0.01744675 -0.08095496 -0.3509982 0.9328701 -0.9573789 -0.2885599 -0.01260834 -0.5310338 0.8473048 0.008830606 0.448548 -0.8921043 0.05435878 -0.9586626 -0.2840114 -0.01742976 -0.5178336 0.8553027 -0.01748543 -0.478043 0.8779566 -0.02583283 0.4730651 0.5273057 -0.7058032 0.8481064 0.04606932 -0.5278193 0.5209198 0.2164996 -0.825694 -0.02196413 0.04458826 -0.998764 -0.6054703 0.7956762 -0.0174691 -0.9991694 -0.03214502 -0.02504509 -0.9997171 -0.02237069 -0.008084833 -0.5495138 0.8113335 -0.1994308 -0.5748338 0.67734 0.4591041 -0.9997322 -0.0156607 -0.01703834 -0.9996467 -0.01415789 0.02249681 -0.9865598 0.07560592 -0.1448568 -0.9988433 0.04782086 -0.005026578 -0.5755303 0.8175802 -0.01809602 -0.5750626 0.8179228 -0.0174753 -0.9986736 0.04864579 -0.01686984 -0.5465414 0.8372396 -0.01795625 -0.9983654 0.05234831 -0.02294576 -0.5472866 0.8367642 -0.01740902 -0.9917067 0.1150887 -0.05720621 -0.9919225 0.1227379 -0.03202009 -0.4953585 0.8509572 -0.1746194 -0.9933313 0.1139864 -0.01732927 -0.9879007 0.1541163 -0.01733767 -0.9686117 0.217139 -0.1210047 -0.9830982 0.1830134 -0.004897892 -0.5189284 0.8546406 -0.01740938 -0.5169802 0.8558618 -0.01524025 -0.4766063 0.8787798 -0.02434635 -0.9819678 0.1886849 -0.01172268 -0.4621788 0.8865457 0.02067786 -0.9737851 0.2265915 -0.01997458 -0.9428519 0.294403 -0.1560676 -0.9655255 0.2596089 -0.01907163 -0.4737031 0.8801913 -0.02947717 -0.4424814 0.8966143 -0.01711648 -0.9660822 0.2576822 -0.01689225 -0.9467819 0.3206398 -0.02818131 -0.4370068 0.8994241 0.007840216 -0.9445787 0.3282448 -0.005160033 -0.3880385 0.9014812 -0.1917234 -0.3848185 0.9228318 -0.01721048 -0.3996047 0.9166727 0.005242466 -0.9338033 0.3573615 -0.01744329 -0.8794151 0.4289693 -0.2064324 -0.9164216 0.2992963 0.2656937 -0.8997734 0.4331791 -0.05257052 -0.9213458 0.3883528 -0.01743787 -0.3661749 0.9303721 -0.01799237 -0.9105908 0.4112543 -0.04116338 -0.8998725 0.4341856 -0.04138231 -0.8941413 0.444463 -0.05444258 -0.8970472 0.4402151 -0.03894877 -0.3219318 0.9467391 -0.006711721 -0.3350782 0.9331001 -0.1305632 -0.3341478 0.9423639 -0.01719909 -0.03173273 -0.9993402 -0.01767438 -0.8934549 0.446671 -0.0471552 -0.03545594 -0.9990681 -0.02461242 -0.1012952 -0.994705 -0.01736414 -0.1659688 -0.9861254 -0.003353238 -0.1018204 -0.9946353 -0.01825809 -0.173654 -0.9846435 -0.01792979 -0.2334102 -0.9723693 -0.004200041 -0.2994065 0.9538912 -0.02115625 -0.2514809 -0.9672615 -0.03409737 -0.83785 0.5430412 -0.05580067 -0.2877403 -0.9145651 -0.2842117 -0.2707155 0.9625718 -0.0129863 -0.2760713 -0.960979 -0.0174399 -0.3133205 -0.9494873 -0.01744389 -0.2740311 -0.9396172 -0.2050041 -0.4183349 -0.8923421 -0.1694748 -0.3184546 -0.9474242 -0.03121185 -0.3846234 -0.9223587 -0.03632444 -0.3594923 -0.9331408 -0.003705918 -0.4230058 -0.9061263 0.001147389 -0.4517358 -0.8914117 -0.03633338 -0.4852647 -0.8743646 -0.002210199 -0.8515532 0.5234193 -0.02982532 -0.5163094 -0.8555797 -0.03752613 -0.5438308 -0.8391916 0.002356529 -0.5774353 -0.8155828 -0.03732448 -0.2565155 0.9662609 -0.02323603 -0.6012691 -0.7990458 -0.001172959 -0.1979292 0.9200498 -0.3381308 -0.6552585 -0.7554014 0.002278625 -0.8183558 0.5746245 -0.0100311 -0.6360997 -0.770694 -0.03752398 -0.6918496 -0.7210784 -0.03728419 -0.7433807 -0.6678839 -0.03628039 -0.7079196 -0.7062925 -9.14589e-4 -0.5843067 -0.6689105 0.4595046 -0.2177333 0.9758718 -0.01633262 -0.8195893 0.5727013 -0.01693791 -0.7907738 -0.6111187 -0.03479468 -0.7499565 -0.6612324 -0.01836174 -0.7721308 -0.6352022 -0.01823103 -0.794664 -0.6068533 -0.01544171 -0.8315237 -0.5554885 0.0010643 -0.2161349 0.9761981 -0.0179724 -0.8341234 -0.5515506 -0.005494475 -0.779483 0.6262004 -0.01671308 -0.7798458 0.625708 -0.01816964 -0.1640532 0.986262 -0.01933676 -0.1623519 0.986592 -0.01667606 -0.08163493 0.9958759 -0.03958487 -0.8728884 -0.4878869 -0.005693554 -0.8711426 -0.4907004 -0.01799291 -0.9023057 -0.430749 -0.01731371 -0.7303296 0.6830873 -0.003223776 -0.9149043 -0.3996109 -0.05710828 -0.916256 -0.280605 -0.2858947 -0.8919696 -0.4504726 -0.03827148 -0.7353519 0.6774158 -0.01911759 -0.9295473 -0.368312 -0.01697731 -0.9344261 -0.354613 -0.0331276 -0.9533869 -0.279497 -0.1137323 -0.9302853 -0.3652842 -0.03371328 -0.7095217 0.7044683 -0.01742482 -0.03491926 0.9992207 -0.01840972 -0.9280381 -0.3435156 0.1440222 -0.03425741 0.9992607 -0.01745098 -0.9532653 -0.3016757 -0.01664841 -0.6335598 0.7335423 -0.2460036 -0.1001389 0.9948516 -0.01557409 -0.6605992 0.5301141 0.5315898 -0.9528312 -0.3031319 -0.01496112 -0.9600843 -0.2788785 -0.02156507 -0.9645371 -0.1844084 -0.1888434 -0.9631024 -0.2685695 -0.01744341 -0.9715892 -0.2360305 -0.01743942 -0.6852341 0.7281146 -0.01742219 -0.6584328 0.7524356 -0.0175206 -0.6852259 0.7281225 -0.01740968 -0.9759934 -0.2135434 -0.04285204 -0.9677473 -0.249898 -0.0318799 -0.9676418 -0.09809207 -0.2324806 -0.6585384 0.7523451 -0.01744085 -0.9859067 -0.1664205 -0.0170961 -0.6185672 0.5984783 0.5091154 -0.6004964 0.7745007 -0.1988792 -0.9861096 -0.1655804 -0.01307374 -0.6333382 0.7736786 -0.01744294 -0.6332037 0.7737929 -0.01725316 -0.9951007 -0.09810996 -0.0122087 -0.6050285 0.7960041 -0.01783531 -0.9991647 -0.02835971 -0.02942144 -0.9946895 -0.1005901 -0.02178061 -0.8317589 -0.3576266 -0.4245945 -0.6881479 0.02800643 0.7250298 -0.3111093 0.001041531 0.9503736 -0.4575003 -0.1114959 -0.8821917 -0.2366354 0.3523775 0.9054468 -0.5993726 0.7996001 0.03731304 -0.3503497 0.3605597 -0.8644373 -0.6214901 0.4148867 -0.6645444 -0.0797615 -0.0341627 0.9962285 0.5893239 0.2692081 0.7617246 0.6675888 0.4566828 -0.5880187 0.4866493 0.5773525 -0.6556194 0.3461231 0.3481485 -0.8712011 0.3147296 0.4252356 -0.8485989 0.3640562 0.6830028 -0.6332222 0.06323534 0.05093568 -0.996698 0.05178225 0.1149095 -0.9920255 0.1025993 0.6463874 0.7560799 0.1153796 0.5631473 -0.818262 0.05944269 0.7157445 -0.6958279 -0.03198379 0.5215361 -0.8526297 0.1175708 -0.1816669 -0.9763064 -0.06612306 0.3056316 -0.9498512 -0.2559496 0.7867267 0.5617393 -0.2544429 0.03707152 0.966377 -0.1867875 0.05473661 -0.9808744 -0.03492563 0.07313591 -0.9967103 -0.6440514 0.2263786 0.7307192 -0.5968353 0.2251214 -0.7701351 -0.2295327 -0.2766809 -0.9331466 -0.8117222 -0.01720011 -0.5837904 -0.3776804 -0.02276659 -0.9256562 -0.3606457 -0.07636338 -0.9295716 -0.6371942 -0.7037424 -0.3142138 -0.9941101 -0.1070228 0.01707142 -0.8075042 -0.5897285 0.01254355 -0.9299665 -0.3672403 -0.01724106 -0.367315 -0.9299371 0.01722693 -0.5896548 -0.8075582 -0.01252883 -0.06706583 -0.9975022 -0.02217161 0.319108 -0.9474647 0.0219317 0.5567766 -0.8304742 -0.01768141 0.800136 -0.5996854 0.01264363 0.9898261 -0.1414008 0.01581639 0.9299245 -0.3673952 -0.01616424 -0.9942461 -0.1070373 0.004225969 -0.9789182 -0.2038154 -0.01336383 -0.8075538 -0.5895459 0.0171085 -0.4045858 -0.9144144 0.01252204 -0.6216977 -0.7830678 -0.01723468 0.1070753 -0.9941861 0.01135772 -0.05649614 -0.9982645 -0.01661854 0.5604014 -0.8282179 -0.002341091 0.5898565 -0.8074578 -0.009019672 0.8281558 -0.5604414 0.007965624 0.9324324 -0.360973 -0.01638621 0.9936919 -0.1109136 0.01657426 -0.9196224 -0.3925868 0.01304972 -0.9105166 -0.4134562 0.003703534 -0.2010733 -0.978854 0.03760957 -0.3613823 -0.9322245 -0.01898741 0.3613319 -0.9317606 -0.03551924 0.5866191 -0.8094344 0.02634221 0.9495 -0.3132942 0.01722037 0.8926248 -0.450474 -0.01715666 -0.9104251 -0.4134564 0.01341253 -0.9279708 -0.372627 0.004398584 -0.5998248 -0.8000147 -0.01366591 -0.3980142 -0.9172728 0.01397877 -0.1072328 -0.9941331 -0.01416343 0.2038607 -0.9787884 0.02034968 0.5479139 -0.8362871 -0.02035886 0.7831118 -0.6217024 0.01490437 0.9168999 -0.3989529 -0.01145678 0.993474 -0.1134582 0.01169258 -0.9822399 -0.1870909 0.01420426 -0.9279265 -0.3726092 -0.01071584 -0.6184011 -0.7857756 0.01170414 -0.5622395 -0.8269682 -0.003207087 0.1038094 -0.9945874 0.004425704 0.06694471 -0.9977515 -0.003211557 0.6272101 -0.7788417 0.003657519 0.5898157 -0.8075296 -0.003667891 0.9302202 -0.3669517 0.006086111 0.8994639 -0.436984 -0.003132581 0.9957158 -0.09227049 -0.006014943 -0.8070689 -0.5903351 0.01201653 -0.7792878 -0.6266488 -0.0046826 -0.1640254 -0.9864177 -0.008711993 -0.1531642 -0.9881897 -0.004691183 0.4133491 -0.9103658 0.0194087 0.6165505 -0.7859894 -0.04567515 0.9313395 -0.3609598 0.04811227 -0.9238966 0.3826374 0.001967728 -0.9937632 0.1084459 -0.02596658 -0.9764946 0.2093037 -0.05148071 -0.7923426 0.6099333 -0.01321649 -0.9294666 0.3683274 -0.02066355 -0.6080073 0.7938932 -0.007795631 -0.8272407 0.5612798 -0.02525645 -0.382565 0.9239279 -0.001177668 -0.6880944 0.7249566 -0.03105026 -0.5133517 0.8573545 -0.03759449 -0.130842 0.9911977 -0.02018868 -0.3191348 0.9475926 -0.01488 -0.1088755 0.9939028 -0.01741939 0.1213763 0.9924525 -0.01748549 0.4252663 0.9047594 -0.02364546 0.6264531 0.7792925 -0.0161181 0.7931199 0.6088184 -0.01735311 0.7968139 0.6039454 -0.0183745 0.1228073 0.9922782 -0.01739358 0.3813726 0.9243857 -0.008134126 0.4719709 0.8810948 -0.03026044 0.6085361 0.7934668 -0.009712338 0.6644954 0.7468523 -0.02564239 0.8268455 0.5619244 -0.02382385 0.940995 0.338123 -0.01418781 0.9911859 0.130907 -0.02034485 0.9964265 -0.08431571 -0.005018174 0.960201 -0.2793008 0.002303063 0.9220297 -0.3858064 -0.03185695 -0.9898741 0.1222527 -0.07213586 -0.9241732 0.3619127 -0.1221604 -0.9822362 0.156862 -0.1029877 -0.8898637 0.4502087 -0.07385629 0.738133 0.6657441 -0.1092913 -0.9822556 0.1564185 -0.1034752 -0.8824362 0.458985 -0.1031463 -0.8885665 0.4373221 -0.1385602 -0.3825483 0.903363 -0.1938868 -0.5589405 0.8111413 -0.1721495 0.5491309 0.8278496 -0.1145439 0.7040449 0.7046476 -0.08827698 0.3183925 0.940687 -0.1171932 0.466777 0.8810424 -0.07670474 0.4611808 0.8772423 -0.1332601 0.06365931 0.9914506 -0.1139005 0.1760798 0.9812628 -0.0782271 0.1603798 0.9798573 -0.1189877 -0.1499126 0.98491 -0.08647847 -0.2368672 0.967495 -0.08858627 -0.1416871 0.9803187 -0.1374777 -0.7918048 0.598399 -0.122327 -0.7265359 0.6795212 -0.1019633 -0.6992465 0.7118092 -0.06619715 -0.7071677 0.6975483 -0.1155005 -0.8819913 0.455193 -0.1220279 0.9910813 -0.06470346 -0.1164962 0.9524165 -0.2834676 -0.1120218 0.9516769 -0.2935317 -0.09027993 0.9754483 0.1865383 -0.1170642 0.9878642 -0.01400572 -0.1546877 0.9934254 -1.05132e-4 -0.1144813 0.9960069 -0.01465564 -0.08806478 0.9563593 0.2801045 -0.08317762 0.8992151 0.4216189 -0.116833 0.9789538 0.1873454 -0.08094024 0.7524055 0.6469514 -0.1238548 0.8404953 0.5353734 -0.08332598 0.9514253 0.2767082 -0.1349905 0.8742563 0.4754379 -0.09815716 0.7241473 0.6845289 -0.08385074 0.8358675 0.5336167 -0.1287593 0.1236883 0.9878787 -0.09379291 0.1433301 0.9842596 -0.1033908 0.07532393 0.9836062 -0.1638448 0.2766526 0.9563562 -0.09405446 0.06640905 0.9976413 -0.01737117 0.1972116 0.9801205 -0.02171105 0.0714249 0.9973668 -0.01257079 0.3244469 0.945572 -0.02506023 0.2127549 0.9770639 -0.009023666 0.4409092 0.8971689 -0.02621299 0.3495873 0.9368763 -0.007181286 0.5511888 0.8338128 -0.03077906 0.4793219 0.8776366 -0.002154648 0.6522623 0.7574719 -0.0281136 0.5993034 0.8005197 -0.001960217 0.7430371 0.668608 -0.02931207 0.706999 0.7072086 -0.002906501 0.8219254 0.5691083 -0.02354615 0.80022 0.5996886 -0.004638314 0.8872933 0.4606619 -0.02238982 0.8777561 0.479013 -0.009536802 0.94077 0.3384773 -0.01962172 0.9367454 0.3497968 -0.01226371 0.9774926 0.2103144 -0.01661729 0.9938917 0.1089564 -0.0175442 0.9770362 0.2125245 -0.01528149 0.9980956 -0.05895543 -0.01815313 0.9990995 0.03906244 -0.01656407 0.9974545 0.07109665 -0.005451321 0.9835367 -0.1790339 -0.02454382 0.9966114 -0.08204752 -0.005831301 0.9548664 -0.2961193 -0.02331554 0.9696992 -0.2442804 -0.003241539 0.9101112 -0.4143109 -0.006652891 0.9192419 -0.3927852 0.02672165 0.8917752 -0.4521195 -0.0180273 -0.9412497 0.1529917 -0.3010693 -0.8193326 0.5726897 -0.02684462 -0.1172538 0.9847994 -0.1281473 0.1966252 0.8662968 -0.4592041 0.07325655 0.9969431 -0.02716797 0.217305 -0.9760293 0.01205891 0.9532406 0.30133 -0.02307653 0.1090196 -0.06692075 0.9917845 0.3495061 -0.3767154 -0.8578642 0.1212082 -0.2099356 -0.9701731 0.2118625 0.1404336 -0.9671571 0.8206279 0.2614641 -0.5081402 0.1627025 0.1136472 -0.9801083 -0.7522434 0.03532606 -0.6579377 -0.4355458 -0.419751 -0.7963096 -0.03367358 -0.766874 -0.6409139 -0.3627384 -0.4341905 -0.8245602 -0.1513792 -0.1078526 -0.9825742 -0.04991406 0.02787905 -0.9983644 0.9859589 0.1540264 -0.06450533 -0.9901694 0.1076962 -0.08925396 -0.9942764 0.07740026 -0.07364583 -0.5422743 -0.8151371 -0.2036913 -0.9860302 0.165351 -0.02009189 -0.1205052 0.9925225 -0.01943033 0.01391541 0.9452662 -0.3260035 0.4480373 0.1418206 -0.8826944 0.8516249 0.05845338 -0.5208822 0.9392728 0 -0.3431715 0.3213903 0.6541978 -0.6846413 0.5085534 0.4748743 -0.7182395 0.1432814 0.8125951 -0.5649421 0.6774815 0.3154558 -0.6644596 0.8670668 0.1128373 -0.4852452 -0.5463775 0.5050991 -0.6680919 -0.05031156 0.1024575 0.9934643 -0.3351758 0.7209054 -0.606591 -0.7886349 0.3167251 -0.5270108 -0.1169821 0.9237322 -0.3647385 -0.9281219 0.06726366 -0.3661493 0 0.9809017 -0.1945044 -0.01234674 0.9833796 -0.1811416 0.2938187 0.9556176 -0.02157998 0.2294771 0.9730808 -0.0213086 0.6184748 0.7853909 -0.02549999 0.9059898 0.4226456 -0.02352088 0.9865639 0.1621422 -0.02004545 0.9998478 0 -0.01745229 0.9998478 9.48418e-5 -0.0174492 -0.9998478 4.12356e-5 -0.01744818 -0.999848 6.28265e-4 -0.01742792 -0.4917505 -0.143617 -0.8588106 -0.3134802 0.003266692 -0.9495892 -0.1310699 0.1611574 -0.9781866 -0.8962666 -0.4290212 -0.1124587 -0.7701892 -0.349101 -0.5337951 -0.6501361 -0.2652689 -0.7120081 -0.1495802 -0.1191799 0.9815406 0.1931265 -0.0545516 0.9796562 -0.4666933 0.1329345 0.8743717 -0.310157 -0.02133911 0.950446 -0.820233 -0.2648669 0.5070143 0.6770885 0.341647 -0.6517887 0.3883062 0.290771 -0.8744544 0.9876374 -0.1337707 -0.08171814 0.03743904 -0.1517374 0.9877116 0.377955 -0.3040335 -0.8744792 0.8648131 0.1399322 -0.4822007 0.4284318 -0.4406316 -0.7888537 0.5827678 -0.1902679 -0.7900506 0.1107671 0.01318418 -0.993759 0.1156849 0.1171161 -0.9863573 0.5123091 0.2092431 -0.8329206 0.3418868 0.1720073 -0.9238652 0.5333805 0.213725 -0.8184296 0.6867007 0.2428262 -0.6851844 0.9051584 0.2699019 -0.3283922 0.9460015 0.2679066 -0.1825029 0.9657033 0.2588815 -0.01994097 0.9562053 0.2655968 -0.1230038 -0.9629354 0.2620653 -0.06385344 -0.9387571 0.268779 -0.2156226 -0.9594492 0.2642257 -0.09819394 -0.868432 0.2677609 -0.417289 -0.9249343 0.2698487 -0.2677281 -0.7616338 0.2549265 -0.5957571 -0.8545586 0.2666797 -0.4456585 -0.6141557 0.2294785 -0.7550844 -0.7336654 0.2508049 -0.6315315 -0.4048246 0.1860002 -0.8952771 -0.5600025 0.2192245 -0.7989605 -0.2130901 0.1413194 -0.9667582 -0.3496541 0.1738123 -0.9206146 -0.0685358 0.1050478 -0.9921028 -0.1211359 0.1186778 -0.985516 -0.492209 -0.1736696 -0.8529767 -0.6783452 0.3678223 -0.6360461 -0.3926529 -0.6005418 -0.6965438 -0.4465824 0.1141234 -0.8874345 -0.8851181 0.0875045 -0.4570656 1.05053e-5 0.08722722 -0.9961885 -0.7125302 0.0320664 -0.7009084 -0.7738832 0.03651082 -0.6322751 -0.3749929 -0.1698985 -0.9113259 -0.3692515 -0.1533731 -0.9165862 -0.3592672 0.2462633 -0.9001564 -0.4170189 0.259622 -0.8710291 0.9577295 0.2842898 -0.04397213 0.9657539 0.2588016 -0.01847398 0.9657433 0.258914 -0.01742684 0.9657783 0.2587807 -0.01746225 0.5377772 -0.8099408 -0.2340762 0.532827 -0.8154939 -0.225976 0.7325162 -0.643988 -0.22068 0.7466644 -0.631686 -0.208483 0.8765202 -0.4346708 -0.2068186 0.8766636 -0.4342204 -0.2071563 0.9528043 -0.192211 -0.2349871 0.9732339 0.04440045 -0.2254871 0.9766662 -0.1101319 -0.1843748 0.9265094 0.2954428 -0.2330107 0.967047 0.1649224 -0.1939607 -0.04494476 0.1677299 -0.984808 0.01565921 0.9997152 -0.01800692 -4.38395e-4 0.9998479 -0.0174362 -0.708155 0.6257572 -0.3270235 -0.5352552 0.8047497 -0.2566706 -0.185092 0.9441046 -0.2727771 -0.7591096 -0.5967528 -0.2600748 -0.009762942 -0.1039102 -0.9945389 -0.7756596 0.01547861 -0.6309618 -0.7644564 -0.6268811 -0.1504213 -0.0577957 -0.04243528 -0.9974262 -0.9000297 0.3944416 -0.1853711 -0.9191211 0.3296501 -0.2157483 -0.9689406 0.1259552 -0.2128132 -0.9718381 0.1151726 -0.2055872 -0.9658974 -0.1325814 -0.2224061 -0.9720414 -0.1093564 -0.20779 -0.8914569 -0.3915575 -0.2280074 -0.9179359 -0.3457657 -0.1945247 -0.7401689 -0.6341211 -0.2236975 -0.9657785 0.2587822 -0.01742911 -0.965992 0.2580029 -0.01714611 -0.9657782 0.2587809 -0.01746231 -0.9372602 0.2849566 -0.2008562 0.04494476 0.1677299 -0.984808 0.2264068 0.0629459 -0.9719969 -0.5542311 0.8317767 -0.03123384 -0.8889967 0.4483546 -0.09307646 0.9320395 -0.3615157 0.02467662 -0.8161402 0.5773066 -0.0251488 -0.5557554 0.8311538 -0.01787304 0 0.9998491 -0.01737928 0.7061485 0.004707217 -0.7080481 -0.005544781 -0.619596 -0.7849013 0.002602159 -0.8512195 -0.5248035 0.001915991 -0.008076965 -0.9999656 0.7497004 0.6466004 -0.1409161 0.9934959 0.09176141 -0.0674237 0.9928476 0.1178048 -0.01938706 0.9081955 0.41793 -0.02270352 0.9804018 0.1958854 -0.02100729 0.7337331 0.6790015 -0.0243473 0.4324002 0.9013932 -0.02281475 0.5231246 0.8519117 -0.02423638 0.1625012 0.9865046 -0.02005535 0.1635067 0.9863379 -0.02007782 -0.1649143 0.9861033 -0.02009004 -0.1178311 0.9928373 -0.01974475 -0.4180089 0.9081509 -0.02303522 -0.5002388 0.8655431 -0.02441853 -0.7340918 0.6786245 -0.02404248 0.9996728 0.0195319 0.01651525 -0.01996111 0.9232273 -0.3837357 0 0.9843369 -0.1762977 0.5324269 0.7888864 -0.3068876 0.6886941 0.5012031 -0.5239236 0.1163771 0.2990911 -0.9471014 -0.9829421 0.179822 -0.03858464 -0.9364393 0.3475112 -0.04813849 -0.08561497 0.327448 -0.9409825 -0.3028865 0.07426261 -0.9501289 -0.6450135 0.4308053 -0.6311612 -0.3115617 -0.01904177 -0.9500352 -0.2719595 0.2983545 -0.9148894 -0.09132146 0.1449969 -0.9852088 -0.3574248 0.7414939 -0.567833 -0.04814642 0.159405 -0.9860386 -0.3336152 0.05846303 -0.9408948 -0.710911 0.409947 -0.5714447 -0.04118931 0.7857066 -0.6172266 -0.3629834 0.6765197 -0.6407529 -0.1854656 0.3073257 -0.9333561 -0.1003478 0.8446938 -0.5257592 -0.3626944 0.01123446 -0.9318405 -0.657524 0.3215399 -0.6813768 -0.007792413 0.1393998 -0.9902055 -0.3485452 0.1674636 -0.9222106 -0.1689815 0.8201789 -0.5465821 -0.3173947 -0.01894629 -0.9481043 -0.202505 0.0998317 -0.9741794 -0.355042 0.1746747 -0.9183866 -0.5291662 0.5665702 -0.6316497 -0.296271 0.3534717 -0.8872887 -0.2375284 0.6866274 -0.6871122 -0.1825859 0.6119191 -0.7695567 0.8707866 0.491372 -0.01686185 0.9147058 -0.4035559 -0.02135235 0.9167881 -0.3985853 -0.025092 0.7430136 0.3591274 -0.564764 0.110706 0.8465812 -0.5206193 0.02819573 0.5921421 -0.8053402 0.01319229 0.2741854 -0.9615864 0.3623805 0.1762995 -0.9152044 0.7774078 0.4397926 -0.4496886 0.0979408 0.04322582 -0.9942531 0.5836045 0.4681052 -0.6635385 0.6078109 0.4746502 -0.6366108 0.1319561 0.1616991 -0.9779781 0.1560559 0.182135 -0.9708108 0.3919267 -0.07353717 -0.9170528 0.9377477 -0.2749836 -0.2121635 0.6333849 0.6060575 -0.4811631 0.8435473 -0.1766065 -0.5071865 0.3883547 -0.07412463 -0.918524 0.469967 0.6058434 -0.6419383 0.4829181 0.6126376 -0.6256718 0.8923375 -0.1280827 -0.4328148 0.8844913 -0.1068898 -0.4541475 0.09889054 0.2024545 -0.9742859 0.09906458 0.07353872 -0.99236 0.4733167 0.7472819 -0.466413 0.3490844 0.08124148 -0.9335631 0.07158893 0.1778182 -0.9814559 0.3901591 0.7281666 -0.5635153 0.983931 0.09922069 -0.1484421 0.1515029 0.4401769 -0.8850374 0.4631062 0.1808961 -0.8676458 0.2115862 0.7953703 -0.5679941 0.1657007 0.4469898 -0.8790583 0.01590287 0.1579781 -0.9873147 0.7822936 0.1703469 -0.5991649 0.3355565 0.1410306 -0.9314035 0.1291052 0.8619272 -0.4903195 0.01054942 0.1392543 -0.9902005 -0.05003648 -0.137937 0.9891763 0 0.1391869 -0.9902662 -0.4361455 0.7240166 -0.5343942 -0.8856586 0.4136883 -0.2108813 0.8885117 0.4082784 -0.2094174 0.8877913 0.41441 -0.2002271 0.01594811 0.0591588 -0.9981212 -1.59938e-5 0.08701354 -0.9962072 1.61116e-5 0.08716386 -0.9961941 7.31351e-5 0.08710002 -0.9961996 -3.95993e-6 0.0871337 -0.9961967 1.59493e-5 0.08726787 -0.996185 -3.85455e-5 0.08712285 -0.9961977 -5.40343e-6 0.08715623 -0.9961947 5.6736e-6 0.08715623 -0.9961947 -1.23884e-4 0.08729183 -0.9961828 2.05854e-4 0.08698201 -0.9962099 4.03796e-6 0.08707016 -0.9962022 2.11047e-6 0.08715337 -0.9961949 -1.04307e-4 0.0871573 -0.9961946 1.7565e-5 0.08717137 -0.9961934 0 0.08714818 -0.9961954 1.85527e-5 0.08726048 -0.9961856 0 0.08715742 -0.9961946 6.7981e-6 0.08756273 -0.9961591 -1.26819e-4 0.08704328 -0.9962046 -1.40965e-5 0.08708423 -0.996201 -2.60328e-5 0.08703863 -0.996205 8.26782e-5 0.08727896 -0.9961839 0.5212257 0.7927539 -0.3160145 0.0522682 0.09052753 -0.9945214 0.05220484 0.09056007 -0.9945218 0.1389353 0.2406515 -0.9606164 0.7605756 0.4617929 -0.4563685 -0.02663314 0.1729418 -0.984572 0.3889589 0.2640396 -0.8826065 0.6606739 0.3001551 -0.6880531 0.9452425 0.2805354 -0.1667833 0.7448251 -0.5406784 0.3910275 0.308041 0.002710342 -0.9513693 0.8674585 -0.07643198 -0.4916035 0.1764673 -0.03574073 -0.9836574 0.3055901 -0.180104 -0.9349744 0.2587512 -0.2209789 -0.9403278 0.7909315 0.3462489 -0.5045186 0.2689891 0.1512736 -0.9511893 -0.510834 -0.34676 -0.7866424 -0.3067346 -0.2516193 -0.9179335 -0.03205394 -0.0812276 -0.9961801 -0.8312798 -0.1266503 -0.5412334 -0.6181173 -0.02409034 -0.7857167 -0.618341 -0.02413749 -0.7855393 -0.3744089 0.06739461 -0.9248114 -0.05292242 0.1657311 -0.98475 -0.8563292 -0.5060908 -0.1028233 -0.6119211 -0.706501 -0.3555403 -0.3054972 -0.2675331 -0.9138368 -0.1320167 -0.1929642 -0.9722841 -0.1380628 -0.6830143 -0.7172378 0.005399942 -0.7615779 -0.648051 0.001052379 -0.9846356 -0.1746184 -3.7051e-4 -0.9251897 -0.3795052 0 -0.9920813 -0.1255981 -0.1071287 -0.9940593 -0.01922243 -0.3560588 -0.9341921 -0.02252775 -0.6481171 -0.7611541 -0.02426832 0.1952488 -0.947123 -0.2546295 -0.488641 -0.8267692 -0.2787163 -0.9923268 0.1221039 -0.01944941 -0.8547341 0.5184881 -0.02448987 -0.1972464 0.9800503 -0.02440047 -0.7557755 0.009891808 -0.6547562 -0.9823635 0 -0.1869818 -0.9881756 -0.002814412 -0.1533004 0.1113715 -0.9720391 -0.2067281 0.3722882 -0.8972409 -0.2374034 0.06492215 -0.9615504 -0.2668446 -0.8436325 0.3804516 -0.3788681 -0.5688459 -0.131674 0.8118353 -0.9369781 0.2848504 -0.2023177 -0.5482883 0.2883188 -0.7850174 -0.290106 0.245487 -0.9249728 0.03703153 0.16999 -0.9847497 0.0634064 0.1630867 -0.9845722 0.4697789 0.0338785 -0.8821339 0.721629 -0.07011818 -0.6887199 0.9130827 -0.3462002 0.2154656 0.2384056 -0.6697925 -0.7032361 -0.1423355 -0.3695074 -0.9182619 0.01760721 -0.20415 -0.9787813 0.2576018 0.9659968 -0.02216994 0.98601 -0.1654698 -0.0200985 0.5352407 -0.844352 -0.02423447 0.1425618 -0.9895886 -0.01976394 0.06544595 0.0092265 -0.9978135 0.5658367 -0.003619253 -0.8245095 0.9567252 -0.003731966 -0.2909692 0.9279043 -0.01127088 -0.3726482 0.9065605 0 -0.422076 0.8672233 0.4899027 -0.08898931 0.6860214 0.2949339 -0.6651231 0 0.9998477 -0.01745223 0 0.9998478 -0.01745289 0.8099567 0.5859612 -0.02489531 0.6681855 0.7032136 -0.2429377 0.03509455 0.7468801 -0.6640321 0.5084423 0.5854835 -0.6314235 0.6152882 0.1330964 -0.7769852 -0.1540447 0.9477146 -0.279477 -0.4945505 0.8566147 -0.1470756 -0.8408157 0.5406777 -0.02639985 -0.9299781 0.3665623 -0.02780157 -0.6802919 -0.7329078 -0.007009148 -0.1302702 0.1376872 -0.9818717 -0.9887428 -0.1427795 -0.04474014 -0.04909336 -0.9912552 -0.1224873 -0.1330386 -0.9907504 -0.02673161 0.1691959 -0.3596217 -0.9176302 -0.5847901 -0.3038252 -0.7521375 -0.4996472 -0.3800079 -0.7784258 -0.1694489 0.1776403 -0.9693973 0.6238731 -0.5283219 -0.5758979 0.2737683 -0.2436244 0.930429 -0.2095768 -0.6950786 -0.6877087 -0.189227 -0.5950358 -0.7811053 -0.1967727 -0.5199234 -0.8312402 0.8429765 -0.2388554 -0.4820153 -0.8651178 0.501556 0.003578186 0.278356 0.2276256 -0.9331155 0.4848073 0.1092848 -0.8677665 0.6608059 -0.02521342 -0.7501333 0.771187 -0.3860796 -0.5061753 0.5613671 -0.228805 -0.7953084 0.6687791 -0.3050932 -0.677977 0.3740866 -0.1045382 -0.9214831 0.1646559 0.02434146 -0.9860507 0.3341016 -0.07896524 -0.9392235 0.65747 -0.7529587 -0.02804785 -0.1023033 0.8507134 -0.5155782 -0.1531993 0.2335081 -0.9602105 -0.1435164 0.2485972 -0.9579157 -0.0522682 0.09052753 -0.9945214 0.8142974 -0.3125598 0.4891074 -0.100382 0.2043197 -0.9737439 0.1200407 -0.2124298 0.9697751 0.8676603 -0.1548175 -0.4724377 0.9585552 -0.2296259 -0.1686534 0.6975143 -0.06209748 -0.7138752 0.7806491 0.1225339 -0.6128398 -0.6431545 0.2972375 -0.7056928 -0.985629 0.1607543 -0.05190157 -0.09449809 -0.1546431 0.9834407 0.0059309 0.1750239 -0.9845463 0.942776 -0.2037381 -0.2639399 -0.9567701 0.2156211 0.1951885 -0.7765573 0.3037908 -0.5519693 -0.9244208 -0.3754589 -0.06690984 -0.9024899 -0.430343 -0.01780921 -0.95209 -0.3047589 -0.02543461 -0.9099234 -0.414761 -0.003575384 -0.9837017 -0.1781812 -0.02413564 -0.9697313 -0.2441361 -0.004348039 -0.9981246 -0.05861699 -0.01764845 -0.9976424 0.06633722 -0.01758396 -0.996609 -0.08203023 -0.006459832 -0.9800506 0.1976912 -0.02047365 -0.9973452 0.07131356 -0.01473087 -0.9448813 0.3266304 -0.02262485 -0.9771016 0.2124252 -0.01216512 -0.8960151 0.4433902 -0.02371114 -0.9368527 0.3495941 -0.00954771 -0.83241 0.5534467 -0.02811276 -0.8776961 0.4791876 -0.005387306 -0.7550964 0.6550404 -0.02741658 -0.8001352 0.5998006 -0.004790365 -0.666332 0.7450646 -0.02967566 -0.7071852 0.7070209 -0.003257751 -0.5662484 0.823853 -0.02508455 -0.5993629 0.800464 -0.004660189 -0.4582225 0.8885159 -0.02391052 -0.4797719 0.8773598 -0.007674932 -0.3492191 0.9369844 -0.01030963 -0.3434209 0.9390589 -0.01518499 -0.2229309 0.9746307 -0.01991873 -0.2124072 0.977122 -0.01076459 -0.1195696 0.9926143 -0.02049416 -0.07129371 0.9974552 6.56327e-4 -0.0355187 0.9992078 -0.01795452 -0.3340462 0.08063513 -0.9391013 0.3765145 -0.06667596 0.9240082 -0.6257411 0.004546463 -0.7800177 -0.1698175 0.1346346 -0.9762354 -0.9547366 -0.2218422 -0.1981521 -0.430882 -0.2712246 0.8606846 0.4350835 0.2724404 -0.8581833 0.8435768 0.3050804 -0.4419324 0.9655773 0.2594045 -0.01923435 0.3241071 0.2516078 -0.9119474 0.8379975 -0.447671 -0.3120113 0.6324112 -0.3642472 -0.6836521 0.2768779 -0.1734964 -0.9451126 -0.8377031 0.4473873 0.3132064 -0.4189854 -0.9077638 -0.02040171 -0.3994407 -0.9159985 -0.0373364 -0.2588046 -0.9651761 -0.03814965 -0.3227056 -0.9464553 -0.009148836 -0.21753 -0.976021 -0.007981181 -0.8116142 -0.04623001 -0.5823617 -0.2726078 -0.02608525 -0.9617717 0.9999916 0.002589762 -0.003186404 -0.8182738 0.0788213 -0.5693991 -0.27138 -0.04094862 -0.9616009 -0.5613679 -0.2287988 -0.7953096 -0.3840103 -0.1113474 -0.9165904 -0.8155294 -0.4251893 -0.392589 -0.7145556 -0.3400607 -0.6113665 -0.8635508 -0.4830769 -0.1446261 -0.8145213 -0.4241462 -0.3957967 -0.798883 -0.1634988 -0.5788387 -0.6605243 -0.02616053 -0.7503488 -0.3765743 0.1534116 -0.9135955 -0.8879789 -0.4383682 -0.1390215 -0.8766489 -0.3022755 -0.3743211 -0.8099303 -0.585868 -0.02778142 0.8120946 0.4304229 0.3940032 0.8442693 0.506736 -0.1744358 -0.5599476 -0.2278915 -0.7965703 -0.3675521 -0.1167967 -0.9226398 0.4322968 0.421899 0.7969447 0.8558909 0.5065705 -0.1041014 -0.3486025 -0.1676937 -0.9221471 -0.6183521 -0.3101624 -0.722108 -0.8632528 -0.4825797 -0.1480255 0.4235452 0.3583356 0.8319887 -0.3570698 -0.1459552 -0.9226041 -0.8624939 -0.4846181 -0.1457722 -0.8124296 -0.4277075 -0.3962632 0.8459195 0.4571416 0.2746668 0.7372434 0.6567368 0.1586475 -0.7196443 -0.3439772 -0.6031515 -0.5928823 -0.2832272 -0.7538388 0.2073826 -0.5068921 -0.8366917 -0.4288834 -0.1726444 -0.886709 -0.5898477 -0.281331 -0.7569232 0.7865566 0.4755015 -0.393989 0.1159605 -0.9925088 -0.03846436 0.2583825 -0.965488 -0.03273105 0.1742193 -0.9846459 -0.01095885 0.2844133 -0.9586367 -0.0111761 0.3976263 -0.9171715 -0.02626532 0.4038272 -0.914627 -0.01952892 0.09644675 -0.274946 -0.95661 0.8412269 -0.07024627 -0.5360995 -0.3595719 -0.6049438 -0.7104584 0.744151 -0.1817089 -0.6428229 0.9729538 0.02963322 -0.2290911 0.7853984 0.08399069 -0.6132658 0.8821283 0.06351995 -0.4667065 0.3840187 0.1273155 -0.9145056 0.6675788 0.1020931 -0.7375064 0.9136105 -0.002603888 -0.4065823 0.8809127 0.0143997 -0.4730597 -0.9181939 0.01102364 0.3959779 0.9189826 0.03976607 -0.3922879 0.9874785 0.01955497 -0.1565373 0.3438779 0.06221401 -0.9369512 0.999848 -1.9084e-5 -0.01744002 0.57612 0.03179985 -0.8167464 0.6063989 0.02893388 -0.794634 0.4166443 0.05374205 -0.9074798 0.3577125 0.05527836 -0.9321943 0.6673966 0.07771635 -0.7406362 0.6064729 0.03648155 -0.7942668 0.3314357 0.09948021 -0.9382187 -0.3751626 -0.0243507 0.9266393 -0.9976775 0.005365967 -0.06790375 -2.71138e-4 0.2092083 -0.9778711 0.98927 -0.004265189 -0.1460367 0.9893878 0.01342302 -0.1446784 0.9967433 -0.07633209 -0.02600544 0.08773285 -0.9960154 -0.01601755 -0.0134924 0.4262193 -0.9045193 0.0386784 0.4872727 -0.872393 -0.204068 0.7327238 -0.6492088 -0.2250562 0.9087222 -0.3515304 -0.4336233 -0.3723852 -0.8205487 -0.331335 -0.9037814 -0.2709175 0.04344266 0.2138892 -0.9758915 -0.3325898 -0.8674904 -0.3699252 -0.2266625 -0.5565444 -0.7993013 -0.8380042 -0.4780687 -0.2630578 -0.4973504 0.2480228 -0.8313407 -0.009155929 0.6308416 -0.7758577 0.1668694 -0.964824 -0.2031484 -0.3449964 -0.9159486 -0.2049776 0.03948277 -0.02552771 -0.9988941 0.3515431 -0.9226384 -0.1586061 0.1728922 -0.4148232 -0.8933252 0.3611252 -0.8900172 -0.2783128 0.2619967 -0.5803202 -0.7710943 0.009548485 -0.2133144 -0.9769369 -0.01172822 -0.9225035 -0.3858104 -0.2857876 -0.9203535 -0.266974 0.05598747 -0.4692423 -0.8812929 -0.1808467 -0.4599684 -0.8693236 -0.07135909 -0.1658169 -0.9835714 -0.2337805 -0.8935983 -0.3831827 -0.2434014 -0.8829696 -0.401398 0.001799881 -0.3990133 -0.9169434 0.127449 -0.9245822 -0.3590328 0.1045214 -0.9191656 -0.3797497 -0.06963896 -0.1698042 -0.9830142 -0.1718064 -0.8471959 -0.5027343 -0.1479468 -0.7851642 -0.6013561 0.1479903 -0.84012 -0.5218213 0.0339325 -0.3059586 -0.95144 0.1702507 -0.8396426 -0.5157666 -0.01161205 0.002726197 -0.9999289 -0.1046384 -0.9212294 -0.3746828 -0.1374589 -0.7807619 -0.6095211 0.02915787 -0.06795936 -0.997262 -0.02133095 -0.1999312 -0.9795778 0.1784822 -0.714798 -0.6761716 -0.03645551 -0.9298224 -0.3661985 -0.04822528 -0.9002318 -0.432732 -0.0156399 -0.244791 -0.9694498 0.197278 -0.7184446 -0.6670224 0.01400911 -0.02854317 -0.9994945 0.8854753 0.4550006 -0.0943821 0.3196405 0.1874122 -0.9288201 0.6943277 0.7120862 -0.1041274 0.9890009 -0.1296613 -0.07116925 0.9264829 0.3723562 -0.05459195 -0.06103497 0.1546403 -0.9860838 -0.2783892 0.2716694 -0.9212465 -0.0500248 0.1484816 -0.9876491 -0.4779985 0.3713 -0.7960239 -0.2877898 0.2766212 -0.916874 -0.849969 0.5062578 -0.1457936 -0.7763205 0.493779 -0.3918021 -0.8500987 0.506348 -0.1447207 -0.352279 0.5585176 -0.7509712 -0.1566954 0.4626773 -0.8725689 0.04071456 0.3263005 -0.9443889 -0.8233653 0.5499278 -0.1401754 -0.7008025 0.6075508 -0.3738422 -0.5413681 0.6096681 -0.5789866 0.3456919 0.9321025 -0.1080844 -0.7738083 0.6333802 -0.007086277 0.3317189 0.936914 -0.1102491 -0.8140828 0.4225033 -0.3984475 0.8634549 -0.4869739 -0.1315373 0.8592098 -0.4779354 -0.1825821 -0.4766307 0.3706552 -0.7971436 -0.2944525 0.2651759 -0.9181391 -0.7054631 0.3468108 -0.6180973 -0.2934098 0.2654359 -0.9183979 -0.05661064 0.1368231 -0.9889767 -0.07264387 0.1334159 -0.9883942 -0.1167818 0.05600523 -0.9915773 -0.5403899 0.2533733 -0.8023595 -0.8512913 0.5042716 -0.1449595 -0.8658423 0.500011 -0.01750153 -0.3514183 0.1601356 -0.9224217 -0.8656727 0.5002987 -0.01766437 0.4804795 -0.2100653 -0.8514764 -0.8507652 0.5049096 -0.1458256 -0.8649697 0.4795209 -0.1479432 -0.7773139 0.4940914 -0.3894317 -0.7775138 0.4905094 -0.393539 -0.6517443 0.4488027 -0.6113964 -0.6511532 0.4485728 -0.6121945 0.05502092 -0.9935448 -0.09920394 -0.3367059 -0.3840366 -0.8597354 -0.4753345 0.0283994 -0.8793467 0.05749392 -0.9939862 -0.09319829 -0.8360996 0.499361 -0.2271033 -0.9971025 -0.03659695 -0.06668919 -0.6320395 -0.7721891 -0.06519329 0.3816117 -0.4438439 0.8107868 0.2237599 -0.1131313 -0.9680563 -0.7579683 -0.5097998 0.4069254 0.7151809 -0.002917468 0.6989333 -0.3243573 -0.5911341 0.7384802 0.192156 0.03694057 -0.980669 -0.03279614 -0.7532479 0.6569187 0.1315811 -0.6244972 -0.7698634 0.2443812 -0.1734095 -0.9540477 0.07570964 0.0623275 -0.9951801 -1.86034e-6 0 1 -0.01161444 -0.0188561 -0.9997549 0 0 1 5.20359e-4 0 1 -0.09632879 -0.04140186 -0.9944882 -0.2421392 0.7582041 0.6053885 -0.5718231 0.1029147 -0.8138962 -0.5846748 -0.02277481 -0.8109481 0.04189288 -0.512111 -0.8578971 -0.07321816 -0.03012984 -0.9968608 -0.8847224 -0.2055931 -0.4183273 -0.6833695 -0.1040936 -0.7226138 -0.9312808 0.3642263 -0.007438302 -0.9684538 0.2488006 -0.0139873 -0.9962201 -0.08467149 -0.01939892 -0.7998045 -0.5605968 -0.2145792 -0.3437234 0.332848 -0.878104 -0.703954 0.2735269 -0.6554631 -0.1204534 0.1812625 -0.9760302 -0.2984082 -0.1512345 0.9423804 -0.1273466 0.05531489 -0.9903147 -0.3022403 -0.151905 0.9410504 -0.5478293 0.3321922 -0.7678096 -0.9657783 -0.2587757 -0.01753968 -0.9657787 -0.2587792 -0.01746207 -0.9655914 -0.2594959 -0.01718515 -0.8755534 -0.468836 -0.1166152 -0.8222417 -0.1752938 -0.5414708 -0.01378953 -0.6514765 -0.7585436 -0.02811861 -0.4190402 -0.9075322 -0.1289437 -0.6100364 -0.7818114 0.06654071 -0.2880325 -0.9553061 -0.7464777 -0.6378425 -0.1895467 -0.5415974 -0.8087779 -0.2292395 -0.3117509 -0.9241259 -0.2209136 -0.2794752 -0.9401457 -0.1949864 -0.06429797 -0.9721291 -0.2254575 -0.07460564 -0.9700042 -0.2313568 0.9244377 0.3809175 -0.01780253 0.9998167 -0.004797756 -0.01853334 0.9990589 0.03972101 -0.01743119 0 -0.9998477 -0.01745384 0 -0.9998477 -0.0174542 -0.9998476 0 -0.01745986 -0.9998477 2.21065e-4 -0.01745373 -0.5858045 -0.7716869 -0.247654 -0.8621208 0.009508371 -0.5066136 0.1489558 -0.967597 -0.2038831 0.9656904 -0.259117 -0.01733195 0.9657786 -0.2587796 -0.01746213 0.9657788 -0.2587791 -0.01745831 -0.6931196 -0.7174194 -0.06996262 0.177238 -0.7766543 -0.604479 0.03769254 -0.5266768 -0.8492296 -0.06638228 -0.3788043 -0.923093 -0.1832274 -0.2479389 -0.9512907 -0.6963677 -0.7145316 -0.06720656 -0.4874623 -0.8621693 -0.1380026 0.7801198 0.2523459 -0.572481 0.4438837 0.2488967 -0.8608239 0.6174126 0.3129153 -0.7217242 0.1802267 0.03783404 -0.9828973 0.1813158 0.1676151 -0.9690355 -0.01840513 -0.04733592 -0.9987094 0.1575972 -0.2864361 0.9450489 -0.09777659 0.0134781 -0.9951172 -1.70347e-4 0 1 -0.1987882 -0.1106023 -0.9737815 4.03437e-6 0 1 -8.17611e-5 0 1 -3.72682e-4 0 1 0.03060656 -0.2153278 -0.9760621 0.2050928 -0.6948351 0.6893048 -0.6807554 0.007165253 0.7324758 0.4464178 -0.4462771 0.7755953 -0.5988603 -0.3494081 0.720611 0.1086983 -0.1092543 -0.9880528 -0.1657582 -0.4001815 0.9013208 -0.1112006 -0.07148879 -0.9912234 0.1163969 -0.284749 -0.9515092 0.9998477 0 -0.01745176 0.9998477 0 -0.01745194 0.7990541 -0.5625767 -0.2121793 0.6265218 -0.4218575 -0.6553676 0.5000128 -0.2492907 -0.8293622 0.4402734 0.6663397 -0.6017897 0.8122497 0.2326364 -0.5349119 0.9243295 -0.003076136 -0.3815829 0.9807395 0.01559013 -0.1946976 -0.03110861 0.9735041 -0.226544 0 0.9305791 -0.3660911 -0.9129459 0.03414881 -0.4066495 -0.9230976 0.04280793 -0.3821758 -0.9538822 -0.2989656 -0.02698755 0.5419508 0.8401711 -0.02004653 -0.9569832 -0.2897576 -0.0149579 -0.9998496 -2.15206e-5 -0.0173459 -0.9979358 -0.01090782 0.06328749 -0.9998221 -0.001445233 0.01880705 -0.4587204 0.1260058 -0.8796011 0.04542267 0.1395062 -0.9891789 0.4274072 0.1319228 -0.8943822 -0.990249 0.01061439 -0.138904 -0.893741 0.03743493 -0.4470186 -0.9331139 0.03026556 -0.3583051 -0.7536273 0.05592846 -0.6549183 -0.8266527 0.04805344 -0.5606571 -0.5829519 0.06966668 -0.8095145 -0.6730893 0.06368029 -0.7368146 -0.1318511 0.08596312 -0.9875352 -0.3758279 0.08063822 -0.9231745 -0.09854942 0.08658653 -0.9913581 -0.07716047 -0.2618513 -0.9620189 -0.1621655 -0.371518 -0.9141536 -0.3676176 -0.8630748 -0.3463514 -0.1890615 -0.7800862 -0.5964239 -0.06546664 -0.2940813 -0.9535357 -0.02304452 -0.1634753 -0.9862782 -0.1132301 -0.6917046 -0.7132487 -0.08938407 -0.8926721 -0.4417543 0.8634462 -0.4887323 -0.1249063 0.8152266 -0.4387958 -0.3779733 0.8294299 -0.4517167 -0.3286306 0.7265173 -0.3698543 -0.5791205 0.757823 -0.3934758 -0.5204626 0.5993041 -0.2812482 -0.7494893 0.6452709 -0.3129532 -0.6969117 0.445456 -0.1811198 -0.8767923 0.4841073 -0.2061716 -0.8503726 0.2751697 -0.07568454 -0.9584121 0.09444737 0.03234905 -0.9950042 0.8654963 -0.5003961 -0.02279889 0.8658944 -0.4999246 -0.01738792 0.8658902 -0.4999237 -0.01761883 0.5060791 0.3539725 -0.7865033 0.7742863 0.576314 -0.2614251 0.06077867 0.394554 -0.9168605 0.3666399 0.5397031 -0.7578231 0.5575942 0.5893043 -0.5846444 0.8199281 0.5040449 -0.271398 0.6515343 0.449143 -0.6113703 0.4779597 0.3712633 -0.7960641 0.6491503 0.4481315 -0.6146397 0.1707487 0.2153068 -0.961503 0.3538055 0.3093212 -0.8826904 0.05002456 0.1485137 -0.9876443 0.6617987 0.3550177 -0.6602916 -0.8677306 -0.4907391 -0.07885885 0.5312127 0.2894341 -0.7962669 0.5725401 0.2083587 -0.7929595 0.3211142 0.1941978 -0.9269158 0.1004251 0.1016459 -0.9897388 0.6270543 0.3953043 -0.6712209 0.6212531 0.4942371 -0.6080906 0.3054439 0.2237678 -0.9255443 0.3036822 0.2241856 -0.9260227 0.08965939 0.1039623 -0.9905318 0.3241231 0.1906929 -0.9265962 0.3616756 0.2954199 -0.8842612 0.05947434 0.1320033 -0.9894635 0.0732274 0.1321012 -0.9885278 0.5872088 -0.8088132 -0.03173464 0.6065267 -0.7935101 -0.04966866 0.9532845 0.2993099 -0.0407707 0.4442132 0.1172829 -0.8882114 0.493977 0.1246734 -0.8604902 0.8788461 0.3518695 -0.3222071 0.03085583 0.1151381 -0.9928702 0.5799041 -0.03202253 0.8140552 0.7714553 -0.3729312 -0.5155378 0.6779184 -0.1539833 -0.7188295 0.5181517 -0.02308601 0.8549771 -0.02896147 0.1080843 -0.9937199 -0.0135011 0.09790492 -0.9951042 -0.7774939 0.2483251 -0.5777871 -0.6720884 0.09467607 0.7343934 -0.3774443 -0.1232887 -0.9177886 -0.09199339 -0.08960211 -0.9917201 -0.06580114 -0.05114209 -0.9965214 -0.06301742 -0.06676191 -0.995777 -0.06750029 -0.0684002 -0.9953719 0.01116371 0.06300187 -0.997951 -0.002187013 0.01019579 -0.9999457 -0.004557013 0.01022487 -0.9999374 0.03997063 0.06934487 -0.9967917 -0.3800663 -0.738396 -0.5570646 -0.03051048 -0.1039373 -0.9941158 -0.03245955 -0.1169874 -0.9926029 0 0.2110884 -0.9774671 0.0339812 -0.5120885 -0.8582603 0.1586574 0.08709573 -0.9834848 0.04168158 -0.1601101 -0.9862188 0.02304947 -0.1075281 -0.9939349 -0.4558669 -0.3314143 -0.8260447 0.03239351 0.04545396 -0.9984412 -0.03783977 0.04712635 -0.998172 -0.05982786 0.08645206 -0.994458 -0.04838579 0.06284159 -0.99685 0.1699193 -0.22728 -0.9588907 0.03476953 -0.06706959 -0.9971424 0.1715219 -0.04124242 -0.9843167 0.6255959 -0.1108812 -0.7722274 0.4497096 -0.3024721 -0.8403999 0.6587979 0.1705851 -0.7327252 0.06919926 -0.3732326 -0.9251534 0.6902194 -0.07899355 -0.7192755 0.846841 0.5021132 -0.1753362 -0.8660059 -0.4997249 -0.01757055 -0.865105 -0.5012574 -0.01829129 -0.8658912 -0.4999285 -0.0174365 -0.8658933 -0.4999237 -0.01747095 -0.08486908 0.0379427 -0.9956694 -0.1086518 0.02423292 -0.9937845 -0.2595436 -0.06603997 -0.9634708 -0.8084666 -0.4329469 -0.3986716 -0.8624255 -0.487432 -0.1365008 -0.8627672 -0.4881216 -0.1317955 0.06803125 -0.9103217 -0.4082724 0.05812406 -0.2363737 -0.9699222 0.3469215 -0.7191496 -0.6020543 0.1735749 -0.5168107 -0.8383188 0.09691226 -0.7293292 -0.6772643 0.15008 -0.5524662 -0.8199129 0.1549878 -0.6451066 0.7482087 0.05693042 -0.03356355 -0.9978139 0.9923272 0.009270131 -0.1232926 0.9906553 0.01048272 -0.1359871 0.9331077 0.03016787 -0.3583294 0.9479861 0.02630019 -0.3172234 0.8266456 0.04802811 -0.5606697 0.8669202 0.04195493 -0.4966781 0.6731576 0.06365537 -0.7367543 0.7301795 0.05823087 -0.6807695 0.4799893 0.07597297 -0.8739785 0.5491975 0.07177305 -0.8326048 0.2900655 0.08308368 -0.9533935 0.3358504 0.08141869 -0.93839 0.09852868 0.08658921 -0.99136 0.1142131 0.08630889 -0.9897 0.9998478 1.65514e-7 -0.01745194 0.9998478 -4.14853e-7 -0.0174492 0.9998477 3.82807e-6 -0.01745766 0.4633741 -0.1219699 0.8777289 0.9692343 0.1188492 -0.2155454 0.1242358 0.1388084 -0.9824957 0.7749161 0.2478833 -0.5814286 -0.9929751 0.0861721 -0.08108609 -0.9347971 -0.3551419 -0.005356252 0.7424535 0.6687409 -0.03935182 0.5360268 0.8403827 -0.08020102 -0.2175773 0.9722408 -0.08607023 -0.8810557 0.4663605 -0.07904911 -0.8658649 0.4999719 -0.01749891 0.102886 -0.149668 0.9833687 -0.02686494 -0.9965526 -0.07849371 -0.08000522 -0.9941194 -0.07297956 -0.03820461 -0.9953954 -0.08791083 -0.1432046 -0.9873116 -0.06861758 -0.1139276 -0.9895126 -0.08879995 -0.2130122 -0.9745078 -0.07043015 -0.1850607 -0.9788689 -0.08699518 -0.2784395 -0.9577835 -0.07157099 -0.2429782 -0.9665122 -0.0825591 -0.2908815 -0.9534513 -0.07948946 -0.3428875 -0.9366074 -0.07207572 -0.3343933 -0.9392464 -0.07744252 -0.4264191 -0.9038193 -0.03574258 -0.3663644 -0.9271337 -0.07874143 -0.3943291 -0.9156966 -0.077488 -0.3924459 -0.9166775 -0.07542216 0.4507461 -0.8884077 -0.08694696 0.4135066 -0.9081351 -0.06559813 0.3681293 -0.9267663 -0.07473337 0.3903524 -0.9171342 -0.08056038 0.3927802 -0.9157851 -0.08403366 0.3522415 -0.9325562 -0.07915169 0.3130025 -0.9470639 -0.07141023 0.3308246 -0.9404948 -0.07761871 0.2441288 -0.9672493 -0.06949782 0.2797335 -0.9560891 -0.08742332 0.1708732 -0.9829071 -0.06852763 0.205622 -0.9746767 -0.08789223 0.1013032 -0.9924153 -0.06963944 0.1344158 -0.9870565 -0.08747512 0.03410232 -0.9968243 -0.07196152 0.07530361 -0.9937106 -0.08287709 0.02474457 -0.9966323 -0.07817864 0.8659653 0.4997974 -0.0175119 -0.9936038 -0.1114162 -0.018386 -0.9687859 -0.2463372 -0.02778363 -0.9737001 -0.2271897 -0.01711726 -0.9258469 -0.3767433 -0.02952992 -0.9426674 -0.3335934 -0.009678721 -0.8654717 -0.5000134 -0.03074687 -0.8950323 -0.4459307 -0.007956743 -0.7891988 -0.6133648 -0.03080576 -0.834517 -0.5509257 -0.007898926 -0.698053 -0.7155019 -0.02791321 -0.7639372 -0.6452218 -0.009439289 -0.6797903 -0.7333216 -0.01116538 -0.5935584 -0.8044133 -0.02465415 -0.5738474 -0.5824161 -0.5757523 0.001533746 -0.6010984 -0.7991735 -0.8221042 -0.142705 0.5511625 0.001559734 -0.6038995 -0.7970591 -0.3382806 -0.03131461 -0.9405242 -0.1238813 -0.6900008 -0.7131287 -0.650772 0.4049021 -0.6423007 -0.9918249 -0.124623 -0.02743458 -0.41582 -0.9089196 -0.03096628 -0.1745138 -0.9704825 -0.166459 -0.8075361 0.5695536 -0.1532781 -0.7424983 0.6600322 -0.1142526 -0.4784707 0.8656566 -0.1473245 -0.5621259 0.8186763 -0.1174044 -0.6791295 0.6985587 -0.2253862 -0.6528648 0.7452725 -0.1354124 -0.4593316 0.8847025 -0.07947421 -0.7216243 0.6873632 -0.08240371 -0.4595237 0.8801382 -0.1191415 -0.07923591 0.9913836 -0.1043084 -0.317144 0.9354868 -0.1558344 -0.08038234 0.991378 -0.1034824 -0.1962609 0.9718696 -0.1301972 -0.2807093 0.9569271 -0.07411462 -0.3451855 0.9338641 -0.09351408 -0.8658766 0.4999485 -0.01758944 -0.8666326 0.4986154 -0.01818639 -0.8658965 0.4999196 -0.01743125 -0.08255535 0.1339513 -0.9875433 -0.2414262 0.2217732 -0.9447381 -0.1009967 0.1441274 -0.9843917 -0.4091454 0.3108413 -0.8578915 -0.288878 0.2469624 -0.9249644 -0.5689605 0.3910308 -0.7234493 -0.4758914 0.344469 -0.8092395 -0.7198603 0.4602003 -0.5196315 -0.6312406 0.4201453 -0.6519305 -0.3205022 -0.3752069 -0.8697691 -0.484986 -0.2089191 -0.8492006 -0.343591 -0.008642852 -0.9390797 -0.8133063 -0.0784533 -0.5765222 -0.3222476 0.02990508 -0.946183 -0.2537416 0.05430632 -0.9657464 -0.4661861 -0.02593475 -0.8843065 -0.5200561 -0.361767 -0.7737354 -0.578067 -0.4521843 -0.6792408 -0.8970758 -0.3325588 -0.2909635 -0.3394201 -0.3166117 -0.8857489 -0.7565947 -0.01423305 -0.6537293 -0.5046957 -0.1695033 -0.8464933 -0.8358744 -0.06088614 -0.5455335 -0.5154704 -0.1810481 -0.8375631 -0.09070467 -0.0302515 -0.9954183 -0.7932536 -0.6086461 -0.0172894 -0.82089 -0.5710484 -0.006593644 -0.5126751 -0.8583145 -0.02145749 -0.6674931 -0.7446102 -0.002957999 -0.6085186 -0.7931495 -0.02487933 -0.3299804 -0.9437963 -0.0190168 -0.3829278 -0.9237079 -0.01141005 0.06282591 -0.9978272 -0.01985132 -0.1354871 -0.9906799 -0.01402628 -0.1303868 -0.9913498 -0.01500326 0.4552519 -0.8903506 -0.004637837 0.2609078 -0.9651296 -0.02125787 0.1296403 -0.9914991 -0.01108938 0.6504086 -0.7595265 -0.009387612 0.3808541 -0.9242808 -0.02559691 0.8158825 -0.578107 -0.01131701 0.6083651 -0.7933114 -0.02343416 0.9321315 -0.3618707 -0.01344132 0.7930665 -0.6087514 -0.02162039 0.9913412 -0.1304035 -0.01541447 0.9235284 -0.3830339 -0.01950001 0.9771147 0.2126166 0.006411254 0.9183269 0.3956201 -0.01267659 0.9912084 0.130338 -0.02276289 0.9239148 0.3822069 -0.01730054 -0.9170627 0.3983482 -0.01774525 -0.969217 0.2449226 -0.02512663 -0.9317595 0.3630246 -0.00612843 -0.9429984 0.3226647 -0.08149617 -0.9753676 0.2201198 -0.01433777 -0.9935575 0.1112231 -0.02174609 0.006643176 0.2494981 -0.9683525 0.001191616 0.07145327 -0.9974433 -0.001168131 0.9653644 0.2609027 -0.003550887 0.745227 -0.6668015 -0.3329275 -0.4332339 -0.8375367 0.1928048 -0.2570374 -0.9469732 -0.245064 0.1271607 -0.9611315 -0.5630352 0.1457141 -0.8134856 -0.5840051 0.1551659 -0.7967821 -0.3800408 0.2115523 -0.9004524 -0.9878201 0.1497415 -0.04229491 -0.8013507 0.3855225 -0.4573942 -0.3769497 0.2086427 -0.9024286 -0.9377574 0.1923171 -0.2891803 -0.9642026 -0.2525709 -0.08075606 -0.9604327 -0.2477168 -0.1273002 -0.9394863 -0.2312371 -0.2527745 -0.6175519 -0.09834474 -0.7803577 -0.7476919 -0.144622 -0.6481059 -0.41646 -0.03293925 -0.9085571 -0.5585387 -0.07810938 -0.8257927 -0.2366747 0.02110236 -0.9713599 -0.3504431 -0.01235169 -0.9365026 -0.07728278 0.06620913 -0.9948084 -0.1193575 0.05473935 -0.9913412 -0.3394113 -0.3110867 -0.8877078 -0.4561618 -0.4091686 -0.7902515 -0.6771373 -0.7358505 0.00303173 -0.1291539 -0.4066826 -0.904394 -0.0200265 0.005562067 -0.9997841 -0.1124072 -0.4762277 -0.8721078 -0.5683023 0.1810786 -0.8026476 -6.14828e-4 0.8146871 -0.5799005 -0.9259739 0.3749567 -0.04449623 -6.41935e-4 0.2896502 -0.9571325 0.9692902 0.2446306 -0.02514904 0.9851237 0.1711947 -0.01495712 0.927306 0.3736801 -0.02160447 0.9624883 0.2709837 -0.01357632 0.006716728 0.5946421 -0.8039625 0.002719879 0.1972185 -0.9803559 0.001733481 0.8522953 0.5230583 0.007690727 0.866373 -0.4993383 0.004687905 -0.804398 -0.5940724 0 -0.9882676 -0.1527328 0.6370961 0.7703988 0.02438008 0.3948693 -0.9184447 -0.02319031 -0.6352472 -0.7718507 -0.02660167 -0.1296321 0.9914258 0.01644563 -0.02631777 -0.1174358 0.9927318 -0.9864683 -0.1606829 -0.03258013 -0.9985713 0.04896593 -0.0213986 0.8391199 0.541993 0.04606032 0.2428402 -0.5492886 -0.7995691 0.1089253 0.0574823 -0.9923866 0.3137335 -0.001616179 -0.9495097 0.5156736 -0.06439179 -0.854362 0.4447368 -0.04140561 -0.8947038 0.6918915 -0.124449 -0.7111952 0.6259081 -0.1008936 -0.7733432 0.8302092 -0.1776955 -0.5283721 0.7774596 -0.1561006 -0.609253 0.9080539 -0.2132113 -0.3605262 0.9467545 -0.236028 -0.2189677 0.8884413 -0.2032863 -0.4115178 0.9642922 -0.2522484 -0.08069264 0.9568102 -0.2440736 -0.1579312 0.668851 0.1883169 -0.7191489 0.684764 0.3442775 -0.6423172 0.7445433 0.3461806 -0.5708014 0.9786211 0.09518116 0.1823223 0.412624 0.1340201 -0.9009885 0.1726398 0.02464497 -0.9846767 0.3753864 0.2018681 -0.9046185 0.4270551 0.2224457 -0.876437 0.7046585 0.204588 -0.6794117 0.977871 -0.001212954 -0.2092058 0.9968565 0.0771867 -0.01787495 -0.007658064 -0.282097 -0.9593554 -0.004795789 -0.9328382 -0.360264 -0.2265698 -0.9738606 -0.01617813 -0.4428829 -0.8965576 -0.00626111 0.06053513 -0.1648465 0.9844598 -0.4352337 -0.9001686 -0.01637774 -0.8811902 0.4711923 0.038495 0.2088249 0.7717239 -0.600695 0.5394575 0.3627561 -0.7598642 0.4832769 0.3974847 -0.7800318 0.4932919 0.7941967 0.3548448 0.419801 0.9074712 -0.01622509 0.9609576 0.2751646 -0.02907001 0.9978402 -0.007712662 -0.06523436 0.4538421 0.8909121 -0.01740932 0.9627459 0.2372199 -0.1297968 0.4341418 0.9003481 -0.02990233 0.4828333 0.8755393 -0.01741045 0.5301963 0.8478688 -0.003257036 0.4828447 0.8755327 -0.0174219 0.9909888 0.1337745 -0.006763875 0.978326 0.2057142 -0.02366524 0.9783078 0.2058969 -0.02281016 0.9902571 0.1386883 -0.01250833 0.458785 0.8693212 -0.1838396 0.5227918 0.8522825 -0.01742202 0.9904884 0.1358757 -0.02169543 0.9910815 0.1321132 -0.01742577 0.5255542 0.8500932 -0.03368079 0.9959138 0.08294057 -0.03573232 0.5428553 0.8378533 -0.05753499 0.9976046 0.066944 -0.01742476 0.5557402 0.830712 -0.03271675 0.9993575 -0.009500861 -0.03455948 0.5875416 0.809188 -0.003109395 0.9998198 -0.006495058 -0.01783913 0.6442347 0.6056054 0.4671228 0.6007714 0.7992303 -0.01745748 0.5616431 0.7938393 -0.233187 0.9966748 -0.07948219 -0.01794201 0.9966409 -0.08004128 -0.01733118 0.6268482 0.7789458 -0.01745945 0.6268766 0.7789235 -0.01743686 0.6522843 0.7577738 -0.0174399 0.9887546 -0.1483836 -0.01861739 0.6903306 0.7234796 -0.004581391 0.6631739 0.7478427 -0.03052264 0.9888123 -0.1481412 -0.01745307 0.6648594 0.7458182 -0.04143947 0.9810665 -0.1870814 -0.0500921 0.9763357 -0.2155489 -0.01753735 0.6904337 0.723376 -0.005354046 0.1181139 0.9917414 -0.04998296 0.9547237 -0.2917811 -0.05802077 0.03162229 0.9993479 -0.01742976 0.03169631 0.9993433 -0.01756083 0.7059515 0.7007747 -0.1027008 0.9588039 -0.2835508 -0.01715219 0.9333114 -0.2723795 -0.2339642 0.9479664 -0.3174321 -0.02442854 0.9312361 -0.3644165 -2.79262e-4 0.7131367 0.6999422 -0.03894764 0.9362986 -0.3507771 -0.01733559 0.8860336 -0.4624819 -0.0324819 0.9107198 -0.4125386 -0.02003628 0.724767 0.6885903 -0.02358728 0.902269 -0.4311724 -9.96075e-4 0.7268907 0.6860729 -0.03056055 0.8687073 -0.4953258 -3.89988e-4 0.09762668 0.9950693 -0.01749956 0.8391109 -0.5406829 -0.05962324 0.8430284 -0.5373706 0.02315461 0.778761 0.6263424 -0.03502184 0.8151374 -0.5780258 -0.03791081 0.6990761 0.6628282 -0.2682377 0.7909405 -0.6118894 -0.00217086 0.6756039 0.7320809 -0.0872749 0.7715392 -0.635325 -0.03300833 0.7457879 -0.6661811 0.001788079 0.7268058 -0.6860253 -0.03350895 0.6948325 -0.7191699 0.001555204 0.1626579 0.9865286 -0.01742583 0.6788622 -0.733514 -0.03321939 0.6408011 -0.7677063 9.78453e-4 0.6324908 -0.7743198 -0.01960521 0.7711192 0.6365149 -0.01496374 0.7704797 0.6112489 -0.1809307 0.5833474 -0.8122222 0.001037657 0.7902014 0.6125373 -0.01949042 0.5730143 -0.8188235 -0.03439319 0.8197439 0.5709212 -0.04548716 0.5234849 -0.8520342 -0.001206994 0.1656475 0.9859293 -0.0224567 0.5127046 -0.8580998 -0.02826333 0.4545931 -0.8906966 -0.002174258 0.4582074 -0.8887989 -0.009090244 0.8148666 0.579388 -0.01738291 0.4197679 -0.9054974 -0.06220364 0.2296882 0.9731038 -0.01767283 0.3940244 -0.9189359 -0.01736551 0.3042585 -0.9484703 -0.08849245 0.3476892 -0.8457369 -0.4047731 0.2296124 0.9731241 -0.01754152 -0.05097377 0.9985938 -0.01456534 0.3060819 -0.948309 -0.08381062 0.8514623 0.5241256 -0.0174455 0.2850819 -0.8774176 -0.3858327 0.5920485 -0.8038274 -0.0577948 0.248498 -0.966953 -0.05701631 0.2633851 -0.9641698 -0.03169953 0.2268487 -0.9735921 -0.02565592 0.8514358 0.5241745 -0.01727122 0.2210289 -0.9751107 -0.01747369 0.1643724 -0.9862438 -0.01746523 0.09474289 -0.9895967 0.1082695 0.03392904 -0.9992706 -0.01752996 0.29566 0.955133 -0.01750105 0.2840263 0.9582665 -0.03247088 0.8844732 0.4662434 -0.01801019 0.8847388 0.4657608 -0.01744943 0.9000709 0.4353938 -0.01745665 0.8836655 0.44182 -0.1546949 0.905922 0.4232344 -0.01334893 0.9109373 0.410159 -0.04430723 0.3574362 0.9337801 -0.01715403 0.9286171 0.3689679 -0.03915226 0.9257135 0.3635921 -0.1041887 0.3682057 0.9289522 -0.03837293 0.9387617 0.3428795 -0.03406137 0.9397492 0.3404453 -0.03112012 0.9610065 0.2748786 -0.03013861 -0.8098883 0.4520868 -0.3737628 0.3056178 0.935184 -0.1789658 0.363453 -0.2130518 -0.9069238 -0.9062853 0.4223064 -0.01744431 -0.9657524 0.2589656 -0.01610243 -0.9971587 0.07194203 -0.02234143 -0.9960135 0.08711385 -0.0191949 -0.9853957 -0.1690297 -0.02059912 -0.9914547 -0.1299003 -0.01199114 -0.923658 -0.3829671 -0.01386845 -0.9237279 -0.3827996 -0.01382565 -0.8310505 -0.5561792 -0.004470586 -0.7927079 -0.609208 -0.02190363 -0.556214 -0.8308078 -0.01961183 -0.7060344 -0.7081385 0.007439851 -0.6081299 -0.7931662 -0.03264272 -0.1967347 -0.9802629 -0.0195021 -0.3819829 -0.924057 -0.01441818 -0.3826528 -0.9237819 -0.01428169 0.002273857 -0.9997767 -0.02100992 -0.1309208 -0.9913281 -0.01132696 0.4251325 -0.90507 -0.01052564 0.2078403 -0.9779348 -0.02111572 0.1310158 -0.9913188 -0.0110448 0.6306746 -0.7759392 -0.01295685 0.3824838 -0.9237004 -0.02199327 0.7988576 -0.601328 -0.01520341 0.6083868 -0.793397 -0.01966965 0.793173 -0.6087509 -0.01729255 0.9433949 -0.3314995 -0.01068073 -0.967859 -0.2478538 0.0426321 -0.7052186 -0.7070775 0.05204075 0.3056754 -0.9519761 0.01743894 0.91971 -0.01671403 -0.3922427 0.8948661 -0.05632162 -0.442767 -0.009172439 0.07292979 -0.997295 -3.53885e-5 0.08633852 -0.9962659 0.1999837 0.9796766 -0.01550543 -0.3548335 0.9340741 -0.03998553 0.001449823 0.9998471 -0.01742875 -0.02588653 0.9507405 -0.308905 -0.1100001 0.8493208 -0.5162889 -0.3726887 0.08071589 -0.9244394 -0.415032 0.07925069 -0.9063486 -0.9608302 0.1384791 -0.2400604 -0.6727393 0.09872347 -0.7332636 -0.7461564 0.1149124 -0.6557788 -0.8259885 0.08917856 -0.5565881 -0.932902 0.0532763 -0.3561678 -0.9501767 -0.0249781 0.3107096 -0.6587831 0.5418678 -0.5219045 -0.09865701 0.1118282 -0.9888182 -0.9992225 0.004434466 -0.03917664 -0.2412959 0.3143754 -0.9181202 -0.5885872 0.6076748 -0.5331946 -0.4845125 0.2881512 -0.825964 -0.7151302 0.3029219 -0.6299422 -0.9551377 0.157372 -0.2508906 -1.49844e-4 0.08723765 -0.9961875 9.84933e-5 0.08716034 -0.9961943 5.65442e-5 0.0871545 -0.9961949 -2.05614e-5 0.0871542 -0.9961948 -9.28414e-5 0.08719211 -0.9961916 0.8572023 -0.4812043 -0.1834303 0.8204934 -0.4295709 -0.3771731 0.7695438 -0.2885867 -0.5696668 0.623104 -0.2385817 -0.7448625 0.4584674 -0.1583541 -0.8744894 0.2871868 -0.05484181 -0.9563034 0.1144874 0.06702387 -0.9911612 0.6337772 0.1718333 -0.7541882 0.5273324 0.5322513 -0.6622909 -0.04201835 0.184068 -0.9820151 -0.131558 0.2713374 -0.9534509 0.02345329 0.5464807 -0.8371433 0.09357953 -0.04452174 -0.9946159 0.3060773 0.9437597 -0.1250377 0.8658874 0.4999329 -0.01750373 0.8657974 0.5000613 -0.01827263 0.8657208 0.5001959 -0.01821553 0.6975309 -0.7158346 -0.03211635 0.6588159 -0.7522369 -0.01008093 0.7892253 -0.6133147 -0.03112179 0.7443876 -0.6677188 -0.006227791 0.8654417 -0.5001417 -0.02947884 0.8223386 -0.5689606 -0.006562411 0.9258183 -0.3769249 -0.02807629 0.8880087 -0.4597427 -0.008791565 0.9688109 -0.246547 -0.02490448 0.9379603 -0.3465665 -0.01105386 0.9738965 -0.2266215 -0.01297652 0.003775477 -0.2311967 -0.9728997 0.001954972 -0.7448254 0.6672566 0.009796977 -0.7488414 -0.6626769 0.005947649 0.006008625 -0.9999644 0.001187443 0.001405954 -0.9999983 -0.001289129 2.71884e-6 -0.9999992 0 0 -1 0.001600563 4.25603e-4 -0.9999986 -2.21711e-5 5.15343e-4 -1 -0.004018306 7.56931e-4 -0.9999917 -4.71682e-4 -3.96609e-4 -0.9999999 -0.003982305 7.39771e-4 -0.9999918 -0.00559026 7.52905e-4 -0.9999842 -0.003500401 0.002630114 -0.9999905 -0.003407895 0.002476632 -0.9999912 0.001303017 0.001205921 -0.9999985 3.29478e-4 -0.001300334 -0.9999992 0.002667486 0.001939296 0.9999946 0.01188755 -0.006649374 -0.9999073 -0.004068136 0.002653002 -0.9999883 -7.01107e-4 0.003692924 -0.999993 -9.98235e-4 3.52876e-4 -0.9999995 0.002010405 -7.12697e-4 -0.9999978 0.001089453 -0.001382827 -0.9999985 9.39853e-6 -4.38666e-4 -1 2.01011e-4 -2.5889e-4 -1 1.97444e-4 7.08237e-4 -0.9999998 0.001919806 0.001430928 -0.9999972 0.003155827 0.001440823 -0.999994 0.322452 -0.08504688 -0.9427576 0.1966403 -0.7764387 -0.5987284 0.2497592 -0.7370274 -0.6280215 0.6536307 0.3915511 -0.6476533 0.5903223 -0.4748398 -0.6527226 0.6045763 -0.1554718 -0.7812273 0.6805367 -0.6058624 -0.4120687 0.5245425 -0.06124091 -0.8491789 0.4380078 -0.5233508 -0.7309263 0.3804122 -0.1332275 -0.9151706 0.3467541 0.03383255 -0.9373458 0.6188791 0.04438018 -0.7842316 0.09640675 -0.2838561 -0.9540081 0.5029916 -0.411578 -0.7600021 0.3935278 -0.2050697 -0.8961486 0.8688861 -0.0850147 -0.4876572 0.6755713 -0.1678099 -0.7179439 0.8142556 0.4954043 -0.3025931 0.7285761 0.4637212 -0.5041226 0.7691891 0.479471 -0.4224402 0.6135835 0.4123151 -0.6734327 0.661734 0.4340903 -0.6112887 0.4700563 0.3418814 -0.8137348 0.5109562 0.3620746 -0.7796319 0.2977809 0.2519924 -0.920775 0.3214565 0.2644111 -0.9092593 0.105393 0.1467199 -0.9835475 0.1093399 0.1488768 -0.9827922 -0.1262555 -0.1515582 -0.9803518 -0.8108012 0.1443086 -0.5672536 0.9222575 0.373454 0.09986656 -0.9813497 -0.1079581 -0.1590529 -0.2028084 -0.07924318 0.9760068 -0.9779323 0.1064494 -0.1797687 -0.3929042 0.5616801 -0.7281084 -0.7719559 -0.4020763 -0.4923605 -0.786713 -0.5018811 0.3594417 -0.4434925 -0.1471323 -0.884119 -0.8588351 -0.4935837 -0.1370311 -0.575948 -0.1759144 -0.7983345 -0.6226598 -0.2296928 -0.7480215 -0.6537285 -0.4010326 -0.6417258 -2.79378e-5 0.08716267 -0.9961941 2.79124e-5 0.08721518 -0.9961895 1.30032e-4 0.08723223 -0.996188 3.74406e-5 0.08711791 -0.9961981 2.92089e-5 0.08711755 -0.9961981 0.9905681 0.06831187 -0.1187791 0.9786528 0.1664683 -0.1205288 0.9297733 0.1155501 -0.3495279 0.8262585 0.08086848 -0.557456 0.6730902 0.06450837 -0.7367416 0.4800156 0.06845444 -0.8745851 0.2900809 0.08973658 -0.9527856 0.09860008 0.1229361 -0.9875044 0.7901788 0.490428 -0.3675568 0.9215094 0.08276015 -0.3794354 0.9208223 0.08393949 -0.3808419 0.4450519 0.3168638 -0.8375716 0.4257731 0.3293683 -0.8427537 0.9822725 0.1163057 -0.1470164 0.9396298 0.1814679 -0.2901126 0.6422987 0.4132605 -0.6454985 0.3725167 0.5265287 -0.7641982 0.8156288 0.5689274 -0.1052205 -0.993348 -0.04814249 -0.1046051 -0.9909792 -0.09853947 -0.09083127 -0.9789499 -0.1606322 -0.1259152 0.8573135 -0.1566362 -0.4903863 0.818828 -0.1275008 -0.5597004 0.2220904 0.2632218 -0.9388239 0.4172664 0.5991024 -0.6833484 0.8546844 -0.2495273 -0.4552481 0.9001318 -0.2970665 -0.3186135 0.4921501 0.602182 -0.6286216 0.2726081 0.1897661 -0.9432252 0.58671 0.02086454 -0.8095283 0.3171463 0.533962 -0.7837747 0.8769371 -0.3622633 -0.315827 0.8282413 -0.3091284 -0.467393 0.4061344 0.09696304 -0.9086546 0.7183862 0.6175483 -0.3202431 0.02449041 0.317086 -0.9480805 0.3693457 0.5211806 -0.7693859 0.8476724 -0.4304297 -0.3101319 0.7940514 -0.3601919 -0.4896367 0.592258 -0.1095569 -0.7982656 0.03306704 0.3239716 -0.9454888 0.7674853 0.569895 -0.2935746 0.7484184 0.5702096 -0.3387198 0.2849537 0.4639838 -0.8387613 -0.1289287 0.9442786 -0.3028457 0.7644937 -0.4218138 -0.4874656 0.2120555 0.1812483 -0.9603028 -0.0988717 0.8033539 -0.5872367 -0.008352816 0.4216629 -0.9067143 0.787645 -0.5348446 -0.3058701 0.7501835 0.5309617 -0.394087 0.4138754 0.4959079 -0.7634021 0.1640152 0.2075411 -0.9643785 0.9062183 0.422672 -0.0108149 0.7232336 -0.4699016 -0.5060886 0.2775105 0.4356075 -0.8562909 -0.06420648 0.7330585 -0.6771283 0.7723662 0.443996 -0.4542226 0.7323853 -0.5700756 -0.3723247 0.6801826 -0.5219135 -0.5147407 -0.03305983 0.8993564 -0.4359647 0.254845 0.08960652 -0.9628213 0.2880407 0.05111938 -0.9562528 0.685863 -0.6178818 -0.3844662 -0.01682883 0.8444594 -0.5353553 0.03957182 0.938471 -0.3430834 0.1872208 0.1362003 -0.9728298 0.6348634 -0.5865366 -0.5029148 0.6676886 0.4468281 -0.5954297 -0.01130592 0.4717861 -0.8816406 0.6392322 -0.64346 -0.4211193 0.4300564 -0.2269504 -0.8738107 0.1595518 0.1163186 -0.9803128 0.5819832 -0.6262381 -0.5187693 0.5835621 -0.7343598 -0.346657 0.5665253 0.4049096 -0.7177028 0.07023763 0.8473467 -0.5263747 0.03376299 0.4528335 -0.8909557 0.7794286 0.35962 -0.5129956 0.2718948 -0.04289162 -0.9613708 0.1416691 0.9312682 -0.3356629 0.08651131 0.9559765 -0.2804012 0.5302026 -0.6922794 -0.4895248 0.9499263 0.2555133 -0.1798692 0.9511789 0.208707 -0.2273771 0.5299415 -0.7416532 -0.411233 0.619589 0.3703506 -0.6920621 0.06368386 0.7248481 -0.685959 0.3692325 -0.3340414 -0.8672276 0.3166486 -0.2106068 -0.9248668 0.4698651 -0.7526152 -0.4612995 0.08724933 0.3412998 -0.9358964 0.02258443 0.3366487 -0.9413595 0.9556267 0.203329 -0.2131547 0.9770082 0.1186802 -0.1771163 0.2487987 0.9201743 0.3022889 0.4691453 -0.7222264 -0.5082242 0.1775809 0.8439764 -0.5061312 0.05767816 0.5418333 -0.8385047 0.07980066 0.1708689 -0.9820569 0.222833 0.8700581 -0.4397094 -0.173852 0.4509127 -0.8754733 0.6308615 0.3194969 -0.7070612 0.03006941 0.3259704 -0.9449017 0.9041885 0.2117627 -0.3709446 0.3897228 -0.7942311 -0.4661687 0.3136402 0.9091426 -0.2740249 0.1774886 -0.05149203 -0.9827749 0.3313602 0.9209702 -0.2049739 0.6041659 0.2955516 -0.7400222 0.04714721 0.4279049 -0.9025934 0.2143904 -0.2766705 -0.9367445 0.05495136 0.4368124 -0.8978727 0.2107561 -0.2677395 -0.9401583 0.344402 0.8691368 -0.3549485 0.896462 0.148533 -0.4174852 0.4280902 0.8959783 -0.1181604 0.2871617 -0.8367313 -0.466282 0.05046868 0.4193764 -0.9064086 0.4740631 0.2776186 -0.8355789 0.3658751 0.8286195 -0.423704 0.9077466 0.05099266 -0.4164084 0.2391257 -0.7539195 -0.6119021 0.1119786 -0.09183782 -0.9894577 0.4226186 0.8142088 -0.3980677 0.1360754 -0.2797068 -0.9503934 0.07083314 0.4372578 -0.8965426 0.5505137 0.8344579 -0.02479439 0.8074724 0.1465349 -0.5714156 0.6538074 0.2058215 -0.7281301 0.920271 0.009902656 -0.3911562 0.4814606 0.210815 -0.8507365 0.4586769 0.7798603 -0.4259499 0.06950491 0.4101924 -0.9093466 0.9383252 -0.0953741 -0.3323398 0.6742043 0.1110764 -0.7301442 0.08710956 0.427369 -0.8998709 0.5867128 0.1468949 -0.7963605 0.5195334 0.7578561 -0.3946383 0.6547518 0.7534879 -0.05963414 0.07884895 0.3967724 -0.9145243 0.602015 0.1193073 -0.7895212 0.5268384 0.712715 -0.4631184 -0.5374438 0.4960293 -0.6819891 -0.7351157 -0.4681906 -0.4903085 -0.7495039 -0.6028441 -0.2735381 -0.5711227 -0.2288311 -0.7883243 -0.2159776 0.1483519 -0.9650624 -0.4465846 -0.09380793 -0.8898103 -0.7949978 -0.4793554 -0.3717486 -0.4085441 0.5215702 -0.7490369 -0.7239633 0.5675644 -0.392107 -0.7683182 0.5936062 -0.2394142 -0.7985122 -0.5123628 -0.3160107 -0.3616631 0.02727746 -0.9319098 -0.4693273 0.5525121 -0.6888122 -0.6803869 0.612783 -0.4019585 -0.8359088 -0.4839173 -0.2589994 -0.1082444 0.37536 -0.9205369 -0.8163411 -0.3899868 -0.4260254 -0.5368214 -0.0651254 -0.8411786 -0.659919 0.6557759 -0.3666948 -0.7330788 0.6448965 -0.2161113 -0.2647976 0.1391472 -0.9542119 -0.303821 0.5168177 -0.8003702 -0.119235 0.4189168 -0.9001621 -0.8564863 -0.3552134 -0.3745059 -0.8485378 -0.3323271 -0.4117552 -0.5676068 0.6862605 -0.4548287 -0.2304161 0.1850662 -0.9553319 -0.6932491 0.7179917 -0.06240087 0.1594609 -0.8967226 -0.4128692 -0.3636576 0.15088 -0.9192326 -0.8966566 -0.3279185 -0.2974498 -0.897188 -0.3297536 -0.2937967 0.086501 -0.1122834 -0.9899041 -0.2487598 0.531223 -0.8098893 -0.527155 0.7259752 -0.4416648 -0.3796358 0.1388311 -0.9146598 0.1822382 -0.9626152 -0.2004029 -0.2617515 0.5400832 -0.7998727 0.08017832 -0.9964213 -0.02675712 0.1105321 -0.6995799 -0.7059537 -0.8517549 -0.1689974 -0.4959371 -0.5031174 0.7691345 -0.3940875 0.0476635 -0.1466658 -0.9880371 0.05666124 -0.3145427 -0.9475507 -0.5210147 0.09600877 -0.848131 0.02652758 -0.9991106 -0.03277778 -0.5406871 0.7924848 -0.2821797 0.02271562 -0.7187559 -0.6948914 -0.2398757 0.5775556 -0.7803135 -0.9057011 -0.1514607 -0.3959362 -0.1837257 0.5178924 -0.8354834 -0.9497918 -0.2755625 -0.1481919 -0.4542026 0.7993881 -0.3932921 0.02622538 -0.03942924 -0.9988782 -0.03666692 -0.8926643 -0.4492282 0.05021339 -0.8887225 -0.4556876 -0.3994431 0.172895 -0.9003069 -0.5274599 0.8480499 -0.05096286 -0.08364135 0.4389345 -0.8946177 -0.02511882 -0.8432788 -0.536889 -0.6191117 0.1127007 -0.777174 -0.9243782 -0.09731847 -0.3688554 -0.459595 0.8461638 -0.2697764 -0.945572 -0.1372256 -0.2950642 -0.02544444 -0.2695607 -0.9626472 -0.02504056 -0.2644956 -0.9640619 -0.1008379 -0.8673344 -0.4874041 -0.5966519 0.1253154 -0.7926555 -0.1205676 -0.9249448 -0.3604727 -0.1738507 0.5989396 -0.7816952 -0.3396074 0.8212969 -0.4584083 -0.401807 0.2405799 -0.8835567 -0.952499 -0.06296837 -0.2979609 -0.03069031 -0.09683144 -0.9948275 -0.1715954 -0.8417116 -0.5119341 -0.9825507 -0.1078136 -0.15156 -0.2089711 -0.9383643 -0.2753246 -0.3030071 0.8633365 -0.4035306 -0.1491378 -0.5585805 -0.8159325 -0.4247847 0.9046561 0.03399127 -0.1356319 -0.4040687 -0.9046173 -0.1140725 0.5427834 -0.8320899 -0.240856 -0.8277129 -0.506833 -0.9132197 0.04917752 -0.4044889 -0.6585537 0.1805681 -0.7305492 -0.05122351 0.4144955 -0.9086086 -0.192313 -0.289024 -0.9378064 -0.2520866 0.8827997 -0.3963801 -0.3067212 -0.7938416 -0.5251075 -0.2672293 -0.9061731 -0.3277789 -0.8909606 0.127905 -0.4356943 -0.9916867 -0.02055525 -0.127024 -0.2894325 0.9433141 0.1624417 -0.3695368 0.2894652 -0.8829795 -0.05201888 0.4148092 -0.9084204 -0.2225243 -0.3541576 -0.9083256 -0.2448472 -0.4187169 -0.8744862 -0.8837144 0.1902546 -0.4276121 -0.9423619 0.1344396 -0.3063987 -0.404717 -0.8571386 -0.3186187 -0.5694471 0.2849766 -0.7710502 -0.5985332 0.2801455 -0.7505176 -0.1190651 0.7889594 -0.6027991 -0.1935909 0.9234543 -0.3312925 -0.4307996 -0.7439615 -0.5108162 -0.9392533 0.1986302 -0.2799094 -0.9341121 0.2039389 -0.2929907 -0.0252676 0.4057004 -0.9136569 -0.1827965 -0.1249163 -0.9751828 -0.3773057 0.3377203 -0.8623141 -0.334002 -0.3564123 -0.8725898 -0.4938519 -0.7200208 -0.4875249 -0.4827646 -0.8371874 -0.2570134 -0.2527198 -0.1354678 -0.9580091 -0.9054735 0.2827641 -0.3164843 -0.9096519 0.2795653 -0.3072078 -0.6898018 0.3518882 -0.6327307 -0.4741054 0.3626521 -0.8023139 -0.5488252 -0.6678236 -0.5027948 -0.8170539 0.3680761 -0.4437825 -0.5883259 -0.7625654 -0.2690106 -0.3005813 -0.115106 -0.9467849 -0.600747 -0.6285336 -0.4940128 -0.3998156 -0.2631831 -0.8779991 -0.8056985 0.4230876 -0.4145443 -0.9429407 0.3228971 -0.08124262 -0.4317135 0.4161365 -0.8002837 -0.3810684 0.411587 -0.8278787 -0.3083586 -0.04017388 -0.9504216 -0.6457418 -0.5634506 -0.5153067 -0.6495358 -0.677825 -0.3444656 0.7090567 0.6225363 0.3311905 -0.7764828 0.4764357 -0.4124118 -0.900387 0.4345514 -0.02164405 -0.6979038 -0.5270385 -0.4849337 -0.6255008 -0.4476719 -0.6390138 -0.4849842 -0.2097282 -0.849002 -0.318927 0.4326843 -0.8432496 -0.01114815 0.3061091 -0.9519312 -0.08940637 0.3021222 -0.9490673 -0.01183807 -0.3308862 0.9435964 0.3812455 0.1799346 -0.9067941 -0.3837681 -0.7618862 -0.5217773 -0.2377001 0.1635396 -0.9574725 -0.9340347 0.3556145 -0.03343212 -0.4784905 0.4969938 -0.7239088 -0.3420487 -0.2554525 -0.9042935 -0.1180536 0.03379499 -0.9924321 0.5684778 -0.438561 -0.6960585 0.5249212 -0.3864624 -0.7583565 -0.6731618 -0.04071927 -0.7383733 -0.05564069 0.3733245 -0.9260308 0.1169818 -0.9385678 -0.3246629 -0.177271 0.1976495 -0.9641109 -0.6677029 0.1213418 -0.7344719 -0.2178441 -0.8611721 -0.4592675 -0.1616703 -0.2185791 -0.9623336 -0.2367581 -0.9165654 -0.3222635 0.1008024 -0.8077569 -0.5808337 0.01659697 -0.04828298 -0.9986959 0.165016 -0.171145 -0.9713286 -0.2742045 -0.763395 -0.5848418 -0.1293601 -0.238587 -0.9624667 0.4331855 -0.7713182 -0.4662816 0.1377978 -0.05496454 -0.9889341 0.03754687 -0.2560068 -0.9659456 0.01852482 -0.9588219 -0.2834035 -0.05684381 0.2209727 -0.9736221 -0.3627758 -0.8471868 -0.3881602 0.09478485 -0.1673331 0.9813336 -0.3654302 -0.861703 -0.3520352 0.07673263 -0.2962031 -0.9520378 0.3642003 -0.7489216 -0.5536013 0.1351325 -0.05427086 -0.9893401 0.195362 -0.2053782 -0.9589858 0.3105512 -0.9190507 -0.2427013 0.004593431 0.2723103 -0.9621986 0.1415517 -0.2403655 -0.9603061 0.1371763 -0.1848433 -0.9731473 -0.1211454 -0.7621538 -0.6359602 -0.1444433 -0.8333992 -0.5334622 -0.0803681 -0.9926818 -0.09013247 -0.05872541 0.07822561 -0.9952046 0.09048825 -0.3243631 -0.9415947 -0.06559699 0.01535934 -0.997728 0.06254374 -0.03663164 -0.9973698 0.1888007 -0.9462213 -0.2627161 0.1908652 0.4375696 -0.8786941 0.4134983 0.5718839 -0.7084971 -0.2125352 -0.281384 -0.9357628 0.2161536 0.2706513 -0.9380968 0.1414843 0.2088841 -0.9676517 0.5234342 -0.4625871 -0.7155627 0.07173377 0.244619 -0.9669622 0.3804565 0.4791671 -0.7909815 -0.6740838 -0.1819477 -0.7158953 0.8390656 0.432393 -0.3301596 0.1902063 -0.9175955 -0.349056 0.9984402 -0.04189252 -0.03690981 -0.2686491 -0.8979908 -0.3484826 0.365461 0.1570256 -0.9174864 0.225745 0.1904191 -0.9553951 0.8579272 -0.3936908 -0.330104 0.1195268 -0.01017445 -0.9927788 0.1472986 -0.004702091 -0.9890809 0.7239853 0.2035472 -0.6591008 0.4650671 0.5592991 -0.6862195 -0.0157485 0.44999 -0.8928948 -0.02806693 0.5381395 -0.8423885 -0.2238438 0.9630908 -0.1494993 -0.5689935 0.4747722 -0.6714445 -0.3463551 0.3196774 -0.8819549 0 -0.988774 -0.1494199 -1.22203e-5 -0.9894568 -0.1448285 1.42814e-4 -0.8739078 -0.4860919 0 -0.202057 -0.9793738 -0.7948166 -0.606701 -0.01344382 0.1345033 -0.9907989 -0.0150485 0 -0.02619647 -0.9996569 -1.8036e-4 -0.2540693 -0.967186 2.8782e-4 -0.06771183 -0.9977049 -2.31208e-4 -0.4756847 -0.879616 2.03801e-4 -0.3351733 -0.9421565 -1.97565e-4 -0.6704987 -0.7419108 2.20356e-4 -0.5625141 -0.8267877 -2.05502e-4 -0.821622 -0.5700327 1.95691e-4 -0.7428131 -0.6694989 -1.32969e-4 -0.9233735 -0.3839029 1.26749e-4 -0.8717249 -0.489996 1.26138e-4 -0.9498954 -0.3125681 -1.90647e-4 -0.9906941 -0.1361072 0 -0.9872633 -0.1590952 -0.3135599 0.8580219 -0.4067906 -0.06284147 0.986127 0.1536379 -0.6202735 0.4045677 -0.6720014 -0.8164166 0.4953728 -0.2967656 -0.07970601 0.1389437 -0.9870874 -0.8459988 0.501763 -0.1803337 -0.2411617 0.2222452 -0.9446947 -0.3966538 0.3322187 -0.8557433 -0.5378183 0.4426236 -0.7175207 -0.4948171 0.4358876 -0.75177 -0.7566575 0.4759057 -0.4483115 -1.9405e-5 0.0872032 -0.9961906 2.54354e-6 0.08717024 -0.9961935 0 0.0868591 -0.9962206 1.37349e-5 0.08707183 -0.9962021 -8.61967e-5 0.08706593 -0.9962025 3.80679e-5 0.08711546 -0.9961983 6.27895e-5 0.087076 -0.9962018 -9.78465e-5 0.08726793 -0.9961849 2.94384e-5 0.08714228 -0.9961959 -1.68899e-5 0.08719348 -0.9961915 0.3823835 0.7145677 -0.5858123 0.3615347 0.5315485 -0.7659954 -0.865882 -0.499944 -0.01744854 -0.8480592 -0.5277022 -0.04822742 0.1092318 0.175702 -0.9783646 0.9305779 -0.3590535 -0.07145273 -0.2103502 0.9774075 -0.02067571 -0.1631088 0.986454 -0.01743644 0.5074011 0.7918173 0.3399553 0 0.9868586 -0.1615871 0.5116702 0.01210439 -0.8590968 0.5122812 -0.1559579 -0.8445384 0.5192809 -0.01636803 -0.854447 0.3750426 0.03824037 -0.9262186 0.3717208 0.03986954 -0.9274882 0.552722 -0.02673155 -0.8329368 0.5072889 -0.1551281 -0.8476989 0.2227464 0.0625301 -0.972869 0.3470036 -0.07059496 -0.9352032 0.349781 -0.07134962 -0.9341106 0.2086976 -0.003470063 -0.9779741 0.1739966 0.004588901 -0.9847356 0.6401822 -0.2943961 -0.7095757 0.052684 0.06331193 -0.9966023 0.7087071 -0.08559888 -0.7002907 0.4826759 -0.1985169 -0.8530036 0.5029158 -0.2027001 -0.8402311 0.3345854 -0.09597218 -0.9374657 0.3390131 -0.09989809 -0.9354628 0.2038255 -0.02047103 -0.9787933 0.04864084 0.06472921 -0.9967167 0.6327751 -0.07909423 -0.7702856 0.6267565 -0.371126 -0.6851582 0.50789 -0.02509623 -0.8610563 0.3686965 0.0132184 -0.9294559 0.3730295 0.01082867 -0.9277564 0.4731901 -0.2293711 -0.8505764 0.4793634 -0.239035 -0.8444366 0.2250528 0.04486405 -0.9733132 0.2217631 0.04651296 -0.9739907 0.6072705 -0.4317271 -0.6669591 0.3219844 -0.1233581 -0.938674 0.3245314 -0.1245858 -0.9376342 0.7889783 -0.009296178 -0.6143508 0.05640316 0.07284814 -0.9957469 0.1929845 -0.03473335 -0.9805868 0.2021711 -0.0387789 -0.9785822 0.06233894 0.04985785 -0.996809 0.07087224 0.07511574 -0.9946532 0.7004117 -0.1613593 -0.6952602 0.4393929 -0.270165 -0.8567057 0.6284559 -0.1069365 -0.7704595 0.3900961 -0.2420648 -0.8883861 0.3056031 -0.1468883 -0.9407607 0.3857911 -0.2381058 -0.8913309 0.585398 0.03403967 -0.8100312 0.6250615 0.006617426 -0.7805475 0.04842996 0.05198061 -0.9974731 0.1809128 -0.04721885 -0.982365 0.1318843 -0.01950168 -0.9910733 0.5307913 -0.08935248 -0.8427793 0.6645717 -0.006578266 -0.7471956 0.4868879 -0.05883151 -0.871481 0.3639868 -0.01615154 -0.9312641 0.08052569 0.02278834 -0.996492 0.5509663 0.03234684 -0.8339003 0.3666771 -0.0163843 -0.930204 0.3826143 0.06685352 -0.9214863 0.2199884 0.02880513 -0.9750772 0.3762628 0.06551921 -0.9241935 0.1901891 0.02982282 -0.9812945 0.225178 0.07781046 -0.9712057 0.4191168 -0.3177411 -0.8505186 0.6976437 -0.2176615 -0.682581 0.6818886 -0.1990113 -0.7038626 0.2299262 -0.129648 -0.9645338 0.7826705 -0.05286926 -0.620187 0.3504201 -0.2904958 -0.8904033 0.06668937 0.06825357 -0.9954366 0.2122011 -0.1137601 -0.970582 0.04468047 0.04452711 -0.9980085 0.3332281 -0.3320595 -0.8824374 0.1511835 -0.09662187 -0.9837722 0.7167264 -0.04680889 -0.6957817 0.5277426 -0.1230083 -0.8404503 0.5212764 -0.1160463 -0.8454609 0.06507444 0.08302962 -0.9944202 0.203145 0.07089632 -0.9765787 0.3555943 -0.04248791 -0.9336743 0.3599451 -0.0455597 -0.9318605 0.06605392 0.08308649 -0.9943509 0.2170412 0.01186931 -0.9760903 0.6708956 -0.2598479 -0.6945345 0.6716539 -0.2600616 -0.6937212 0 0.08727461 -0.9961843 2.09519e-6 0.08726298 -0.9961853 -2.02089e-5 0.08716887 -0.9961937 -1.35135e-5 0.08714497 -0.9961957 2.02684e-4 0.08710235 -0.9961994 3.67854e-5 0.08716803 -0.9961937 8.62556e-6 0.08714079 -0.9961961 -7.64416e-6 0.0871576 -0.9961946 3.61351e-6 0.08716702 -0.9961937 2.94406e-6 0.08711361 -0.9961984 1.19128e-5 0.08714097 -0.996196 -2.83897e-5 0.08716833 -0.9961936 -1.48116e-5 0.08715629 -0.9961947 4.59751e-4 0.999665 -0.02587985 -5.57677e-5 0.9926831 -0.1207498 -6.5121e-5 0.9508345 -0.3096994 3.24497e-5 0.9569602 -0.2902194 7.74261e-5 0.8831816 -0.4690313 -3.49741e-5 0.5847048 -0.8112462 4.75912e-5 0.7546411 -0.6561378 -1.36916e-4 0.3808294 -0.9246453 1.93323e-4 0.5573434 -0.8302821 0 0.2055603 -0.9786446 3.11643e-4 0.9992885 -0.0377165 0.4816757 0.8763495 -5.44698e-4 0.9945973 0.1008915 -0.02443641 0.9952591 0.09251862 -0.02999877 0.05501073 0.9983168 -0.01837068 -0.001227736 0.08342367 -0.9965134 8.04808e-6 0.08718234 -0.9961924 5.02941e-5 0.08707958 -0.9962014 8.93631e-5 0.08729833 -0.9961823 3.49748e-5 0.08719235 -0.9961916 -7.22044e-5 0.08721572 -0.9961895 -3.41491e-5 0.08716732 -0.9961937 -5.60173e-5 0.08708095 -0.9962013 2.26488e-5 0.08713823 -0.9961963 -7.87795e-6 0.08719015 -0.9961917 6.38026e-5 0.08784925 -0.9961339 0 0.08715724 -0.9961947 -5.15546e-4 0.08608669 -0.9962876 -5.1058e-4 0.9996885 -0.02495217 -1.00538e-4 0.9952517 -0.09733521 -8.45191e-5 0.9535759 -0.3011531 -1.4832e-4 0.8720766 -0.4893695 1.48526e-4 0.9402575 -0.3404643 -1.77678e-4 0.7317562 -0.6815665 1.42954e-4 0.8397544 -0.5429664 -1.78091e-4 0.5225595 -0.8526028 1.71818e-4 0.6894997 -0.724286 -1.6664e-4 0.2440295 -0.9697679 1.14161e-4 0.4857712 -0.8740861 0 0.2252332 -0.974305 -0.5245851 0.8511328 -0.01958388 0.4657496 0.8843129 -0.03268164 0.7941617 0.6075145 -0.01527553 0.5204436 0.8538168 0.01163733 -0.1874588 0.9821284 -0.01682329 -0.443153 0.896244 -0.01903069 0.184594 0.9826707 -0.01683264 -0.1829915 0.9829654 -0.01712495 -0.1844109 0.9826987 -0.01720762 -0.5226997 0.8523034 -0.019082 -0.1799066 0.9835212 -0.01788359 -0.4351152 0.9001329 -0.0208736 -0.2717869 0.9622773 -0.01242482 -0.1674947 0.984048 -0.05996072 0.255595 0.9575524 -0.1332842 0.1842613 0.9827328 -0.01685422 0.5231753 0.8520411 -0.01771563 -0.7294518 0.6838464 -0.01594477 0.7925609 0.6095511 -0.01716595 0.5271005 0.8496297 -0.01717233 -0.5195952 0.8541504 -0.02116817 -0.7092081 0.7046338 -0.02269548 -0.5068815 0.8619156 -0.01314467 -0.5385354 0.8425887 -0.004913926 0.7601742 0.6492167 -0.02555465 0.6442945 0.7644415 -0.02267134 0.4890121 0.8721647 -0.01400649 -0.1826227 0.9830332 -0.01716715 0.4167261 0.909015 0.00559169 0.1662864 0.9844008 -0.05748057 0.1049458 0.9943267 -0.01734465 0.1743038 0.984641 0.01001584 0.3883825 0.9211075 -0.02683168 0.6556533 0.754984 -0.010872 0.4911701 0.8705474 -0.02998858 -0.8237248 0.5669432 0.007289052 -0.5982032 0.8005566 -0.03552824 -0.505485 0.8627151 0.01440769 -0.7819308 0.6227918 -0.02673906 -0.5062995 0.8617933 -0.03119486 -0.1725758 0.9849842 -0.004884958 0.5024514 0.8644323 -0.01730585 0.1828273 0.9829968 -0.01707631 -0.7291669 0.6841812 -0.01455712 0.9150962 -0.01189994 -0.4030601 0.04617112 0.3718845 -0.9271301 0.3512721 0.2599726 -0.8994566 0.4656726 0.6722251 -0.5755541 0.1265932 0.4296297 -0.8940876 0.9464831 -0.1044294 -0.3053923 0.8038224 -0.006374061 -0.5948353 -0.1502536 0.9460492 -0.28708 0.4055552 0.2087474 -0.8899155 -0.02052766 0.4365846 -0.8994291 0.9266076 -0.1694227 -0.3357 -0.05331158 0.6723908 -0.7382741 0.1198758 0.4204356 -0.8993686 0.6258784 0.6318897 -0.4571562 -0.06211191 0.8760573 -0.4781902 0.2342749 0.4766412 -0.8473067 0.7894359 0.6036949 -0.1111011 0.9145312 -0.234364 -0.329706 -0.02842622 0.9603117 -0.2774768 0.2352633 0.2301506 -0.9442892 0.7736172 0.5690213 -0.2788034 0.1584368 0.4080452 -0.8991091 0.7968435 0.5663605 -0.2104191 0.8641972 -0.189377 -0.466154 0.8981199 -0.2927939 -0.3281043 0.02496176 0.843185 -0.537044 0.02831614 0.807726 -0.5888777 0.1711883 0.4154095 -0.893381 0.7985602 0.5246832 -0.2949735 0.8209337 0.514862 -0.2469514 0.3237265 0.4494889 -0.8325629 0.1711465 0.2493649 -0.9531663 0.8668488 -0.3326078 -0.3714097 0.8613628 -0.2932158 -0.4148235 -2.04755e-4 0.3553115 -0.9347479 0.06323343 0.8305506 -0.5533421 0.06538909 0.8385578 -0.5408744 0.2397021 0.4017055 -0.8838415 0.8014383 -0.2878979 -0.5242245 0.8758279 0.4474009 -0.180992 0.7729042 0.4667438 -0.4298483 0.2126034 0.4006181 -0.8912379 0.1602397 0.9816783 -0.1031072 0.8767322 0.4015038 -0.2648311 0.4023007 0.04156124 -0.9145638 0.8057378 -0.4899926 -0.3327072 0.1571459 0.263454 -0.9517864 0.9100605 0.3879375 -0.1459255 0.023974 0.4682281 -0.8832824 0.1820663 0.9536272 -0.2396813 0.1461893 0.3674952 -0.918464 0.7760758 -0.5282784 -0.3444244 0.2379179 0.3781359 -0.8946555 0.9166652 0.3258246 -0.2314377 0.9312363 0.3172661 -0.1792799 0.7268724 -0.5680516 -0.3859716 0.7118315 -0.4341071 -0.5521295 0.6108108 -0.2819833 -0.7398619 0.5978141 0.3931803 -0.6985897 0.2086644 0.3777747 -0.9020785 0.1761646 0.7065968 -0.6853373 0.667856 -0.5036516 -0.5479996 0.6886961 -0.6408206 -0.3391853 0.2809281 0.04804748 -0.9585254 0.982473 0.1683473 0.08003741 0.2331052 0.09654867 -0.9676469 0.1621242 0.6915829 -0.7038671 0.119058 0.344031 -0.9313796 0.6455504 -0.6793476 -0.3489291 0.5881761 -0.5893971 -0.553769 0.1847643 0.09193581 -0.9784733 0.8250647 0.2995377 -0.4791091 0.2295326 0.02193987 -0.9730538 0.5895913 -0.7726734 -0.2352827 0.7065404 0.3359782 -0.6228317 0.2026893 0.3472384 -0.9156106 0.5245786 -0.7481341 -0.4063408 0.5026707 -0.6947492 -0.5144374 0.9481349 0.2060922 -0.2420046 0.227393 -0.05730646 -0.9721155 0.08520269 0.4616872 -0.8829414 0.4730133 -0.8257254 -0.3073046 0.2590136 -0.1075394 -0.9598684 0.9466246 0.1536238 -0.2833755 0.1944183 -0.08474224 -0.9772514 0.4126919 -0.8346978 -0.3646439 0.4234235 -0.7724347 -0.4733468 0.1964316 -0.09121626 -0.9762654 0.3522748 0.8176714 -0.4553199 0.9559549 0.1470846 -0.254001 0.07162696 0.4459855 -0.8921697 0.1482431 -0.05861687 -0.9872123 0.3527112 -0.7917066 -0.498794 0.1621095 -0.1117938 -0.9804196 0.37366 0.3124141 -0.8733703 0.2799575 -0.8605939 -0.4254432 0.3163732 0.3073283 -0.8974727 0.1431294 -0.1622269 -0.9763178 0.2267329 -0.9207679 -0.3174567 0.9747442 0.06421536 -0.2138931 0.241767 -0.8316797 -0.4998578 0.122117 -0.2528203 -0.9597756 0.105708 -0.08122175 -0.9910747 0.07744008 0.4781602 -0.874852 0.4000462 0.7449305 -0.533893 0.7587867 0.1775099 -0.6266843 0.8336406 0.1165762 -0.5398642 0.5520314 0.7914001 -0.262578 0.3918538 0.7326573 -0.5564746 0.2682831 0.2721548 -0.9240974 0.7417019 0.1256241 -0.6588604 0.6034343 0.7535353 -0.2608674 -0.9304127 0.2580775 -0.2602469 -0.6914188 0.7218249 -0.03014993 -0.818945 -0.2673457 -0.5077948 -0.08903181 0.4037362 -0.9105331 -0.4077463 0.09122157 -0.9085272 -0.757445 0.3520449 -0.5498559 -0.2501755 0.364998 -0.8967658 -0.3494811 0.1349675 -0.9271714 0.1771035 -0.9283748 -0.3267331 0.1722776 -0.9453477 -0.2768362 0.09743636 -0.2595726 -0.9607957 0.0839228 -0.2135177 -0.9733279 0.1039784 -0.9723457 -0.2091228 0.1100086 -0.9567856 -0.2691828 -0.4726823 0.7257829 -0.4998106 0.05636417 -0.2665527 -0.9621708 -0.9254748 0.3107807 -0.2165911 0.03511983 -0.1933339 -0.9805043 0.01059257 -0.9660823 -0.2580173 0.05838853 7.49912e-4 -0.9982937 0.06004065 -0.8273988 -0.5583962 -0.3668215 0.3679774 -0.8544206 -0.2584133 0.5930536 -0.7625681 -0.05329918 -0.9123239 -0.4059858 -0.03208869 -0.780121 -0.6248053 -0.4955679 0.7554364 -0.4286355 -0.01881068 -0.004577398 -0.9998126 -0.1129938 -0.9165154 -0.3837084 -0.2354901 0.5965746 -0.767231 -0.1144911 -0.9122072 -0.3934081 -0.3630222 0.1648673 -0.9170789 -0.03850799 0.01401305 -0.9991601 -0.0380544 -0.02736628 -0.9989009 -0.6755703 -0.001463711 -0.7372942 -0.1789177 -0.931765 -0.3159152 -0.1740596 -0.9137754 -0.3670394 -0.8892167 0.3797577 -0.2551035 -0.7909945 0.4264624 -0.4387 -0.3458957 0.4081286 -0.8448594 -0.7371429 -0.01134014 -0.6756417 -0.1340267 -0.303603 -0.9433251 -0.2602916 -0.8882747 -0.3784394 -0.2732676 -0.9345878 -0.227751 -0.1357572 -0.2270231 -0.9643809 -0.1392735 -0.2358424 -0.9617595 -0.1139132 0.2863772 -0.9513212 -0.2607473 0.6845988 -0.6806874 -0.148962 -0.1264143 -0.9807292 -0.3304947 -0.8636616 -0.3806073 -0.3349658 -0.8773621 -0.3435602 -0.1742887 -0.1793035 -0.9682323 -0.2064308 0.3892223 -0.8977151 -0.8501001 0.4394702 -0.2901655 -0.6860424 0.04685747 -0.7260512 -0.1855723 -0.1141363 -0.9759795 -0.4398456 0.1875971 -0.8782615 -0.3979488 -0.842438 -0.3632284 -0.4009872 -0.8520369 -0.3365152 -0.2479722 0.256421 -0.9342153 -0.1863667 -0.1157305 -0.9756403 -0.398592 0.4529265 -0.7974849 -0.4639222 -0.805635 -0.3684134 -0.4687395 -0.8168805 -0.3361391 -0.26346 0.6861544 -0.6780716 -0.2433078 -0.1720063 -0.9545759 -0.5314803 -0.7716981 -0.3493005 -0.511569 -0.7217218 -0.4662776 -0.2906667 0.7415438 -0.6046699 -0.1985354 -0.01015496 -0.9800412 -0.2235782 -0.04754108 -0.973526 -0.9722111 -0.1094602 -0.20694 -0.5915184 -0.7299296 -0.3425038 -0.5276531 0.1887569 -0.8282229 -0.5915122 -0.7300669 -0.3422218 -0.5822483 0.5211082 -0.6240459 -0.267506 0.7508456 -0.6038804 -0.556196 0.5159345 -0.6515042 -0.2571528 -0.02595472 -0.9660223 -0.6490849 -0.6858257 -0.3291382 -0.4065289 0.2443951 -0.8803439 -0.9750948 -0.03042644 -0.2196919 -0.2262958 0.03338539 -0.9734864 -0.6491378 -0.683983 -0.3328475 -0.9225313 0.04473406 -0.3833209 -0.7964563 0.5648105 -0.2159779 -0.299757 -0.04022133 -0.9531674 -0.7018785 -0.631609 -0.3292973 -0.4703377 0.2408102 -0.8489953 -0.08470648 0.5354276 -0.8403226 -0.7018833 -0.6323453 -0.3278707 -0.7581614 0.5956412 -0.2653358 -0.09242445 0.5219872 -0.8479312 -0.9963458 -0.01153594 -0.08462792 -0.2837311 0.02980238 -0.9584407 -0.745532 -0.5737078 -0.3391774 -0.2958784 0.01256847 -0.955143 -0.08496683 0.5139558 -0.8535983 -0.2625499 0.3048643 -0.9154918 -0.7140474 -0.4579263 -0.5295656 -0.2844805 0.06190872 -0.9566808 -0.3795243 -0.01915371 -0.9249835 -0.1907404 0.8656278 -0.4629326 -0.8022547 -0.5216239 -0.2903378 -0.7557673 -0.4352639 -0.4892454 -0.1187739 0.6617958 -0.7402156 -0.4281592 0.5699895 -0.7012786 -0.2927663 0.5140199 -0.8062703 -0.9389762 0.123713 -0.3209655 -0.3270132 0.08228582 -0.9414305 -0.470126 0.306904 -0.8275213 -0.2125228 0.947661 -0.2382705 -0.8305471 -0.4430629 -0.3374714 -0.2965649 0.1151394 -0.9480465 -0.2929677 0.5109994 -0.8081148 -0.8355404 -0.4702013 -0.2842234 -0.4443883 0.3153581 -0.8384917 -0.9439661 0.1930876 -0.2676663 -0.674222 0.7025296 -0.2277651 -0.4841654 0.3188011 -0.8148312 -0.3779974 0.06449949 -0.9235572 -0.8663651 -0.3787535 -0.325511 0.8416674 -0.5370305 -0.05651873 0.8657753 -0.5001218 -0.01764923 -0.5491143 -0.01763081 -0.8355614 -0.3298218 -0.1170502 -0.9367587 -0.5381498 -0.1125097 -0.8353062 -0.124514 -0.007716894 -0.9921879 -0.4280292 -0.06978583 -0.9010666 -0.5894961 -0.2824882 -0.756766 -0.2504346 -0.00660336 -0.968111 -0.5719123 -0.4115259 -0.7096216 -0.4587349 -0.07365757 -0.885515 -0.7691219 -0.07405436 -0.6347972 -0.4122533 0.01062011 -0.9110074 -0.07788902 0.07491391 -0.9941435 -0.2774001 0.04091483 -0.9598829 -0.07608705 0.06268447 -0.9951289 -0.3303328 -0.2953265 -0.8964723 -0.6319772 -0.1099882 -0.7671424 -0.1615251 -0.03508836 -0.9862446 -0.2156745 -0.03223139 -0.9759332 -0.1231869 -7.72494e-4 -0.9923832 -0.03266257 0.07028073 -0.9969924 -0.5815985 -0.3670781 -0.7259456 -0.1835488 2.10092e-4 -0.9830107 -0.6328465 -0.09392356 -0.7685595 -0.1071131 0.03993344 -0.9934446 -0.4339594 -0.3913141 -0.8115125 -0.5885465 -0.2268629 -0.7759809 -0.4187369 0.01134836 -0.9080368 -0.5921015 -0.06698626 -0.8030746 -0.554811 -0.1886938 -0.8102961 -0.6413719 0.001164376 -0.7672293 -0.2148389 0.03104382 -0.976156 -0.41838 -0.02627372 -0.907892 -0.4159595 -0.02757787 -0.9089649 -0.7523291 -0.109601 -0.6496067 -0.3412095 -0.1466224 -0.9284815 -0.6188116 0.003483116 -0.7855317 -0.411349 -0.107017 -0.9051738 -0.3983093 -0.1030246 -0.9114471 0.09548425 0.09787172 -0.9906079 -0.4135888 0.04103422 -0.9095388 -0.1625642 -0.01885181 -0.9865179 -0.2157094 -0.008427143 -0.9764212 -0.10636 0.0608201 -0.9924659 -0.2153835 0.03080165 -0.9760437 -0.5166077 -0.2653162 -0.8140785 -0.5102208 0.04768621 -0.8587204 -0.4247134 -0.3338262 -0.8415335 -0.4695408 -0.3721505 -0.800647 -0.7034701 0.02692478 -0.7102147 -0.3583369 -0.1513954 -0.9212352 -0.347417 -0.2257564 -0.9101295 -0.2369807 0.0611214 -0.9695898 -0.5724018 -0.1522133 -0.8057216 -0.3375958 0.0640667 -0.9391085 -0.1095477 0.08168298 -0.9906197 -0.2275527 -0.1264411 -0.9655219 0.02036601 0.2785391 -0.960209 0.1897857 0.6878691 -0.7005837 0.07415187 0.5616284 -0.8240602 0.07563346 0.5649697 -0.8216379 0.1040911 0.8319172 -0.5450493 0.2036167 0.6448579 -0.7366808 0.01195859 0.294682 -0.9555206 0.1626338 0.6073888 -0.777579 0.2152382 0.8199173 -0.5304793 0.06867796 0.3908665 -0.9178817 0.3130143 0.701018 -0.6407776 0.02466964 0.2102295 -0.9773408 0.06466114 0.426708 -0.902075 0.0506218 0.7959575 -0.6032322 0.1745395 0.6572374 -0.733195 0.141907 0.5483438 -0.8241247 0.09286952 0.4323557 -0.896908 0.2953789 0.840465 -0.4542797 0.1494828 0.4907172 -0.8584006 0.1345146 0.9079009 -0.3970161 0.04806649 0.3023141 -0.9519957 0.06007236 0.6972882 -0.7142692 0.1904769 0.5936625 -0.7818462 0.1454752 0.6998252 -0.6993439 0.03022563 0.5807172 -0.8135442 0.03213715 0.6109047 -0.7910517 0.08694708 0.4340711 -0.896673 0.3287986 0.849569 -0.412461 0.02896797 0.9272631 -0.3732882 0.1248165 0.618842 -0.7755357 0.1228179 0.6162611 -0.7779063 0.1686464 0.5335741 -0.8287684 0.08927339 0.3823925 -0.9196773 0.1305059 0.4974718 -0.8576071 0.2353928 0.7813262 -0.578031 -0.009408295 0.5213597 -0.8532852 0.1354089 0.4459645 -0.8847486 0.1044972 0.3852927 -0.9168588 0.07921081 0.498716 -0.8631386 0.05271577 0.7137966 -0.6983663 0.1281511 0.8289094 -0.5445058 0.04397529 0.634387 -0.7717639 0.01363694 0.1744812 -0.9845662 0.007304191 0.1211566 -0.9926066 0.06259679 0.2968296 -0.9528766 0.1293678 0.827748 -0.545983 0.01714879 0.8534215 -0.5209394 0.1091433 0.5581516 -0.8225295 0.04708427 0.8935214 -0.4465453 0.04733753 0.3955168 -0.9172381 0.06734246 0.2704857 -0.9603659 0.04182004 0.5568267 -0.8295753 0.03241389 0.4694233 -0.8823782 0.007468819 0.1395086 -0.9901927 0.04211145 0.2115445 -0.9764608 0.3669416 0.832185 -0.4157186 0.1905754 0.8263708 -0.5298985 -5.04468e-4 0.7579478 -0.6523151 0.07839983 0.2587755 -0.9627507 0.1092838 0.5561267 -0.8238812 0.1014729 0.7065234 -0.7003771 0.1029886 0.7078921 -0.6987718 0.06732505 0.8922126 -0.4465692 0.1722142 0.8196109 -0.5464249 0.1058202 0.3105468 -0.9446496 0.02171343 0.1411401 -0.9897515 0.02323335 0.3940673 -0.9187879 0.1035853 0.9252564 -0.3649255 0.3722388 0.8222035 -0.4306039 0.2497713 0.7767311 -0.5781896 0.04488807 0.7968307 -0.602533 0.04003387 0.4408848 -0.8966705 0.04067748 0.4461236 -0.8940465 0.30083 0.6464504 -0.7011442 0.02181577 0.1427911 -0.9895125 0.08690589 0.6235999 -0.7768982 0.08394747 0.61914 -0.7807807 0.002893686 0.1303057 -0.9914698 0.2618312 0.8505337 -0.4561107 0.03115147 0.2940979 -0.9552676 0.03438109 0.2874374 -0.9571822 0.1457101 0.6993044 -0.6998158 0.7232712 0.4685052 -0.5073281 0.4606207 0.3580551 -0.8121732 0.2784916 0.2847792 -0.9172477 0.06955707 0.2081587 -0.9756187 0.7178667 0.4819557 -0.5023805 -0.03974753 0.1728186 -0.9841514 0.5847824 0.4600051 -0.6681504 0.7051953 0.6519074 -0.2787765 0.3606462 0.9098769 -0.2050816 0.1120529 0.9917511 -0.0622403 0.5132472 -0.8582087 -0.007425189 0.9957832 -0.07149165 -0.05748862 0.9921205 -0.1234999 -0.02109056 0.1668804 -0.9857745 -0.01999175 2.42368e-4 -0.9989291 -0.0462678 -0.5115231 -0.7734663 0.3742915 0 -0.0480616 -0.9988445 0 -0.262759 -0.9648615 0 -0.04806166 -0.9988445 -1.60307e-4 -0.4413052 -0.8973571 1.61606e-4 -0.2942279 -0.9557353 -2.18966e-4 -0.6405593 -0.7679088 2.00282e-4 -0.5100369 -0.8601525 -1.5053e-4 -0.7970062 -0.6039712 1.39795e-4 -0.6977535 -0.716338 -8.56644e-5 -0.8989425 -0.4380667 8.84086e-5 -0.8297012 -0.5582078 -1.12324e-4 -0.9635103 -0.2676711 1.46276e-4 -0.9222831 -0.386515 -3.63151e-4 -0.9997164 -0.0238164 2.56558e-4 -0.9882696 -0.1527193 0 0.08715605 -0.9961947 1.53512e-5 0.08695912 -0.9962119 0 0.08712202 -0.9961977 8.59582e-5 0.08711993 -0.9961979 4.48604e-5 0.08718204 -0.9961925 -3.83696e-5 0.08707523 -0.9962018 -2.3833e-5 0.08712172 -0.9961978 1.52635e-5 0.0871576 -0.9961946 -0.2423 0.7428911 -0.6240221 -0.1457881 0.4912207 -0.8587479 -0.2354642 0.8718859 -0.4293854 -0.01912778 0.9250323 -0.3794069 -0.127642 0.9051281 -0.4055253 -0.2259616 0.8377785 -0.4970601 -0.005801081 0.1031213 -0.9946519 -0.02936905 0.1726657 -0.9845426 -0.04280126 0.8783596 -0.4760806 -0.07269388 0.457334 -0.8863189 -0.06476002 0.5140777 -0.8552954 -0.09252041 0.6527537 -0.7518993 -0.1423324 0.4886419 -0.8607966 -0.3559203 0.8204201 -0.4474725 -0.1025941 0.3907195 -0.9147747 -0.03035783 0.8545492 -0.5184827 -0.2012519 0.8189167 -0.537469 -0.04958689 0.3219114 -0.9454703 -0.1480655 0.6266211 -0.7651292 -0.209935 0.6639795 -0.7176758 -0.2100849 0.6629422 -0.7185903 -0.05929064 0.2254787 -0.9724423 -0.303183 0.867986 -0.3932943 -0.0619955 0.3625934 -0.9298831 -0.2994725 0.8138859 -0.4979017 -0.108743 0.8898026 -0.4432004 -0.1483309 0.6014827 -0.7849947 -0.03528702 0.2211849 -0.9745934 -0.13363 0.8250163 -0.5490822 -0.08583325 0.8955566 -0.4365901 -0.004879415 0.8482445 -0.5295824 -0.1903678 0.5997914 -0.7771812 -0.1964372 0.9030132 -0.3820728 -0.05164313 0.3083349 -0.9498751 -0.02469134 0.1908601 -0.9813067 -0.2974709 0.8118669 -0.5023776 -0.06818413 0.2569471 -0.9640172 -0.05893343 0.3900951 -0.9188867 -0.1236201 0.536313 -0.8349171 -0.04593819 0.798898 -0.5997098 -0.1279046 0.4972673 -0.8581175 -0.1138741 0.7934982 -0.597824 -0.0238204 0.7590811 -0.6505602 -0.1011806 0.4586595 -0.882833 -0.04970669 0.6084623 -0.7920246 -0.3203731 0.886282 -0.3344627 -0.1817012 0.590176 -0.7865603 -0.2031213 0.7558476 -0.6224438 -0.07062125 0.5134585 -0.8552036 -0.04581922 0.5685195 -0.8213928 -0.171289 0.8854332 -0.4320513 -0.2462844 0.7448875 -0.6200699 -0.2667887 0.8947622 -0.3580847 -0.03959244 0.6977049 -0.7152904 -0.06364125 0.244866 -0.9674661 -0.09206986 0.3316017 -0.9389161 -0.01770055 0.1799617 -0.9835144 -0.03159242 0.6667397 -0.7446208 -0.1153566 0.7234785 -0.6806407 -0.08036863 0.3090383 -0.9476478 -0.08869034 0.5462474 -0.8329153 -0.004273831 0.1134827 -0.9935308 -0.102795 0.6178531 -0.7795452 -0.02674245 0.4052774 -0.9138026 -0.03466498 0.4510893 -0.8918055 -0.02402693 0.3989281 -0.9166674 -0.02683138 0.2802303 -0.9595578 -0.01957988 0.5311246 -0.8470675 -0.0831241 0.3951073 -0.9148665 -0.01682257 0.2419559 -0.9701415 -0.1597893 0.8346077 -0.5271597 -0.09915906 0.6964737 -0.7106981 -0.01129782 0.2413637 -0.9703691 -0.1680248 0.6751399 -0.7182992 -0.1664767 0.7098684 -0.6843774 -0.003725826 0.1116054 -0.9937457 -0.007364213 0.1105342 -0.9938451 0.0847482 0.1263084 -0.9883643 0.05011337 0.1196036 -0.9915562 -0.08853751 -0.03123337 -0.9955831 0.04164469 0.02294778 -0.998869 -0.0388717 0.04627782 -0.9981721 -0.03735411 0.0613017 -0.9974201 0.1299461 0.6361107 -0.7605769 0.02189308 0.06609612 -0.9975731 -0.07672405 0.04331767 -0.9961109 -0.08874696 -0.03080147 -0.9955779 -0.07377469 0.04561603 -0.9962312 0.04043 0.03335362 -0.9986255 0.2717555 0.2482734 -0.9297899 -2.55788e-4 0.3275043 -0.9448497 1.65902e-4 0.3274356 -0.9448735 -4.02846e-5 0.3275402 -0.9448372 -0.002238929 0.3304476 -0.9438217 4.24341e-4 0.3277588 -0.9447614 -0.001307368 0.329425 -0.9441809 8.31848e-4 0.3277691 -0.9447575 -0.001084685 0.327178 -0.9449622 -0.005056738 0.3297829 -0.9440433 -1.14874e-4 0.3269942 -0.9450265 9.60159e-5 0.3269333 -0.9450474 -0.003104329 0.3380788 -0.9411127 -0.002275705 0.3273208 -0.9449107 -8.90129e-4 0.3278182 -0.9447404 -8.6113e-5 0.327593 -0.944819 -1.22167e-4 0.3269331 -0.9450475 -0.001027286 0.3318793 -0.9433214 -5.79366e-5 0.3275157 -0.9448458 4.03632e-5 0.3276045 -0.944815 -4.268e-4 0.3322041 -0.9432075 2.61616e-4 0.328125 -0.9446343 9.77928e-4 0.3274545 -0.9448665 -5.05818e-4 0.3337859 -0.9426488 -3.40207e-4 0.3275029 -0.9448502 5.32787e-4 0.3275427 -0.9448363 6.37042e-4 0.3275033 -0.9448498 -2.28936e-4 0.32757 -0.944827 7.07599e-5 0.330448 -0.9438243 7.57737e-5 0.3260722 -0.9453449 0.001342415 0.334261 -0.9424796 0.00759226 0.3247726 -0.9457617 8.98769e-4 0.3275479 -0.9448342 0.001434147 0.3343239 -0.9424573 0.009400784 0.3247305 -0.9457599 0.007674932 0.3254476 -0.9455289 2.79035e-4 0.3274984 -0.9448518 0.007171988 0.3151277 -0.9490222 8.06672e-4 0.3271589 -0.944969 0.003355443 0.3361711 -0.9417951 0.007207334 0.3151041 -0.9490298 0.01279962 0.3314594 -0.9433827 2.66451e-4 0.3273826 -0.9448919 0.01098263 0.3187667 -0.9477696 0.002414822 0.3318969 -0.9433127 0.002107918 0.3288121 -0.944393 0.003566384 0.3253204 -0.9455972 0.008334755 0.3199387 -0.9474016 -7.50565e-4 0.3275657 -0.9448281 0.001263201 0.3282241 -0.9445991 -7.89539e-4 0.3270978 -0.9449902 -3.92806e-4 0.3270937 -0.9449919 7.3991e-4 0.3283492 -0.9445562 -0.001016974 0.3268616 -0.9450718 3.37033e-4 0.3278489 -0.9447302 -6.40703e-4 0.3268584 -0.9450731 4.61429e-4 0.3277969 -0.9447481 0.2582809 -0.2295385 0.9384046 0.1983058 0.06425446 -0.9780319 -0.7070172 -0.1353726 -0.6941189 -0.007618606 -0.3348501 0.9422407 -2.8818e-4 0.3238918 -0.9460941 0.001868426 0.3290044 -0.9443265 -0.001208961 0.3264568 -0.9452114 0.00129652 0.3281974 -0.9446083 9.8752e-4 0.3294857 -0.9441601 -0.1027013 0.3190765 -0.9421479 6.47992e-4 0.3275167 -0.9448453 0.07732003 0.3332019 -0.9396799 -0.00126332 0.3339579 -0.9425872 7.13428e-4 0.3278935 -0.9447145 -2.35023e-4 0.3293008 -0.9442251 -2.85274e-4 0.3286638 -0.944447 -0.0633974 0.3673928 -0.9279028 -1.21252e-4 0.3274783 -0.9448587 -0.06361281 0.3610004 -0.9303936 -0.02814179 0.341274 -0.9395426 -5.35885e-4 0.329084 -0.9443005 -0.04059672 0.3411448 -0.9391338 -0.003340244 0.3299397 -0.9439961 -0.00200212 0.3277927 -0.9447476 -0.00300759 0.3257098 -0.9454652 0.005295336 0.324072 -0.9460177 -0.04017204 0.3198261 -0.9466243 -0.002148985 0.3267858 -0.9450961 0.005324482 0.3209914 -0.9470672 0.006795942 0.3245772 -0.9458348 0.006157696 0.3353918 -0.9420587 -3.97429e-4 0.3279961 -0.944679 0.003357052 0.3203466 -0.9472945 9.70879e-4 0.323282 -0.9463022 0.003336131 0.3417088 -0.9398 0.001724302 0.3209822 -0.9470837 0.1142219 0.993387 -0.01165342 -0.1246769 0.9848071 -0.1208741 -2.59647e-4 4.32601e-4 -0.9999999 7.07805e-4 -5.10425e-4 -0.9999997 -4.98778e-4 3.59059e-4 -0.9999998 5.13715e-4 -8.50982e-4 -0.9999995 1.15938e-4 -0.001979231 -0.9999981 -0.004869222 7.6669e-4 -0.9999879 0.001305162 -0.001050591 -0.9999987 -0.05706262 -0.005142807 -0.9983574 -1.86157e-4 -7.51654e-4 -0.9999998 0.01104015 -0.06709688 -0.9976854 0 -0.9998477 -0.01745182 5.1869e-5 0.999848 0.01743984 1.0339e-5 0.9998478 0.01745051 0 -0.9998477 -0.01745182 0.5017362 -0.1470105 -0.8524369 0.123058 0.03081035 -0.9919212 0.3518965 -0.05100512 -0.9346482 0.5947062 -0.1219774 -0.7946358 0.5695852 -0.102243 -0.8155484 0.9404169 -0.3384197 0.03298801 0.7145416 -0.1912183 -0.6729531 0.8911204 -0.1217596 -0.4371259 0.9475125 -0.1830402 -0.2621384 0.3893962 -0.7051708 -0.5925411 0.6266632 -0.6130258 -0.4811367 0.466508 -0.3866028 -0.7955557 0.4770588 -0.3879647 -0.7886053 0.2184038 0.3835151 -0.8973382 0.3166137 0.2734107 -0.9082965 0.6034991 0.6419166 -0.4730032 0.7838079 0.370595 -0.4983017 0.07254159 0.2028179 -0.9765258 0.203916 0.3727246 -0.9052594 0.2985204 0.4619762 -0.8351429 0.3889851 0.5081496 -0.7684235 0.6200343 0.3322944 -0.7107305 0.5229455 0.3249766 -0.7879836 0.5551308 0.593449 -0.5827934 0.1938 0.2051654 -0.9593481 0.7429171 0.3286736 -0.5831362 0.6694664 0.616841 -0.4139107 0.5111169 0.3082557 -0.8023329 0.5442469 0.3031786 -0.7822265 0.08920174 0.1449477 -0.9854103 0.0970515 0.1478344 -0.9842389 0.3976176 0.2803983 -0.8736574 0.5171479 0.5853427 -0.6244454 0.2247879 0.209848 -0.9515431 0.6554666 0.3208613 -0.683675 0.3507038 0.2598887 -0.8997026 0.3427655 0.2526015 -0.9048229 0.02162587 0.1013289 -0.994618 0.4459329 0.2838643 -0.848861 0.1435631 0.277496 -0.9499399 0.6706613 0.5662915 -0.4790902 0.01066368 0.09302639 -0.9956066 0.8048641 0.2654022 -0.5308067 0.2861728 0.411562 -0.8652872 0.460892 0.5177283 -0.7207884 0.5379021 0.2789333 -0.7955235 0.756601 0.2851902 -0.5884058 0.2357707 0.368771 -0.8991218 0.2346026 0.2040885 -0.9504259 0.4234087 0.2532541 -0.8698204 0.3908659 0.5057097 -0.7690784 0.3531002 0.2251009 -0.9081024 0.3407964 0.2246365 -0.9129055 0.05132579 0.1564403 -0.986353 0.6862553 0.2641194 -0.6777127 0.6515558 0.272031 -0.7081485 0.805098 0.2339919 -0.5450369 0.4412066 0.2503525 -0.8617776 0.1422253 0.2788555 -0.9497429 0.2312658 0.1909198 -0.9539738 0.2586343 0.3821938 -0.8871508 0.5689436 0.252201 -0.7827503 0.5664747 0.2527535 -0.784361 0.01750564 0.1115557 -0.993604 0.3669235 0.4539518 -0.8119698 0.3285965 0.4202523 -0.8458206 0.2292075 0.189341 -0.9547847 0.7744445 0.219334 -0.5934042 0.682213 0.558404 -0.4719857 0.4485233 0.2252575 -0.8649197 0.1395019 0.2582414 -0.9559554 0.1038071 0.1352568 -0.9853577 0.6872811 0.2378476 -0.6863478 0.4045794 0.1923422 -0.894047 0.5797644 0.2070723 -0.7880319 0.08425456 0.1198613 -0.989209 0.3410164 0.1994223 -0.9186613 0.1695761 0.287207 -0.9427387 0.07139283 0.1707845 -0.9827185 0.2403638 0.1692543 -0.9558129 0.55686 0.1883575 -0.8089675 0.5868209 0.5532194 -0.591261 0.2503348 0.3403424 -0.9063662 0.4669894 0.4880578 -0.7373741 0.4005378 0.204631 -0.8931381 0.5273384 0.4806236 -0.7006534 0.4561635 0.6930735 -0.5581792 0.2990465 0.1531858 -0.9418627 0.2248063 0.1559267 -0.9618467 0.4108309 0.4451323 -0.7956603 0.0926035 0.1175506 -0.9887399 0.163277 0.2554132 -0.9529453 0.6895388 0.527904 -0.4958364 0.08175176 0.1101005 -0.9905527 0.3230971 0.3979848 -0.8586132 0.3980956 0.4207131 -0.8151813 0.367613 0.5803808 -0.726649 0.257407 0.3385997 -0.9050369 0.5094436 0.7033649 -0.495727 0.5260642 0.4843282 -0.6990585 0.7116425 0.5132658 -0.4797116 0.3599352 0.390458 -0.8473424 0.07903212 0.170314 -0.9822155 0.2544535 0.3264772 -0.910311 0.4604189 0.4209926 -0.781524 0.5582096 0.4685882 -0.6847096 0.446322 0.6234754 -0.6419309 0.1926299 0.2605186 -0.9460571 0.3283555 0.3399115 -0.8812734 0.5505208 0.6765185 -0.4891316 0.4606266 0.4145029 -0.7848635 0.3283498 0.3411686 -0.8807896 0.692862 0.4609488 -0.5544985 0.557843 0.4520749 -0.6960169 0.07270431 0.1542684 -0.9853504 0.5509163 0.6744725 -0.4915061 0.01370674 0.1003224 -0.9948606 0.4907689 0.4005712 -0.7737498 0.1939358 0.2420312 -0.9506892 0.3423214 0.3381249 -0.8766344 0.609798 0.4287261 -0.6665887 0.1975824 0.2478639 -0.9484328 0.3333387 0.5037144 -0.7969675 0.765245 0.398019 -0.5059458 0.01613587 0.1022663 -0.9946263 0.08813744 0.160585 -0.983079 0.4852172 0.3821728 -0.7864531 0.3427181 0.3375032 -0.8767188 0.589443 0.388244 -0.7083951 0.2992801 0.2784506 -0.9126318 0.4057678 0.3397193 -0.8484947 0.7654917 0.3969656 -0.5063999 0.1065997 0.2462024 -0.9633385 0.1298009 0.2708815 -0.9538213 0.769873 0.4037055 -0.4942848 0.511758 0.3785314 -0.7712442 0.4868018 0.6165453 -0.6187859 0.2130775 0.2357435 -0.9481683 0.5992748 0.3766532 -0.7064008 0.4582526 0.6144542 -0.6422232 0.01263302 0.1057755 -0.9943098 0.3333784 0.5043162 -0.7965702 0.2624434 0.4419429 -0.8577938 0.3169767 0.2740076 -0.9079899 0.3775162 0.3061121 -0.8739433 0.6130453 0.6430026 -0.459046 0.6242322 0.3682408 -0.6890086 -0.5730005 -0.7723779 0.2740494 0.2595385 0.9647584 -0.0433706 0.2872725 0.9520729 0.1050326 0.9107517 0.2082051 -0.3566257 0.9434976 0.2239555 -0.2442463 0.8884122 0.2704545 -0.3709153 0.9189099 0.2813547 -0.2764855 0.8864746 0.3438112 -0.3097689 0.8616417 0.4689097 -0.1941584 0.7408937 0.5172867 -0.4283585 0.6283996 0.7472018 -0.216341 0.5665031 0.7686033 -0.2971928 0.5718472 0.782231 -0.2471951 0.5212445 0.8124033 -0.2613524 0.6363182 -0.7035691 -0.3163697 0.6536342 -0.6955128 -0.2983694 0.646573 -0.6009799 -0.4698582 0.6663058 -0.6096242 -0.4294122 0.632722 -0.4688958 -0.616279 0.6678404 -0.3950442 -0.6308165 0.6903752 -0.3643282 -0.6250178 0.8639388 -0.3171834 -0.3911579 0.2654969 0.07485169 -0.9612017 0.8798179 -0.2713498 -0.3902432 0.7905731 -0.3514928 0.5014451 0.7904773 -0.1884009 -0.5827955 0.8762028 -0.1712408 -0.4504945 0.8091777 -0.1527395 -0.5673642 0.8942936 -0.1446031 -0.4234726 -0.6647163 -0.64634 -0.3746957 -0.9361779 0.1081932 -0.3344627 0.736768 0.6710183 -0.08311158 0.6386716 0.6668389 -0.3839591 0.5211758 0.1735764 -0.8356118 0.3944377 -0.05242711 -0.9174259 0.2511044 0.1653795 -0.9537276 0.8670618 0.221329 -0.4463379 0.26535 0.327037 -0.906993 0.2444505 0.329427 -0.9119879 0.6022635 0.6115685 -0.5130915 0.005006551 0.2061512 -0.9785073 0.7491785 0.444984 -0.4906332 0.5877943 0.6117137 -0.5294378 0.03934538 0.9033539 -0.4270876 0.834837 -0.4215958 -0.3539834 0.8337906 -0.419719 -0.3586491 0.02685886 0.8592898 -0.5107834 0.9817327 0.1020471 -0.1605843 0.1917176 0.4296883 -0.8823901 0.5093885 0.1777257 -0.841984 0.9524262 -0.3008816 0.04852497 0.1424018 0.1658532 -0.9758148 0.04482638 0.8300275 -0.5559182 0.06178677 0.7334162 -0.6769661 0.2561521 0.3042339 -0.9175118 0.6811401 -0.6717047 -0.2913091 0.645561 -0.5954971 -0.4781572 0.1207379 0.9152415 -0.3843895 0.2635803 0.3854808 -0.8842681 0.8491702 0.1740933 -0.4985998 0.0951569 0.6543406 -0.7501891 0.1724902 0.9669654 -0.1876839 0.3894847 0.03856152 -0.9202253 0.8264879 0.5542857 -0.09841382 0.8303065 0.5509301 -0.08406764 0.8375234 0.3887933 -0.3839197 0.9222916 0.335296 -0.1922363 0.6950818 -0.3523485 -0.6266672 0.1647044 0.831996 -0.5297689 0.2194772 0.8508741 -0.477329 0.7382365 -0.04883718 -0.6727718 0.8085889 -0.5504787 -0.2077435 0.7257151 -0.04100757 -0.6867722 0.2160564 0.8337737 -0.5080761 0.2591424 0.132617 -0.9566912 0.5116723 0.512035 -0.6899361 0.1633043 0.3073714 -0.9374725 0.8486244 0.3294241 -0.413904 0.5257617 0.3744589 -0.763777 0.2849146 0.8447445 -0.4530236 0.7352305 -0.4742813 -0.4842453 0.2628892 0.796386 -0.5446639 0.9002057 0.1077427 -0.4219255 0.9277449 0.07909196 -0.3647383 0.2940991 0.7419582 -0.602498 0.3212105 0.7636228 -0.5600929 0.7499167 0.5600019 -0.352169 0.2710896 0.6950663 -0.6658778 0.4854614 0.042427 -0.873228 0.2502037 0.414313 -0.8750673 0.8968794 -0.2960541 -0.3285716 0.9797776 0.1941475 -0.04840207 0.7666717 0.08583945 -0.6362752 0.9515726 -0.02485769 -0.3064178 0.6022192 -0.6916322 -0.3987191 0.2811139 0.36026 -0.8894875 0.1396946 0.1437569 -0.9797038 0.3172617 0.6612835 -0.679742 0.7272228 0.5001674 -0.4700846 0.7396472 -0.6130567 -0.2776394 0.1633754 0.3070957 -0.9375504 0.5157533 0.4494707 -0.729366 0.6014406 -0.6687974 -0.4370117 0.5656087 -0.02781939 -0.8242045 0.5475226 0.8291046 -0.1131574 0.4127542 0.7144519 -0.5649713 0.3121553 0.5931365 -0.7421242 0.4706271 0.6807063 -0.5613815 0.739494 -0.1866413 -0.6467717 0.6881363 -0.5419446 -0.4824566 0.2342917 0.1033408 -0.9666582 0.8499732 0.002170026 -0.5268214 0.3012596 0.5414607 -0.7848968 0.8733131 0.270498 -0.4051605 0.8826054 0.4417131 -0.1609259 0.2168952 0.4760684 -0.8522413 0.442101 0.8950586 -0.05845409 0.4306623 -0.8946961 0.1185288 0.5358202 0.8436436 -0.03409218 0.546354 0.7066097 -0.4496666 0.5627748 0.5090372 -0.6512802 0.817087 0.004827141 -0.5764943 0.5300676 0.7327052 -0.4268155 3.3755e-7 0.9998478 -0.01745402 0 0.9998478 -0.01745164 1.82598e-5 -0.9998478 0.01744902 0.3812158 0.887074 -0.260335 0.1842015 0.1737856 -0.9674029 0.5026081 0.5690758 -0.650798 0.3256107 0.1047098 -0.939688 0.4443179 0.1890388 -0.8756975 0.6175024 0.2632871 -0.7411954 0.7659035 0.291795 -0.572929 0.8759236 0.2828083 -0.3908676 0.3640167 0.1951069 -0.9107279 0.2231774 0.01950442 -0.9745827 0.3541964 0.208894 -0.9115417 0.5249316 0.5157198 -0.6771115 0.7034203 0.5060976 -0.4990643 0 0.9998478 -0.01745122 -1.77644e-7 0.9998477 -0.01745158 0.8031365 -0.5732933 0.1621933 -0.2523651 0.142324 -0.957108 -0.1847996 0.09149402 -0.978508 -0.2624677 0.1584857 -0.9518367 -0.379112 0.2145006 -0.9001466 -0.5609719 0.2982882 -0.7722271 -0.9300559 0.3673493 0.007110893 -0.8167712 0.1567131 -0.5552711 -0.8989615 0.20467 -0.3872705 -0.0546568 0.155857 0.9862664 -0.6997274 0.7109837 -0.06988465 -0.2293637 0.9678477 -0.1032631 -0.4433217 0.5225725 -0.7282746 -0.4104697 0.6085424 -0.6791103 -0.016146 0.1229304 -0.9922839 -0.499447 0.5288459 -0.6862032 -0.4461767 0.5194408 -0.7287713 -0.5477271 0.6260873 -0.5549863 -0.5225377 0.6994963 -0.4875031 -0.6546539 0.754704 -0.0430147 -0.07308495 0.9943224 -0.07734143 -0.3911882 0.9187361 -0.05381512 -0.09765762 0.9916025 -0.08477818 -0.08311814 0.9939149 -0.07228237 -0.1088074 0.9919424 -0.06489443 -0.7833828 -0.538168 -0.310945 -0.2816014 0.3623147 -0.8884981 -0.9222267 0.2970565 -0.2474983 -0.1564561 0.8222194 -0.5472446 -0.1949743 0.884042 -0.4247999 0.120298 0.2900516 -0.94942 0.1287465 0.2250692 -0.9657993 -0.3132613 0.4950446 -0.8104308 -0.672361 0.6006212 -0.4326488 -0.7391154 0.5946208 -0.3164406 -0.1249879 0.8265063 -0.5488766 -0.119166 0.8990157 -0.4213909 -0.200791 0.112733 -0.9731261 -0.9220727 0.07600194 -0.3794811 -0.4211444 0.1688567 -0.8911368 -0.4134824 0.02734577 -0.9101014 -0.07724976 0.835882 -0.5434463 -0.1293897 0.4175111 -0.8994125 -0.9496222 -0.243821 -0.1968984 -0.04368209 0.9163605 -0.3979639 -0.04545038 0.8855583 -0.4622995 -0.7489157 -0.3467065 -0.5647299 -0.7518651 0.2162194 -0.6228549 -0.8178382 0.01332432 -0.5752939 -0.5791366 0.4284914 -0.6935388 -0.6446769 0.6550658 -0.3940566 -0.8075388 0.5773876 0.120435 -0.8458414 -0.4540986 -0.279905 -0.8184732 -0.3532625 -0.4531085 -0.3031302 0.1286314 -0.9442278 -0.4337641 0.424827 -0.7945886 -0.9268276 0.3475801 -0.1420521 -0.3586892 -0.1926375 -0.9133635 -0.7748501 0.2568046 -0.5776321 -0.5615434 -0.7957385 0.2268687 -0.4331317 0.2258623 -0.8725728 -0.307819 0.3342283 -0.890808 -0.5996088 0.68154 -0.4194909 -0.1966231 0.5000416 -0.8433848 -0.7523718 0.455349 -0.4760192 -0.4643607 0.06054532 -0.8835744 -0.9165431 0.2116423 -0.339347 -0.2858811 0.2734419 -0.9184235 -0.3226237 0.4342562 -0.8410324 -0.4555668 0.6573384 -0.6003042 -0.9006765 -0.005949616 -0.4344497 -0.7072277 0.7069703 -0.004708409 -0.9744882 -0.1900601 0.1193732 -0.8509305 -0.2970218 -0.4332385 -0.2658228 0.1996415 -0.9431233 -0.2798154 0.5906627 -0.7568494 -0.2502498 0.06492084 -0.9660022 -0.9036266 -0.391884 -0.1728754 -0.5016359 -0.2655934 -0.8232993 -0.414182 0.467116 -0.7811887 -0.7634738 0.3111682 -0.5659348 -0.4066535 0.7328209 -0.5455332 -0.6966096 -0.5129395 -0.5016257 -0.1689964 0.3187873 -0.9326387 -0.8627276 0.4672799 -0.1932631 -0.4482609 0.7574769 -0.4746482 -0.6877685 -0.4794701 -0.5450533 -0.3389036 0.7473797 -0.5714613 -0.3698906 0.7968752 -0.4776725 -0.4315897 0.3516947 -0.8306874 -0.3361778 0.04433059 -0.9407547 -0.8597995 -0.2008226 -0.469484 -0.2204114 0.7074396 -0.6715268 -0.3918949 0.8635285 -0.3173908 -0.8464676 0.1152222 -0.5198237 -0.2468865 0.776523 -0.5797061 -0.2882291 0.08458381 -0.9538185 -0.4244527 0.4668282 -0.7758295 -0.3082155 0.2141168 -0.9269073 -0.9504815 -0.3076562 -0.04396235 -0.2551438 0.8533567 -0.4546254 -0.2098294 0.8025348 -0.5584889 -0.760648 -0.4903094 -0.4254545 0.7891517 -0.5930246 -0.1598801 0.01520186 -0.02616739 -0.999542 0.5245701 -0.4627655 -0.7146149 -0.4688931 -0.8678776 -0.1640972 -0.5892851 -0.6969408 -0.4086768 -0.9457575 -0.1094976 -0.3058646 -0.8307406 0.06753003 -0.5525485 -0.8729793 -0.08705478 -0.4799258 -0.820452 -0.05370891 -0.5691872 -0.9451602 -0.1797471 -0.2726963 -0.8922997 -0.2272482 -0.3900766 -0.8826167 -0.2099995 -0.4205806 -0.8352587 -0.2563436 -0.4864473 -0.8350833 -0.2562962 -0.4867733 -0.8000917 -0.2878596 -0.5262985 -0.816034 -0.3205156 -0.4809972 -0.7924661 -0.3463833 -0.5020122 -0.8375464 -0.442439 -0.3205681 -0.8030402 -0.4549717 -0.3848732 -0.7215147 -0.650541 -0.2370929 -0.6895153 -0.6746141 -0.2635613 -0.5120285 0.7581837 -0.4037132 -0.6223218 0.7662488 -0.1599322 -0.6049873 0.7243641 -0.3305861 -0.6754981 0.7197157 -0.1603485 -0.6390195 0.6769319 -0.3652632 -0.7220227 0.6722327 -0.1636656 -0.6861301 0.6281207 -0.3670012 -0.7628282 0.6219131 -0.176967 -0.7284461 0.5681223 -0.3828883 -0.8109652 0.5659724 -0.1483606 -0.8507318 0.4975124 -0.1695196 -0.8429409 0.4902497 -0.2215982 -0.8827106 0.4497664 -0.1361335 -0.8347043 0.4229091 -0.3527276 -0.9036468 0.3840594 -0.1895281 -0.8683342 0.363447 -0.3374941 -0.9078128 0.3260275 -0.2637845 -0.8585324 0.2873637 -0.4246696 0.3443836 0.8978417 -0.2743726 0.294578 0.9309538 -0.2157521 0.2768064 0.912923 -0.2999165 0.226107 0.932891 -0.2803395 0.2026481 0.9090431 -0.3641078 0.1604357 0.9298371 -0.3311548 -0.006224989 0.02848333 -0.9995749 -0.01874577 0.9977968 -0.06364113 -0.3923273 0.6234973 -0.6762621 -0.4448274 0.3855234 -0.8083937 -0.2959817 0.4536669 -0.8405839 -0.4563705 0.4071292 -0.7911838 -0.454161 0.4053899 -0.7933455 -0.1194596 0.1601791 -0.9798327 -0.5954003 0.5155484 -0.6162047 -0.152102 0.2671396 -0.9515784 -0.2011835 0.2263814 -0.9530355 -0.5929883 0.5061129 -0.6262704 -0.3235121 0.3111827 -0.8935913 -0.3411421 0.5418069 -0.7681585 -0.296658 0.4531115 -0.8406451 -0.08241885 0.1863266 -0.9790248 -0.08703821 0.1948442 -0.9769648 -0.2350253 0.3777508 -0.8955822 -0.1553725 0.2828786 -0.9464879 -0.9211505 0.1899295 -0.3397186 -0.1993162 0.4068726 -0.8914751 -0.02913057 0.1071401 -0.9938172 -0.09758359 0.1529931 -0.9833975 -0.2255846 0.4083756 -0.8845005 -0.02716958 0.1212997 -0.9922441 -0.6174349 0.5947191 -0.5148625 -0.7896813 0.2325115 -0.5677518 -0.6195152 0.6115664 -0.4921255 -0.585865 0.6295997 -0.5102612 -0.2480959 0.2265518 -0.9418719 -0.1406297 0.2952235 -0.9450219 -0.4765245 0.4492315 -0.7557218 -0.1648988 0.355255 -0.92011 -0.7882534 0.2339116 -0.569159 -0.1038162 0.2509156 -0.962426 -0.074427 0.2057887 -0.9757621 -0.6034536 0.1861348 -0.7753694 -0.3921374 0.3896018 -0.83333 -0.1481941 0.2018787 -0.9681341 -0.5676734 0.5476139 -0.614708 -0.5071157 0.2081975 -0.8363538 -0.3101153 0.3313943 -0.8910703 -0.4295879 0.1840834 -0.8840631 -0.1141895 0.1769715 -0.9775695 -0.6327846 0.2410618 -0.7358484 -0.5134539 0.5386819 -0.6679723 -0.2791438 0.3288158 -0.9021967 -0.8076159 0.2805682 -0.5186889 -0.5923873 0.6495569 -0.4766059 -0.01778817 0.1133133 -0.9934002 -0.4051033 0.190039 -0.8943023 -0.454524 0.4762945 -0.752696 -0.6127206 0.2529621 -0.7487214 -0.5601369 0.6869113 -0.4630332 -0.2158435 0.2766098 -0.9364287 -0.720606 0.3186775 -0.6157693 -0.215687 0.1345267 -0.9671514 -0.4354807 0.4714635 -0.7668631 -0.1857138 0.2565749 -0.9485145 -0.5263488 0.2336683 -0.8175306 -0.3725246 0.4194523 -0.8278195 -0.2075791 0.2796624 -0.93739 -0.4842894 0.2360813 -0.8424544 -0.6136723 0.2562369 -0.7468259 -0.5000737 0.6019633 -0.6225485 -0.3669555 0.4171795 -0.8314475 -0.3473644 0.1845832 -0.9193841 -0.4478886 0.2269885 -0.8647959 -0.02409482 0.110576 -0.9935756 -0.488982 0.5632764 -0.6660454 -0.2505437 0.1586837 -0.9550118 -0.2868503 0.3503809 -0.8915998 -0.7525457 0.3727032 -0.542925 -0.5416406 0.2856415 -0.7905911 -0.7149989 0.339127 -0.6113668 -0.02776902 0.1130499 -0.9932012 -0.1203432 0.1261715 -0.9846819 -0.4073989 0.5043514 -0.7613513 -0.04036915 0.0996707 -0.9942013 -0.5134148 0.2801342 -0.8111291 -0.3365321 0.2289737 -0.9134097 -0.494993 0.6838869 -0.5359857 -0.4054595 0.5039107 -0.7626774 -0.734995 0.4113985 -0.5390117 -0.7474489 0.4196017 -0.5150287 -0.3578409 0.2240602 -0.9065026 -0.3423931 0.4435754 -0.828256 -0.6161054 0.3574716 -0.7018749 -0.2331234 0.167684 -0.9578808 -0.4872846 0.6134744 -0.6214522 -0.7567287 0.4781941 -0.4457488 -0.5730768 0.3379864 -0.7465576 -0.4605421 0.2901974 -0.8388602 -0.6665305 0.4142847 -0.6197623 -0.1341519 0.1364748 -0.9815182 -0.5102325 0.3302242 -0.7941128 -0.3401552 0.4424604 -0.8297731 -0.2578897 0.1890358 -0.9475012 -0.02565282 0.0965988 -0.9949929 -0.4698903 0.3091002 -0.8268374 -0.3735262 0.2467244 -0.8942065 -0.620457 0.3816919 -0.6850873 -0.52579 0.7659236 -0.3700082 -0.4919172 0.6865593 -0.5354006 -0.4948839 0.6864765 -0.5327664 -0.06502753 0.1581587 -0.9852702 -0.100643 0.1793692 -0.9786204 -0.2220107 0.1929857 -0.9557552 -0.5393423 0.3837844 -0.7495462 -0.3180444 0.2557824 -0.9129201 -0.4260247 0.3131052 -0.8488039 -0.6918532 0.4951186 -0.5255443 -0.111658 0.1406165 -0.9837477 -0.5247524 0.374244 -0.7645761 -0.2305364 0.205671 -0.9510796 -0.2975358 0.4642708 -0.8342212 -0.04447329 0.1092537 -0.9930185 -0.1147173 0.1428945 -0.9830673 -0.3230672 0.2638277 -0.9088579 -0.2674809 0.3716377 -0.8890104 -0.2643052 0.3710033 -0.8902244 -0.4859569 0.7807916 -0.3926962 -0.6269938 0.4788302 -0.6144921 -0.620968 0.4660089 -0.6302654 -0.4267634 0.3262681 -0.8434585 -0.4595606 0.3609136 -0.8115082 -0.3729556 0.5718637 -0.7306682 -0.5441662 0.4169202 -0.7280528 -0.2508062 0.3838046 -0.8887014 -0.6892901 0.5531839 -0.467832 -0.37093 0.5360966 -0.758295 -0.6690783 0.5554556 -0.4937646 -0.1703076 0.2738554 -0.9465721 -0.3188323 0.2993056 -0.8993121 -0.3023661 0.2806833 -0.910929 -0.3578214 0.7290077 -0.5835338 -0.02780258 0.1059287 -0.993985 -0.1852821 0.2021871 -0.9616606 -0.3912528 0.5587738 -0.7312272 1.53589e-7 -0.9998478 -0.01744925 -1.6636e-7 -0.9998478 -0.01745182 -0.5619978 -0.3818753 -0.7337096 -0.4522971 0.289925 -0.8434281 -0.4984306 -0.3011979 -0.8129249 -0.5474201 -0.5384488 -0.640628 -0.1460462 -0.9470745 0.2858681 -0.1073868 0.03794193 -0.9934931 -0.418497 0.08986455 -0.9037614 -0.5219167 -0.00272572 -0.8529922 -0.6557267 -0.1060846 -0.7475082 -0.781141 -0.187116 -0.5956564 -0.6317676 -0.1649535 -0.7574036 -0.9167981 -0.1232028 -0.3798718 -0.961157 -0.1779533 -0.2109736 -0.9490717 -0.3134706 0.03161275 -0.2515072 -0.217561 -0.9430861 -0.671274 -0.3414356 -0.6578853 -0.8174173 0.5755138 -0.0247572 -0.6403673 0.7680452 0.006028294 0.6850007 -0.7177766 0.1247832 -0.2869374 0.9577178 -0.02106344 -0.07157647 0.9970965 -0.02598667 0.03569495 0.9989524 -0.02863657 -7.46564e-4 -0.003353893 -0.9999941 -0.001495778 9.9397e-4 0.9999984 0.004331052 -0.005145013 0.9999774 0.004068613 4.81948e-4 0.9999917 0.7902171 -0.5645859 0.238327 -0.9565519 0.281379 -0.07638388 -0.2232619 -0.07964533 -0.9714993 -0.4902552 -0.8686091 -0.07188892 -0.265034 -0.2592238 -0.9287411 -0.1937054 -0.09736824 -0.976216 -0.3564167 -0.3022311 -0.8840947 0.9868208 -0.1595912 0.02674961 -0.003101348 -0.3302986 0.9438715 -0.9202206 0.3841735 0.07486665 0.1926235 0.981249 -0.006817877 0.4685923 0.078547 0.8799158 0.002567172 -0.3463735 0.9380932 0.1726294 -0.2718276 0.9467359 0.2208034 -0.2705603 0.9370395 -0.2974147 0.2724761 0.9150418 -0.1847446 0.06610661 0.9805607 -0.8515681 0.4336494 0.2945846 -0.259874 0.2189808 0.9404855 0.3596149 -0.0030272 -0.9330959 0.791056 -0.5068454 -0.3425468 0.2300149 0.1505931 -0.9614651 0.7550557 -0.3773007 -0.5362232 0.3856466 0.4687973 -0.7946735 0.6994792 0.5235658 -0.4864234 0.8051339 0.5305054 -0.2651858 0.1422383 0.4026532 -0.9042337 0.3126352 0.0948956 -0.9451212 0.8316931 -0.4688743 -0.2973946 0.7496742 0.5820182 -0.3150294 0.7718403 0.5817308 -0.2566164 0.2797179 0.1500039 -0.9482916 0.1400864 0.4045763 -0.9037113 -0.8289256 0.3546847 0.4325289 0.8552829 -0.3883777 -0.3430074 0.1537984 0.4092924 -0.8993475 0.7141746 0.6321224 -0.300626 0.2098669 0.2045861 -0.956086 0.8778932 -0.3219115 -0.3545088 0.2973114 0.5043369 -0.8107098 0.8832508 -0.3293852 -0.3337267 0.3599899 0.1434528 -0.9218615 0.7116652 0.6776655 -0.1852079 0.05635106 -0.9453812 -0.3210594 0.9145697 -0.2783476 -0.2934021 0.02500629 -0.5507167 -0.8343177 0.8930869 -0.2522292 -0.372527 0.04466557 -0.9035478 -0.426153 0.3348138 0.1673132 -0.9273112 0.2528868 0.5045589 -0.8255112 0.6583839 0.7234331 -0.207787 0.1581618 0.2681733 -0.950299 0.1108843 -0.9160373 -0.3854612 0.9299597 -0.2249739 -0.2907955 0.926624 -0.2202378 -0.3047352 0.3926611 0.1933151 -0.8991366 0.1555468 0.4531884 -0.8777389 0.2225681 0.5147204 -0.8279651 0.1299937 -0.8195867 -0.5580137 0.05096423 -0.1080534 -0.992838 0.1781492 -0.8919613 -0.4155332 0.96701 -0.1935744 -0.1655915 0.9317972 -0.143929 -0.3332244 0.03790557 0.07371979 -0.9965584 0.5526835 0.766462 -0.3272263 0.4030592 0.197696 -0.8935657 0.2154121 -0.7792776 -0.5884932 0.253513 0.563053 -0.7865765 0.9563733 -0.09650218 -0.2757492 0.08749717 -0.04916882 -0.9949506 0.3281797 0.2364182 -0.9145516 0.3888359 0.2276385 -0.8927416 0.6134099 0.7877588 0.05625319 0.07138925 0.09354728 -0.9930521 0.1111921 0.4565488 -0.8827229 0.4595199 0.8094097 -0.3656467 0.3193399 -0.6778565 -0.662218 0.2068698 0.2986264 -0.9316798 0.2827398 0.2896156 -0.9144293 0.1727558 -0.1414973 -0.9747483 0.2000151 0.5749402 -0.7933712 0.2835361 0.2947363 -0.9125447 -0.4564432 -0.8743343 0.1649219 0.1430871 0.003802657 -0.9897029 0.2131226 -0.1163278 -0.9700756 0.1753827 0.5853921 -0.7915536 0.1309799 0.5349047 -0.8346984 0.3906933 0.9031023 -0.1782276 0.2286931 -0.09985911 -0.9683634 0.4954437 -0.7737452 -0.3947833 0.3401716 0.3092101 -0.8880723 0.1591941 0.6118158 -0.7748153 0.1389553 0.1194511 -0.9830682 -0.5587707 0.7562214 0.3404478 0.55379 -0.743488 -0.3748897 0.1728679 0.3376437 -0.925264 0.1425105 0.1161803 -0.9829511 0.8024569 0.284163 -0.5247042 0.906069 0.2525938 -0.3394634 0.2798945 0.3468496 -0.895184 0.07126003 0.5076577 -0.8586068 0.1147453 0.5800685 -0.8064454 0.01522833 0.3706255 -0.9286575 0.2425924 0.9330188 -0.2657536 0.1402015 0.1317368 -0.9813201 0.6076437 -0.6935847 -0.3869229 0.6117532 -0.7033645 -0.3619895 0.2366805 0.02339303 -0.9713059 0.9143791 0.2982378 -0.2737978 0.9389004 0.2830153 -0.1958791 0.3232399 0.3784818 -0.8673336 0.1915255 0.92154 -0.3377602 -0.6681212 0.6728265 0.3176767 0.6571534 -0.6480081 -0.3850129 0.265202 0.01664531 -0.9640492 0.9078224 0.3565083 -0.2208175 0.8825922 0.367453 -0.2932738 0.2492476 0.3793857 -0.8910344 0.2509015 0.3798378 -0.8903773 0.0833112 0.6762648 -0.7319325 0.01328271 0.3726143 -0.9278913 0.7144308 -0.6206403 -0.3231012 0.2078141 0.1112566 -0.9718207 0.8620837 0.4265741 -0.2735803 0.3185828 0.01058208 -0.947836 0.3250648 0.4280723 -0.8432598 0.7427908 -0.5144395 -0.4285019 0.2189379 0.3947084 -0.8923406 -0.3325538 0.1643164 -0.9286594 -0.03377801 -0.9409429 -0.3368761 -0.1884884 0.4653943 -0.8648008 -0.1032624 0.4012591 -0.9101253 -0.7506487 0.6399569 -0.1642613 -0.02315992 -0.5681306 -0.8226125 -0.01184344 -0.4620631 -0.886768 -0.8741572 -0.3888072 -0.2909951 -0.008974254 0.1938828 -0.9809837 -0.2376099 0.2025939 -0.9499986 -0.8797224 -0.3993495 -0.2580867 -0.7375122 0.6029589 -0.304165 -0.08278834 0.3783915 -0.9219361 -0.3356198 0.1261303 -0.9335152 -0.6455501 0.5859217 -0.489858 -0.8389219 -0.4775286 -0.2611064 -0.3369528 0.1206761 -0.9337559 -0.2277687 0.4433091 -0.8669478 -0.8285557 0.5371584 -0.1579755 -0.1299521 0.2357572 -0.9630842 -0.2634946 0.1121199 -0.958123 -0.8073444 0.5086227 -0.2991626 -0.7050807 0.5040169 -0.4988269 -0.01536417 0.7332476 -0.6797882 -0.3340752 0.02257883 -0.942276 -0.1224677 0.3731643 -0.9196467 -0.2131138 0.4100678 -0.8868072 -0.8778045 0.4356713 -0.1991229 -0.01930409 0.5232606 -0.8519541 -0.3108178 0.03522944 -0.9498164 -0.7135244 -0.605499 -0.3524969 -0.03809386 0.5939466 -0.8036022 -0.3574416 -0.03173971 -0.9333961 -0.8654078 0.4047013 -0.2954427 -0.02409923 0.3027769 -0.9527568 -0.1620259 0.9582072 -0.2357681 -0.6692721 -0.6592177 -0.3427929 -0.6670021 -0.6184871 -0.41543 -0.03209048 0.5847868 -0.8105521 -0.3209426 -0.04603379 -0.9459794 -0.1489611 0.3196012 -0.9357701 -0.8850832 0.3392841 -0.3186128 -0.3725354 0.4122962 -0.8314021 -0.1214325 0.4068785 -0.9053751 -0.2498569 0.03042101 -0.9678049 -0.05757218 0.2607778 -0.9636808 -0.6110498 -0.6744937 -0.4143387 -0.6123828 -0.694507 -0.3776869 -0.2025539 0.9397494 -0.2753961 -0.1968858 0.9329602 -0.3013657 -0.4105293 -0.2531019 -0.8760167 -0.9125499 0.2794744 -0.2985749 -0.04870188 0.2692083 -0.9618499 -0.5577882 -0.7659856 -0.3195909 -0.03838449 0.4769325 -0.8781014 -0.02466982 0.4066475 -0.9132521 -0.2797811 0.9501355 -0.1377134 -0.3098022 -0.1613225 -0.9370152 -0.2575514 -0.08362251 -0.9626395 -0.149325 0.3421403 -0.9277079 -0.7750988 0.3096772 -0.5507468 -0.931304 0.2147096 -0.2942323 -0.2635428 0.3444859 -0.9010409 -0.4967694 -0.7856427 -0.3687628 -0.2259048 0.783076 -0.5794472 -0.2272112 -0.06971096 -0.9713472 -0.2151595 -0.03607177 -0.9759125 -0.2313317 0.7945197 -0.5614483 -0.9455218 0.1539602 -0.2868533 -0.2373543 0.7642672 -0.599632 -0.4344153 -0.8203669 -0.3718623 -0.437595 -0.7905626 -0.4283943 -0.3597224 0.8775548 -0.3170133 -0.6336988 0.2683491 -0.7255444 -0.4671573 0.3005822 -0.8315135 -0.9430878 0.09852892 -0.3176123 -0.1793009 -0.04408794 -0.9828059 -0.1417343 0.02006065 -0.9897016 -0.3665567 -0.8489132 -0.380766 -0.06034272 0.4312338 -0.9002202 -0.3707463 -0.8187487 -0.4384037 -0.4606129 0.8792237 -0.1216618 -0.2694416 0.3087705 -0.9121743 -0.1527819 -0.05430465 -0.9867668 -0.9467953 0.02839237 -0.3205815 -0.8676494 0.1084434 -0.4852058 -0.1101276 0.01290369 -0.9938338 -0.3941837 0.7972379 -0.4571993 -0.2967433 -0.8938689 -0.3360683 -0.3031646 -0.8426449 -0.4450177 -0.2183602 0.5988248 -0.7705373 -0.1402658 -0.1624849 -0.9766904 -0.4124686 0.2716597 -0.8695233 -0.9531919 -0.03687977 -0.3001086 -0.3016729 0.2599215 -0.9172973 -0.225004 -0.9096788 -0.3490813 -0.2201375 -0.9287306 -0.2983269 -0.1605967 0.5054943 -0.8477525 -0.1079844 -0.1258367 -0.9861565 -0.5309894 0.1691437 -0.8303257 -0.4128323 0.7160781 -0.5628515 -0.9459443 -0.1241045 -0.2996459 -0.3544251 0.2722188 -0.8945837 -0.1874947 0.5260748 -0.8295125 -0.05863463 0.02802759 -0.9978861 -0.03402996 0.1484597 -0.9883329 -0.1515108 -0.9196033 -0.3624562 -0.1489086 -0.9357053 -0.3198153 -0.09513413 -0.4327074 -0.896501 -0.938046 -0.1885904 -0.2906949 -0.9344727 -0.1802308 -0.3070466 -0.2792251 0.235647 -0.9308619 -0.1870853 0.5249866 -0.830294 -0.2835186 0.5716072 -0.7699887 -0.05246758 -0.9986075 -0.005499243 -0.9201367 -0.2574196 -0.2950994 -0.9082952 -0.2274947 -0.3510642 -0.30864 0.201627 -0.9295634 -0.2238898 0.2497535 -0.9420704 -0.5906586 0.6614567 -0.4621663 -0.8810585 -0.2761173 -0.3840514 -0.8976108 -0.3217768 -0.3012551 0.5097696 0.3386582 0.7908512 0.5159089 0.345973 0.7836713 0.1657549 0.9861072 -0.01086753 -0.3400568 0.3162431 0.8856364 -0.14471 0.4778042 -0.8664653 -0.3430917 0.3247672 0.8813709 0.1526084 -0.2272526 0.961804 0.1021228 0.3092563 -0.9454795 -0.5337228 -0.3538472 0.7680704 0.3684458 -0.50241 0.7821969 -0.07283973 0.3370198 -0.9386758 -0.5850292 -0.3523748 0.7304608 -0.9980772 -0.0616272 0.006642699 -0.8698608 0.287188 0.4010801 -0.8592891 -0.5113166 -0.01333451 0.5334206 -0.840733 -0.09290134 0.4612216 -0.7749494 -0.4321205 0.06489622 0.190419 -0.9795556 0.5769898 -0.7693797 -0.2741123 0.2523316 -0.03403598 -0.9670422 0.5203964 -0.7178884 -0.4624111 0.2300471 -0.04103016 -0.9723142 0.2939957 0.9557992 0.00381112 0.2577643 0.966153 -0.01030415 0.8267445 0.5623162 -0.01715213 0.8131163 0.5815037 -0.02637165 0.6023741 0.7982102 -0.002497792 0.6540349 0.7560707 -0.0244016 0.6516538 0.7580403 -0.02688074 0.876384 0.4663883 -0.1201382 0.337657 0.9410957 -0.01807785 0.576223 0.8158384 -0.0487312 0.5877445 0.8090421 -0.002713739 0.03767818 -0.6370381 -0.7699109 0.5843984 -0.7151607 -0.3834369 -0.7805463 0.5747115 -0.2458748 -0.5218555 -0.7135403 -0.467469 0.2241024 -0.9687393 -0.1064065 0.9625235 -0.250326 -0.1043344 0.9730398 -0.2055208 -0.1046658 0.9698814 -0.2055501 -0.1306878 0.5714635 -0.818388 -0.06058675 0.4845522 -0.8654202 -0.1275034 0.821465 -0.5614836 -0.09965747 0.6922438 -0.7112767 -0.122 0.9244467 -0.3669266 -0.1037468 0.8597766 -0.4958276 -0.1222267 0.9680044 -0.2189523 -0.1225864 0.1369631 -0.948414 -0.2859233 0.7604945 -0.4084961 -0.5047564 0.9449386 0.1768315 -0.2753576 0.8194966 0.4924058 0.2931925 0.9821699 0.1716786 -0.07660841 0.9608407 -0.2658002 -0.07832884 0.4348861 0.8970728 -0.07832336 0.9894965 -0.1209402 -0.07918542 0.9556353 0.2838816 -0.07856613 0.6590829 0.74797 -0.07842677 0.7336716 0.6749598 -0.07845592 0.4335849 0.8976905 -0.07845842 0.984712 -0.1559857 -0.07752972 0.8782785 0.4648032 -0.1121827 0.9460147 0.3144891 -0.07844132 0.9560514 0.2825086 -0.07845127 0.5096226 0.8566303 -0.08043432 0.08688127 0.9931386 -0.07827848 0.1725679 0.9707064 -0.1671813 0.970287 -0.2288902 -0.07843685 0.9783715 -0.1913174 -0.07865774 0.9745855 -0.1962764 -0.1079756 0.9392744 -0.3340846 -0.07842981 0.5379798 0.8320814 -0.1349763 0.00335294 -0.9966268 0.08199959 0.04566466 0.9959332 -0.0776652 0.9232217 -0.3760006 -0.07928115 0.5896857 0.8000909 0.1101148 0.8595508 0.4938802 0.131357 0.9651668 0.2495956 -0.07845437 0.1336703 0.9879745 -0.07771015 0.4874425 0.869657 -0.07808077 0.7555755 0.6503499 -0.07842707 0.9609154 -0.2655059 -0.07841086 0.8288961 -0.5242917 0.1950631 0.9515743 -0.2969501 -0.07954305 0.1889433 0.97885 -0.0784415 0.2207552 0.9721688 -0.07845455 0.5127675 0.8549836 -0.07792681 0.8920913 0.4031885 -0.2039903 0.9582614 -0.2678637 -0.09992128 0.9647328 -0.01907992 0.262539 0.99137 0.03316605 0.126829 0.9139136 -0.3982591 -0.07843309 0.9142065 -0.3975319 -0.07870727 0.9651388 0.24971 -0.07843595 0.8979236 -0.4330999 -0.07847148 0.9392308 -0.3342182 -0.07838231 0.5967525 0.7943079 -0.1138478 0.9347176 0.3466227 -0.07845985 0.5163347 0.8527923 -0.07838451 0.8805649 -0.4674363 -0.07815957 0.4822847 0.8724645 -0.07878607 0.2426933 0.9670191 -0.07729285 0.8281788 -0.5587297 0.04405701 0.7795106 0.6214577 -0.07844501 0.8616467 -0.5013837 -0.07861071 0.9729568 0.2172574 -0.07844859 0.9370419 -0.3355494 -0.0967428 0.2383739 0.9605231 -0.1434342 0.9330405 0.3327197 -0.1368688 0.9459996 0.314533 -0.07844752 0.860471 -0.5033611 -0.07885116 0.7996336 0.5953444 -0.07842862 0.8959009 -0.4371925 -0.07889419 0.7076838 0.702164 -0.07841855 0.7598631 -0.2147741 0.6135798 0.6285309 0.7772447 0.02897995 0.9106228 -0.4001806 -0.103062 0.984501 0.1566469 -0.07886397 0.4950331 0.8653421 -0.07826393 0.733655 0.6749776 -0.07845711 0.3074247 0.9484205 -0.07738661 0.9879159 0.1336693 -0.0784527 0.842895 -0.5322982 -0.07865679 0.5517547 0.830309 -0.0784465 0.8813605 -0.4658243 -0.07881331 0.8768244 -0.4691005 -0.1054688 0.9528927 0.2689763 -0.1401692 0.8929982 0.443913 -0.0741328 0.7555804 0.6503443 -0.07842779 0.7794436 0.6215408 -0.07845163 0.3758893 0.9233395 -0.07843148 0.6525446 0.7509407 -0.1013599 0.9756206 0.2045598 -0.07949769 0.8025596 -0.5913877 -0.07847923 0.463492 0.8825506 -0.07924485 0.1566807 0.984503 -0.07877236 0.8401414 -0.5363246 -0.08073687 0.3682847 0.9264702 -0.07758462 0.3987724 0.8234183 0.4036867 0.9914572 0.1037954 -0.07898831 0.8918882 0.4460557 -0.07463139 0.2960894 0.9335092 -0.2022167 0.4059371 0.9105283 -0.07844322 0.1896275 0.9787234 -0.0783708 0.08913069 0.9929256 -0.07845145 0.06759905 0.9946219 -0.07847076 0.04771733 0.995824 -0.07783102 0.7592754 -0.646032 -0.07838112 0.7089276 0.6980497 -0.1007387 0.9075409 0.4125679 -0.07846897 0.9700748 -0.1801843 0.1627529 0.8032199 -0.590424 -0.0789774 0.8375806 0.5406478 -0.07847791 0.7111913 -0.6988332 -0.0764144 0.8193426 0.567911 -0.0784533 0.8233195 0.1056816 0.5576525 0.994312 0.0720095 -0.07847595 0.7996811 0.5952796 -0.0784384 0.55163 0.8303939 -0.0784263 0.8575574 0.5083693 -0.0784614 0.759171 -0.6461445 -0.0784651 0.7151639 -0.6944214 -0.07949602 0.578317 0.8120309 -0.07845532 0.8193267 0.5679337 -0.07845491 0.1249677 0.9890546 -0.07844734 0.9912182 0.1064725 -0.07842278 0.9970144 -0.001531422 -0.07720196 0.9940212 -0.07633036 -0.07807391 0.7140196 -0.6954915 -0.08042192 0.6551567 -0.7520607 -0.07193458 0.9224662 0.3780211 -0.07846266 0.9943417 0.0716232 -0.07845306 0.9225668 0.3777813 -0.07843285 0.7099301 -0.699989 -0.0775541 0.8377504 0.5403902 -0.07843947 0.7572029 0.6435632 -0.1116706 0.5781971 0.8121168 -0.07845157 0.6317315 -0.7708731 -0.08167117 0.9962526 0.03642481 -0.07844758 0.3539004 0.911457 -0.2097642 0.9346386 0.3468394 -0.07844191 0.8527908 -0.3394973 0.3968494 0.8611822 0.07436847 0.5028268 0.05190062 0.9885349 -0.1417928 0.9962365 -0.03650158 -0.07861769 0.9962558 0.03622388 -0.07849961 0.994371 0.04251426 -0.09705066 0.03499436 0.9940232 -0.1034099 0.605542 0.7919399 -0.07842242 0.3165475 0.9453285 -0.07843464 0.7792494 0.6213716 0.08165663 0.8018573 0.5878324 -0.1071355 0.6308169 0.771955 -0.07845693 0.9962754 -0.03584021 -0.07842743 0.9763524 -0.2024986 -0.07569897 0.4217064 0.900121 -0.1092974 0.6680337 -0.739968 -0.07860386 0.3470024 0.9345769 -0.07845586 0.8574094 0.5086229 -0.0784341 0.99427 -0.07261592 -0.07844984 0.6055487 0.7919349 -0.07842105 0.2534537 0.9641628 -0.07843029 0.5979915 -0.7980023 -0.07482349 0.5994268 -0.796834 -0.07578372 0.8654622 0.01351201 0.500792 0.8739794 0.4795921 -0.07843196 0.5779795 -0.8120689 -0.08052206 0.4053496 0.9107875 -0.07847201 0.6312928 0.7715651 -0.07846581 0.1071909 0.9731797 -0.2035472 0.8412173 0.5294276 -0.109817 0.5039747 0.8618078 -0.05741965 0.2844365 0.9554799 -0.07844811 0.6591309 0.7479286 -0.07841747 0.7361119 0.6714701 -0.08524787 0.9899376 -0.1178295 -0.07835841 0.05499154 -0.02952146 -0.9980504 0.6768693 -0.05200439 -0.7342639 0.6028888 0.5064855 -0.6164395 -0.4722335 0.7455493 -0.4702678 0.09031772 0.9899202 -0.1090907 0.962242 0.2515307 -0.1040327 0.9679448 0.2244215 -0.1127733 0.9656215 0.231448 -0.118351 0.6963418 0.7098413 -0.1059882 0.8650646 0.4902613 -0.1063349 0.8531925 0.5089011 -0.1143788 0.4691263 0.8761421 -0.1108857 0.7002111 0.7057143 -0.1080362 0.2264414 0.9680948 -0.1073163 0.2227576 0.9687653 -0.1089627 0.4643393 0.8790004 -0.1083847 -0.2184 0.9698543 -0.1080932 -0.4775053 0.872245 -0.105723 -0.4290258 0.8945341 -0.1254823 -0.6948281 0.7102829 -0.1127488 -0.8610175 0.4947988 -0.1175707 -0.8073518 0.5814321 -0.1005977 -0.9640682 0.2442421 -0.1044911 -0.9075801 0.4080492 -0.09896647 -0.9660541 0.2313028 -0.1150595 -0.9676173 0.226296 -0.111835 -0.0848791 0.9909541 -0.1039503 -0.3707755 0.9254065 -0.07841217 -0.9250012 -0.3718321 -0.07819122 -0.371061 0.9252895 -0.07844263 -0.5524044 0.829877 -0.0784446 -0.3054816 0.9489624 -0.07843184 -0.3395129 0.9373241 -0.0784524 -0.8964807 0.4358845 -0.0795443 -0.9691045 -0.2338494 -0.07842838 -0.1885743 0.9789806 -0.07769709 -0.3293244 -0.3843496 0.8624505 -0.5370389 0.8363541 -0.1100049 -0.9729791 -0.2182328 -0.07540673 -0.9264392 -0.3475388 -0.144663 -0.9385214 -0.33619 -0.07844698 -0.5182725 0.851704 -0.0774219 -0.5298675 0.8444449 -0.07844269 -0.9496313 -0.303395 -0.07843583 -0.5122212 0.855291 -0.07814782 -0.5020174 0.8616102 -0.07487481 -0.1680281 0.982591 -0.07925605 -0.2357918 0.9686329 -0.07843738 -0.7341786 0.6724703 -0.093625 -0.953267 -0.2819455 -0.1085774 -0.8660058 0.499841 -0.0138933 -0.8555042 0.5114488 -0.08082616 -0.9865519 -0.1434029 -0.07842844 -0.9676661 -0.2011407 0.1522001 -0.9905252 -0.1128219 -0.07830172 -0.1348406 0.9876872 -0.07932293 -0.5906088 -0.8029226 -0.08060222 -0.1992142 0.9768111 -0.07844609 -0.6010787 -0.7953889 -0.07785207 -0.6239286 -0.7787092 -0.06576704 -0.9904742 -0.1131942 -0.07840889 -0.1139337 0.9904407 -0.07775861 -0.4612977 0.8872354 -0.004232466 -0.9827578 -0.1667579 -0.07986873 -0.9715399 -0.2127783 -0.1040947 -0.638733 -0.7651664 -0.08087402 -0.9758725 -0.1275852 0.1771858 -0.9945459 -0.09017616 0.05241072 -0.6375746 -0.7662107 -0.0801236 -0.9191745 0.3775986 -0.1119713 -0.9765384 -0.2005556 -0.07842373 -0.9224897 0.3779727 -0.07841938 -0.2692196 0.9598746 -0.07849609 -0.4663942 0.8811141 -0.0781942 -0.9097117 0.4078059 -0.07822543 -0.05699723 0.99551 -0.07557362 -0.6853856 0.7210315 -0.1017852 -0.9839546 -0.1602651 -0.07841271 -0.4176205 0.900879 -0.1183648 -0.6876342 -0.7217595 -0.07888221 -0.8075569 0.5771587 0.1214076 -0.6874759 -0.7219225 -0.07877027 -0.4144622 0.8469389 0.3330405 -0.9944194 -0.07242333 -0.07671332 -0.735851 -0.6725917 -0.07838249 -0.7361813 -0.6722024 -0.07861971 -0.235817 0.9686266 -0.07844084 -0.9965255 0.02804601 -0.07842534 -0.9949781 0.06215584 -0.07845616 -0.8429819 0.5321877 -0.07847207 -0.9961563 -0.03823274 -0.07881045 -0.8945646 0.4399617 -0.07866287 -0.9961256 -0.03977823 -0.0784313 -0.9968891 -0.007547497 -0.07845658 -0.9968883 -0.007612168 -0.0784592 -0.7806006 -0.6200675 -0.07860749 -0.992371 0.09572219 -0.0776984 -0.9921358 0.09881281 -0.07682842 -0.3265144 -0.03839176 0.9444122 -0.3453575 0.9207441 -0.1815455 -0.4347464 0.8971291 -0.07845544 -0.7816683 -0.6186197 -0.07939952 -0.7244869 -0.5821486 0.3690824 -0.9965256 0.02803266 -0.07842838 -0.861909 0.5009644 -0.07840692 -0.8422152 0.5334028 -0.07845449 -0.8213508 0.5650073 -0.0784204 -0.8231648 -0.5623319 -0.07862997 -0.9824089 0.1694064 -0.07857632 -0.337593 0.810679 0.4783624 -0.6275007 0.7676427 -0.1302604 -0.9806186 0.0211637 -0.1947801 -0.8238295 -0.561295 -0.07907432 -0.6625102 0.730593 -0.1652703 -0.8442996 -0.5303493 -0.07673358 -0.4347805 0.8971129 -0.07845157 -0.7797556 0.6211542 -0.07841432 -0.892197 -0.4454556 -0.07452327 -0.2823529 0.9336749 -0.220291 -0.7565249 0.6492417 -0.07845699 -0.7342994 0.6738008 -0.08244317 -0.1032974 0.9915522 -0.07844787 -0.9672859 0.2410486 -0.07907915 -0.8215237 0.5647639 -0.07836234 -0.9668019 0.2428288 -0.07955151 -0.7802731 0.6204847 -0.07856768 -0.07403224 0.9941655 -0.07844901 -0.4038584 0.9114508 -0.07845997 -0.7088071 0.7010253 -0.07846075 -0.2169176 0.9649604 -0.147642 -0.7566319 0.6491171 -0.07845568 -0.8617255 -0.5012906 -0.07833844 -0.8618565 -0.5010511 -0.0784294 -0.9826103 0.1680671 -0.07893341 -0.07369846 0.9941875 -0.07848459 -0.05503875 0.9953793 -0.07868188 -0.1471861 0.9767223 -0.1560452 -0.8801206 -0.4682229 -0.07845485 -0.6290126 0.7733828 -0.0788815 -0.6290476 0.773355 -0.07887512 -0.9386271 0.3346985 -0.08340311 -0.9477385 0.3095239 -0.07737421 -0.8639498 -0.4793047 -0.1544595 -0.9289875 -0.3625181 -0.07458561 -0.8972098 -0.4346223 -0.07821756 -0.08962982 0.9885976 -0.121002 -0.3054606 0.9489693 -0.07842922 -0.03087478 0.9949734 -0.0952605 -0.2326314 0.6140463 0.7542081 -0.9914746 0.09294128 -0.09132432 -0.7340082 0.6745979 -0.07841992 -0.684419 0.7248587 -0.07842552 -0.684448 0.7248308 -0.07843106 -0.9348623 0.3462318 -0.07846009 -0.4026395 0.9119825 -0.07854586 -0.2694193 0.9598196 -0.07848387 -0.6574485 0.7494043 -0.07845246 -0.3394652 0.9373416 -0.07844877 -0.582225 0.8092288 -0.07850354 -0.9396432 0.3322123 0.08188802 -0.8213872 0.5636379 -0.08738124 -0.8996613 -0.41812 -0.1256391 -0.9110993 -0.4043505 -0.07999318 -0.9545741 -0.2885147 -0.07448184 -0.922473 0.3780121 -0.07842624 -0.4642341 0.8822689 -0.07802784 -0.5728227 0.8160557 -0.07698947 -0.9605218 -0.2669211 -0.07842993 -0.7086926 0.7011417 -0.07845574 -0.7340158 0.6745899 -0.07841825 0.1177607 0.2207908 -0.968186 0.1847985 -0.02202361 0.9825296 -0.005558013 0.5755379 -0.8177562 -0.8692817 0.08020287 -0.4877674 -0.6278985 -0.4917163 -0.6032899 -0.6145096 -0.4028977 -0.678271 -0.9757758 -0.1919321 -0.1049934 -0.9629191 -0.2527844 -0.0942707 -0.9145397 -0.3502817 -0.2022868 -0.8618055 -0.4950041 -0.1107357 -0.8600198 -0.4977274 -0.1123985 -0.694612 -0.7085096 -0.1246137 -0.6504359 -0.7530832 -0.09898972 -0.4744448 -0.871774 -0.122116 -0.4930009 -0.8627572 -0.1122508 -0.3127591 -0.9447189 -0.09842771 -0.8608297 0.5014295 -0.08683717 -0.8161948 0.5771956 -0.02591449 -0.6022227 0.798263 0.01020199 -0.639158 0.7689812 -0.0120458 -0.2842774 0.9585483 -0.01927649 -0.6290353 0.7771773 -0.01761215 -0.3020382 0.9531875 -0.0143724 -0.2936037 0.9558568 -0.01161271 -0.7194342 0.6942072 -0.02215665 -0.5740787 0.8151332 -0.07740604 -0.553034 0.8322556 0.03878635 -0.07152193 0.9972835 -0.01761615 -0.8166632 -0.3891754 -0.42615 -0.2836835 -0.8096825 -0.513749 0.2970805 -0.6115134 -0.7333449 -0.8778812 0.4767866 -0.04471302 -0.8659297 0.4998604 -0.01747936 -0.3984094 -0.6239804 -0.6722488 -0.4856192 -0.8704962 -0.08006626 -0.5386787 -0.8370645 -0.09564691 -0.5292893 -0.8448649 -0.07782232 0.852382 -0.5200631 -0.05458432 0.8657808 -0.5000856 -0.01838386 -0.7930487 0.6089113 -0.01734286 -0.6071613 0.7945115 -0.01033067 -0.8115274 0.5838717 -0.02274286 -0.3823149 0.9240314 -0.001168549 -0.6818503 0.7308672 -0.03022474 -0.5134584 0.8572902 -0.03760403 -0.1307287 0.991212 -0.02021688 -0.3192563 0.9475514 -0.01489222 -0.108923 0.9938979 -0.01740127 0.7654051 -0.3372978 -0.5480742 -0.8424351 0.5365561 0.04909825 0.8560187 -0.516649 -0.01748913 0.8385039 -0.5446149 -0.01749151 0.8384634 -0.5446784 -0.0174514 0.8198799 -0.5722697 -0.01744949 0.685331 -0.3156153 -0.6562839 0.6555738 0.7545673 -0.0291745 -0.6329482 -0.7722203 0.05524849 0.6114288 0.7911064 -0.01748162 0.7986522 -0.6015394 -0.01746243 0.151102 -0.420449 0.8946457 0.8198934 -0.5722499 -0.01746189 0.7986384 -0.6015583 -0.01745098 0.7419506 -0.5108496 -0.4342143 0.6634487 0.7478725 -0.02285921 0.7779982 -0.6280244 -0.01744425 0.6396126 0.7684987 -0.01748698 0.777993 -0.6280306 -0.01745074 0.1226375 -0.4161772 0.9009754 0.6370491 0.720372 -0.2742856 0.6913612 -0.7191765 -0.06931829 0.6773657 0.7352397 -0.02446079 0.7546229 -0.655927 -0.01744425 0.7315195 -0.6815974 -0.01744365 0.7546224 -0.6559277 -0.01744049 0.6758075 -0.548353 -0.4925376 0.7474099 -0.6613657 -0.06303799 0.2696063 -0.05268621 0.9615283 0.09728699 -0.4092929 0.9072015 0.7096462 0.7045465 -0.004092276 0.7062408 -0.7077568 -0.01744145 0.6821592 -0.7309957 -0.0174418 0.6821054 -0.7310446 -0.01749503 0.09012097 -0.415813 0.9049739 0.4135749 -0.1846344 -0.8915526 0.654885 -0.7555261 -0.01749086 0.649608 0.6863591 -0.3269874 0.654838 -0.7555682 -0.01743686 0.6293671 -0.7769126 -0.017439 0.6100116 -0.7923629 -0.006853997 0.6100383 -0.7923419 -0.006902039 0.5966625 -0.8019912 -0.02835786 0.7029448 0.7112336 0.003943324 0.7140159 0.6999125 -0.01743334 0.7365202 0.6761905 -0.01744699 -0.6935974 -0.7192673 0.03971457 0.7667633 0.6409645 -0.03519642 0.596735 -0.8019327 -0.02848488 0.05134636 -0.3952153 0.9171524 0.5768529 -0.8166617 -0.01744621 0.5472674 -0.8367762 -0.01743936 0.7139078 0.6321119 -0.301281 0.7455102 0.6657146 -0.03222793 0.5403243 -0.8409274 -0.0298506 0.01405787 -0.3521851 0.9358248 0.5168778 -0.8558824 -0.01739883 0.7495151 0.6619397 0.007933497 -0.4963824 0.7482541 0.4401369 0.1542172 -0.2061516 0.9662912 0.7994567 0.6006541 -0.00915426 0.2940534 -0.9526833 -0.07698804 0.4439854 -0.8955661 0.02895605 0.4857366 -0.8739323 -0.01739305 -0.4226443 0.9057828 0.03048712 0.4370253 -0.8033872 -0.404448 0.4651402 -0.8824977 -0.06958729 0.0127424 -0.9963962 -0.08385866 0.4204252 -0.907243 -0.01236379 0.6867778 0.5866513 -0.4291581 0.400999 -0.9132133 -0.07239764 0.3613278 -0.9322739 -0.01754504 0.8010238 0.5985229 -0.0114665 0.802803 0.595989 -0.01745045 -0.7986624 -0.6013135 0.0236718 0.8219164 0.5693408 -0.01745629 0.2645784 -0.9610844 -0.07946777 0.2992948 -0.954001 -0.0174579 0.2794967 -0.956915 -0.07871079 0.1811179 -0.9817695 0.05766218 0.6247238 0.5360285 -0.5677972 0.1717766 -0.9851087 -0.007329106 0.8347105 0.5505382 0.01288592 0.1440237 -0.2426234 0.9593702 0.841698 0.5396663 -0.01746463 0.8779147 0.4786771 -0.01157927 0.8606645 0.5088724 -0.0174812 0.1111193 -0.9650633 -0.2372872 -8.99439e-5 0.3273748 -0.9448946 0.04109877 -0.9990013 -0.01753109 0.005245745 -0.999881 -0.0145092 -0.4824616 0.5418758 -0.6881871 0.9103221 0.412783 -0.03039658 0.8790491 0.4765245 -0.01404476 0.8789297 0.476912 -0.006132304 0.9071503 0.4190832 -0.03804868 0.04687935 0.9979124 -0.04442095 0.8430283 0.431867 -0.3206158 0.9038606 0.4263195 -0.03588598 0.06753289 0.9967397 -0.04415303 0.9048398 0.4240387 0.03816133 0.02168554 0.9996162 -0.01724493 0.373256 -0.1425207 0.9167158 0.9233953 0.3834535 -0.01745378 0.07173007 0.9966076 -0.04035228 0.8791086 0.3691908 -0.3014402 0.1000133 0.9943863 -0.0345425 0.2320907 -0.2492069 0.9402287 0.9362549 0.3508882 -0.0174483 0.9362537 0.3508917 -0.0174387 0.9474188 0.3195208 -0.01744234 0.9580161 0.2866634 -0.005428731 0.1259843 0.9675697 -0.2189447 0.110773 0.9931595 -0.03692829 0.8374364 0.3339943 -0.4326064 0.1174913 0.7107195 0.6935948 0.9539632 0.2997605 0.009899497 0.9579976 0.2862496 -0.01737433 -0.9468849 -0.318299 0.04577046 0.9678382 0.2509725 -0.01738041 0.1333845 0.9909112 -0.01742851 0.2041175 0.9550742 -0.2148705 0.972467 0.2327626 0.01137942 0.9760658 0.2167487 -0.01776158 0.4058682 -0.2133969 0.8886691 0.9830933 0.1827232 -0.0118224 0.168522 0.9855438 -0.01743537 0.169313 0.9853985 -0.01797527 0.1729692 0.6549334 0.7356248 0.9884197 0.1506412 -0.01826971 0.9362085 0.1892415 -0.2961442 0.1999568 0.9796409 -0.01791936 0.1993309 0.9797769 -0.01745063 0.9885812 0.1496747 -0.01745444 0.1984117 -0.300857 0.9328011 0.2533465 0.9436405 -0.2129746 0.9933943 0.113415 -0.0174604 0.9965819 0.08239424 -0.005995273 0.9968902 0.06157219 -0.04918187 0.9293952 0.1313446 -0.3449249 0.2341652 0.9720403 -0.01744318 0.2341793 0.9720367 -0.01746058 0.2127647 0.5299538 0.8209021 0.9934771 0.1083415 -0.03557163 0.9963687 0.08507955 -0.003340005 -0.9949328 -0.0953533 0.03188115 0.2683824 0.9631541 -0.01746547 0.3027389 0.9529137 -0.01746112 0.9985555 -0.006889522 -0.05328887 0.2683875 0.9631527 -0.01746791 0.3296746 0.9205194 -0.2096632 0.9960837 0.07300335 -0.049878 0.9350048 0.06426382 -0.3487641 0.3361198 0.9416564 -0.01751857 0.3022353 0.953067 -0.01780682 0.1447305 0.05274146 0.9880645 0.9997031 0.02149021 0.0114926 0.3845113 0.9225943 -0.03115838 0.08142012 -0.3275281 0.9413269 0.9981935 -0.05963432 -0.007331609 0.9953682 -0.07876574 -0.05511999 0.9381788 -0.004638016 -0.34612 0.3637347 0.931497 0.003232181 0.9988136 0.03130662 -0.03730171 0.3846324 0.9225439 -0.03115594 0.3727949 0.8851938 -0.2783092 0.9874575 -0.134589 -0.08254474 -0.5530819 0.5665664 0.6108216 0.9907939 -0.1343039 -0.01703274 0.9850648 -0.1494053 -0.08558893 0.9904946 -0.1369272 -0.01309788 0.8554781 0.1133784 0.5052749 0.4159983 0.8608545 -0.2930445 0.9597633 -0.1948065 -0.2022497 0.4139885 0.9102765 0.003204882 0.4354442 0.9000471 -0.01742625 0.09557217 -0.1510816 0.9838904 0.0941177 -0.3469572 0.9331467 0.4646239 0.8853367 -0.017425 0.9802519 -0.1964934 -0.02228611 0.9803571 -0.1964586 -0.01744097 0.5067172 0.8615531 -0.03105098 0.9631261 -0.2689352 -0.007884263 0.9721354 -0.2337701 -0.01744854 0.4888064 0.8723908 -0.0016613 0.9647251 -0.2626378 -0.01808232 -0.9576974 0.2826892 0.05387651 0.9550636 -0.2961981 -0.01096463 0.4919064 0.8326977 -0.2542493 0.498032 0.8665985 -0.03116804 0.9449934 -0.3265385 -0.01897978 0.4685559 0.8832778 0.01660621 0.9446554 -0.3275721 -0.01797193 -0.4540554 -0.8893549 0.05368155 0.8666648 -0.2982642 -0.3999134 0.9207952 -0.3896557 -0.01745718 0.9277461 -0.3722795 0.02636784 0.01732492 -0.3350194 0.942052 0.4822106 0.7749376 -0.4085885 0.9057476 -0.4234579 -0.01746237 0.8683806 -0.4955875 0.01755505 0.8097572 -0.3076142 -0.4996667 0.5386832 0.8424849 0.006314694 0.5537235 0.8325166 -0.01750659 0.07996851 -0.2242832 0.9712374 -0.8641411 0.4946491 0.09264212 0.8907349 -0.4541882 -0.01745003 0.5827601 0.8124555 -0.01751959 0.873285 -0.4868972 -0.01745337 0.6146435 0.7888049 6.41634e-4 0.8534204 -0.521212 0.003453195 0.8759144 0.4809984 -0.03761017 0.8659565 0.4998113 -0.01755648 -0.9588866 -0.2829911 -0.02127319 -0.9288949 -0.3702936 -0.006080389 -0.9943765 -0.1051886 -0.01228046 -0.8624503 -0.5057939 -0.01876997 0.5449916 -0.834088 -0.08533209 0.5189182 -0.851666 -0.07340937 0.485367 -0.8704913 -0.08163303 0.03937268 0.2163934 -0.9755121 0.08666378 0.143249 -0.985885 0.1065715 0.1393359 -0.9844939 -0.006854116 0.2193722 -0.9756172 -0.1623013 0.6048254 -0.7796438 0.2608296 -0.007868528 -0.9653529 0.28653 0.06416058 -0.9559205 -0.008745431 -0.00402671 -0.9999537 -0.4769937 -0.03537923 -0.8781943 -0.6189602 -0.153575 -0.7702617 -0.006605446 -0.004832267 -0.9999665 -0.00184369 -0.01106673 -0.9999371 0.006069242 -0.03058439 -0.9995138 -0.02349269 -0.5246832 -0.8509734 0.01063811 -0.02341276 -0.9996693 0.02899938 -0.01124417 -0.9995163 0.07816803 0.03661334 0.9962677 0.5844345 -0.1770254 -0.7918955 -0.01178139 -0.02982747 -0.9994857 -0.2131049 -0.1344605 -0.9677328 0.1846453 0.141977 0.9724962 0.7117052 -0.0871399 -0.6970525 0.06596004 -0.01869601 -0.9976471 0.08349066 0.01650822 -0.9963718 0.3552907 -0.2981711 -0.8859248 0.4852304 -0.6068061 -0.6295537 0.4664666 -0.6681985 -0.5795858 0.1533665 -0.6268247 -0.7639172 -0.003811359 -0.00485748 -0.9999809 0.8011947 -0.03657245 -0.5972852 0.3472661 0.03243446 -0.9372056 0.8835746 0.08102703 -0.4612272 0.9394457 0.1080535 -0.3252171 0.9217823 0.04279178 -0.3853394 0.3627706 0.009079992 -0.9318343 0.3852108 0.01236957 -0.9227458 0.814758 0.02048915 -0.5794391 0.3849953 -0.01294416 -0.9228278 -0.01076799 0.5066504 -0.8620845 0.281216 0.1863578 -0.9413758 0.6678565 0.1359857 -0.7317619 0.1042286 0.06155407 -0.9926468 -0.08856379 0.07102024 -0.9935355 0.260107 0.5195574 -0.8138825 0.1263418 0.4751777 -0.870772 0.2908431 0.2000305 -0.9356272 0.6464771 0.4835833 -0.5900971 0.2004399 0.1682592 0.9651491 -0.5281971 7.3346e-4 -0.8491215 -0.3504163 -0.001422703 -0.9365931 -0.5202419 0.001817703 -0.8540171 -0.1281968 0.003875136 -0.9917412 -0.3420967 -0.002867877 -0.9396604 -0.1107041 0 -0.9938535 -0.09890341 0.2285189 -0.9685025 -0.962758 0.2419254 -0.1207036 -0.2228708 0.1342441 -0.9655606 -0.1700205 0.1359159 -0.9760225 -0.001179754 0.03839921 -0.9992618 -0.4456066 0.5371298 -0.7161889 -0.03976106 0.0262919 -0.9988633 -0.9463151 0.04897844 -0.3195135 -0.8896906 0.07878828 -0.4497144 -0.4049983 -0.01024419 -0.91426 -0.9228086 0.01991546 -0.3847438 -0.9165087 0.02133584 -0.3994454 -0.2878674 0.01102328 -0.9576069 -0.4118722 -0.006159901 -0.9112209 -0.2601746 -0.9578883 -0.1214874 -0.8806686 -0.3258449 0.3438723 -0.5973231 -0.1679102 -0.7842266 -0.9718765 -0.1430562 -0.187059 -0.06739491 -0.02572762 -0.9973947 -0.01484602 -0.05021601 -0.9986281 -0.404826 -0.5628216 -0.720658 -0.02003335 -0.0395559 -0.9990166 -0.02186512 -0.03700309 -0.999076 0.05040723 -0.03714674 -0.9980378 0.5268536 -2.5495e-4 -0.8499561 0.3506528 0.002051949 -0.9365033 0.5202968 -0.001263558 -0.8539847 0.1282087 -0.003875136 -0.9917396 0.3453546 0.002972304 -0.9384676 0.1107041 0 -0.9938535 -0.2763545 0.2078964 -0.9383003 -0.2970218 0.773505 -0.5598823 -0.2050174 0.9762235 -0.07039576 -0.2805343 0.7887703 -0.5469388 0.1783329 0.5862744 -0.7902404 -0.09950172 -0.4777438 0.8728461 0.0115087 0.005731105 -0.9999174 0.007019877 -0.002145051 -0.9999731 -0.653903 0.2769948 -0.7040489 -0.5075141 -0.8436297 -0.1752671 0.005319058 -0.02606314 -0.9996463 0.04861479 -0.02887731 -0.9984002 0.8645432 0.5010557 -0.03883743 0.3622481 0.7362676 -0.5715649 0.1169153 0.3732129 -0.9203494 0.01870906 0.1421036 -0.989675 -0.1954512 0.844874 -0.4979828 -0.2793496 0.1919651 -0.9408047 -0.7125614 0.4345497 -0.5508384 -0.003090083 0.3233297 -0.9462813 1.48816e-4 0.3274585 -0.9448655 -0.005752921 0.3223871 -0.9465906 -1.88551e-4 0.3276187 -0.9448101 9.7896e-5 0.3274201 -0.9448789 -0.004952967 0.3207066 -0.9471657 1.92142e-5 0.3276146 -0.9448115 -9.06858e-4 0.3270314 -0.9450131 -0.001826286 0.3265466 -0.9451794 1.92799e-4 0.3276801 -0.9447888 3.80124e-4 0.3282878 -0.9445777 1.33193e-4 0.3279397 -0.9446986 1.91405e-4 0.3271117 -0.9449856 -3.24862e-4 0.3273896 -0.9448894 2.987e-4 0.3282617 -0.9445868 3.20548e-4 0.3281463 -0.9446269 1.71445e-4 0.3271506 -0.9449722 8.17896e-5 0.327786 -0.944752 -6.85893e-4 0.3273385 -0.944907 -8.71414e-4 0.3272894 -0.9449238 -2.20895e-4 0.3275724 -0.9448261 3.47917e-5 0.3276679 -0.9447931 -4.90333e-4 0.3275914 -0.9448195 -3.4537e-4 0.3275836 -0.9448221 2.98773e-5 0.3269525 -0.9450409 -9.01752e-6 0.3278552 -0.9447281 -1.90844e-5 0.3276785 -0.9447893 -1.90169e-5 0.3276888 -0.9447858 -5.59698e-4 0.3274303 -0.9448752 8.4374e-5 0.3276371 -0.9448037 1.66998e-5 0.3278108 -0.9447435 -6.94653e-4 0.3275658 -0.9448282 -1.07145e-4 0.3275479 -0.9448347 -7.14324e-5 0.3276113 -0.9448126 -1.792e-4 0.3278323 -0.944736 1.21075e-4 0.3275085 -0.9448483 5.16095e-4 0.3275455 -0.9448353 1.7511e-4 0.3283452 -0.9445579 -1.97057e-4 0.327534 -0.9448394 1.68081e-4 0.3273645 -0.9448981 -6.02208e-4 0.3274341 -0.9448739 -1.07206e-4 0.3274466 -0.9448696 2.15611e-5 0.3276529 -0.9447982 -1.44131e-4 0.3275355 -0.9448389 -7.82302e-5 0.32754 -0.9448373 1.07858e-4 0.3278724 -0.9447221 -4.34832e-4 0.327677 -0.9447898 -1.79148e-4 0.327542 -0.9448366 7.81838e-5 0.3275785 -0.944824 1.81857e-5 0.3275606 -0.9448302 1.80969e-4 0.3288053 -0.9443978 -1.75835e-5 0.3275415 -0.9448368 9.92941e-5 0.3278549 -0.9447281 0.001820445 0.3280311 -0.9446652 -2.10278e-4 0.3275015 -0.9448506 -2.50504e-4 0.3275321 -0.94484 1.2161e-4 0.3277531 -0.9447634 1.08788e-4 0.3272689 -0.9449313 2.23262e-4 0.3276547 -0.9447976 -3.23013e-4 0.3276229 -0.9448086 -3.60113e-4 -0.3267362 0.9451155 0.003512799 -0.3346944 0.9423202 9.52891e-5 0.3277267 -0.9447726 -5.05808e-4 0.3276676 -0.944793 1.45542e-4 0.3279213 -0.9447051 2.13224e-4 0.3276606 -0.9447955 3.64282e-4 0.3280118 -0.9446737 2.39062e-4 0.3278472 -0.9447308 3.88285e-4 0.3277931 -0.9447496 2.06925e-4 0.3276877 -0.9447861 -5.66524e-4 0.3288614 -0.9443781 -2.76234e-4 0.3277146 -0.9447768 -1.72531e-4 0.3276 -0.9448165 1.61837e-4 0.3276793 -0.9447891 -3.02809e-4 0.3278381 -0.944734 0.00141859 0.3268689 -0.9450687 -2.27572e-5 0.3276956 -0.9447835 3.78521e-4 0.3272686 -0.9449313 -0.00106287 0.3286108 -0.9444649 -8.85536e-4 0.328374 -0.9445474 4.20731e-4 0.3272716 -0.9449303 -2.03934e-4 0.3276105 -0.9448129 6.20288e-4 0.3270278 -0.9450145 -1.91422e-4 0.3277721 -0.9447569 -8.45748e-5 0.3276087 -0.9448136 -1.22001e-4 0.327745 -0.9447663 1.43793e-4 0.3275135 -0.9448465 -1.11708e-4 0.3276209 -0.9448093 -3.63521e-5 0.3274155 -0.9448806 0.7082419 0.4181104 -0.5688385 0.4951391 0.8679719 -0.0382359 0.1953448 0.4727841 -0.8592531 -0.2790154 0.6771656 -0.6808798 -0.06271225 0.3726713 -0.9258419 -0.7815554 0.4566577 -0.4250118 -0.9977476 0.0444951 -0.05019998 0.03949481 -0.06239587 -0.9972698 -0.418241 -0.2367631 -0.8769366 -0.1947931 -0.07875698 -0.9776774 0.4626758 -0.8789445 -0.1157047 0.1650033 -0.2649117 -0.9500504 0.2348787 -0.4614243 -0.8555231 -8.40156e-6 0 -1 9.90253e-6 0 -1 2.3798e-5 -2.0644e-4 -1 -4.23548e-5 -2.66649e-4 -1 -1.20334e-5 0 -1 -0.005304276 -0.004023551 -0.9999779 1.17191e-4 -1.74494e-4 -1 2.66133e-4 -1.22411e-4 -1 -9.87583e-6 0 -1 -0.001017749 -0.002696037 -0.9999959 -4.20475e-4 2.35721e-4 -0.9999999 -3.20069e-5 3.13083e-4 -1 -2.03473e-4 2.73268e-4 -1 -6.47335e-4 2.24536e-4 -0.9999998 4.72425e-7 0 -1 2.15613e-6 0 -1 -9.6037e-6 0 -1 2.67655e-4 0.001322865 -0.9999991 0.002785623 0.003836572 -0.9999889 -5.42082e-4 2.14358e-4 -0.9999998 -6.96849e-4 0.001074016 -0.9999992 -0.00127995 0.001215398 -0.9999985 -2.77285e-5 5.18064e-4 -0.9999999 -1.65307e-4 3.39978e-4 -1 2.53519e-6 0 -1 4.27235e-4 0.001407921 -0.999999 2.6759e-6 0 -1 5.9487e-6 0 -1 3.94174e-4 -3.37342e-4 -1 3.58058e-5 5.22472e-4 1 -9.99193e-7 0 1 1.25845e-5 0 -1 -2.17011e-4 3.66652e-4 -1 -2.54401e-4 3.54808e-4 -1 -5.99863e-4 8.97397e-4 0.9999995 -0.002076983 0.001384973 0.999997 0.009530782 0.3236491 -0.9461292 -0.1634101 0.3485201 -0.9229469 0.1475463 0.2783787 -0.9490708 -0.2957366 -0.01182323 -0.9551964 -0.1414697 0.7342857 -0.6639359 -0.3297889 0.391836 -0.8588969 -0.3260591 0.3914989 -0.8604731 -0.5146666 -0.2646989 -0.8155077 -0.54697 -0.2700351 -0.7924047 -0.3475227 0.4166134 -0.8400366 -0.2186242 0.4037859 -0.8883471 -0.1183039 0.258136 -0.9588379 -0.1992651 0.1836378 -0.9625854 0.05763757 0.07863008 -0.9952363 -0.1644178 0.3911343 -0.905528 0.03523349 0.1550326 -0.9872809 -0.8614749 -0.4526677 -0.2301154 -0.2993819 0.1083614 -0.9479601 -0.08320879 0.2560217 -0.9630832 0.170253 0.2103139 -0.9626952 -0.8938576 -0.3949653 -0.2121818 -0.5020954 0.5602023 -0.6588427 -0.4593781 0.5668803 -0.6838265 -0.6058639 -0.01690447 -0.7953886 -0.2574973 0.4905954 -0.8324731 -0.3467791 0.1678137 -0.9228125 -0.07342177 -0.880107 -0.4690639 -0.448398 0.1162274 -0.8862452 -0.05291372 -0.7107727 -0.7014288 -0.3308995 0.5404688 -0.7735627 -0.09041613 0.2954394 -0.9510734 -0.3526251 0.5844929 -0.7307692 -0.11367 -0.9445866 -0.3079535 -0.1906176 0.4972453 -0.8464114 -0.2230817 0.2628607 -0.93869 -0.005806028 0.2985753 -0.9543684 -0.1829912 -0.8393161 -0.5119207 -0.144568 -0.597491 -0.7887362 -0.1373891 0.4583661 -0.8780801 -0.01020848 0.2813427 -0.9595531 -0.2645534 0.5881937 -0.7642249 -0.2496082 -0.9673256 -0.0444625 -0.06723231 0.3429889 -0.9369304 -0.2734137 -0.6347597 -0.7227207 -0.1607719 -0.1786714 -0.9706849 -0.2309388 0.2963271 -0.9267457 0.1396149 0.3237043 -0.9358009 -0.04599791 0.322709 -0.94538 -0.053685 0.4510654 -0.8908749 -0.07263612 0.3093414 -0.9481729 -0.2132906 -0.1390076 -0.9670492 -0.2051077 0.6530947 -0.7289707 -0.3440017 -0.3994742 -0.8497548 -0.3960782 -0.4310659 -0.810743 -0.03869825 0.4091348 -0.9116531 -0.3408783 0.3309382 -0.8799329 -0.3742676 0.3279079 -0.8674101 -0.1213079 0.1756849 -0.9769439 0.02761942 0.2123039 -0.9768134 0.1163612 -0.1049864 -0.9876427 0.8676677 -0.009563386 -0.4970526 0.442013 0.7037353 -0.5562204 0.3093239 0.2504535 -0.9173832 0.4516662 0.7167654 -0.5312676 0.539331 0.1005252 -0.8360723 0.04316663 0.3006491 -0.9527575 0.2747787 0.2305862 -0.9334489 0.4064566 0.6116798 -0.6787053 0.172017 0.4722847 -0.8644983 0.5813187 0.6498991 -0.4895915 0.6640362 -0.03779208 -0.7467448 -0.04717528 0.7138343 -0.6987239 0.2540146 0.4732266 -0.8435244 0.8004898 -0.2238625 -0.5559691 0.3087437 0.4973984 -0.8107233 -0.03325533 0.5072111 -0.8611801 0.03707134 0.3478942 -0.9368007 0.3297553 0.1095373 -0.9376903 0.2538217 0.1709455 -0.9520254 0.6235334 -0.1822091 -0.760267 0.2952618 0.4238048 -0.8562768 0.3164188 0.4303492 -0.8453868 0.2761726 0.1510199 -0.949169 0.7701583 -0.6370773 0.03144609 0.716293 -0.5070984 -0.4793493 0.2773305 0.3993222 -0.873859 0.2042135 0.3861184 -0.8995607 0.1214984 0.3627744 -0.9239225 0.08408653 0.2565013 -0.9628794 0.03031885 0.2992163 -0.9537035 -0.00143969 0.4749241 -0.8800256 0.09158468 0.8348782 -0.5427622 0.1793037 0.9389581 -0.2936118 0.4421346 -0.2581311 -0.8590027 0.399864 0.3519279 -0.8463189 0.1790474 0.8794424 -0.441048 0.3437359 0.3443889 -0.8736372 0.5381019 -0.5230245 -0.660978 0.03327244 0.4283305 -0.9030095 0.3862182 -0.3150231 -0.8669464 0.4181679 -0.3968292 -0.8171061 0.2668607 0.3394101 -0.9019902 0.4820903 0.3095065 -0.8196309 0.7600474 0.1990256 -0.6186411 0.3779504 -0.4371624 -0.8161143 0.2093856 0.6705738 -0.7116801 0.3161359 -0.3174133 -0.8940398 0.6435261 0.2127628 -0.7352593 0.1436189 0.5651163 -0.8124145 0.04108661 0.2347114 -0.9711964 0.1669864 -0.1884153 -0.9677888 0.6677971 0.2033371 -0.7160316 0.3585114 0.7629551 -0.5379306 0.1093358 0.3143066 -0.9430044 0.2192661 0.5798252 -0.7846815 0.9482768 -0.2207319 0.2281423 0.1649613 0.5145069 -0.8414692 0.1603755 -0.1317895 -0.9782184 -2.31455e-5 4.32433e-4 -1 2.63511e-4 -3.73883e-5 -1 2.30724e-4 -1.65348e-4 -1 1.96686e-4 4.76008e-4 -0.9999999 3.39539e-4 1.51895e-4 -1 -4.61015e-4 -1.74636e-5 -0.9999999 2.44167e-4 3.70266e-4 -1 4.07173e-4 2.3824e-4 -1 3.83542e-6 0 -1 -4.83781e-4 -5.27858e-5 -0.9999999 -3.91612e-4 2.26584e-4 -1 -2.41748e-6 0 -1 -4.27506e-4 1.99195e-4 -1 -2.56818e-4 3.58211e-4 -1 -1.97116e-4 4.75177e-4 -1 0.5962536 -0.06971657 0.7997633 0.5405339 -0.07381027 0.8380783 0.5960983 -0.0706402 0.799798 -0.5408556 -0.07380014 0.8378717 -0.5963015 -0.07064014 0.7996465 -4.6642e-5 -0.08724933 0.9961865 -1.64944e-6 -0.08715766 0.9961946 -3.96127e-4 -0.0867896 0.9962266 -1.03484e-4 -0.08716374 0.9961941 -2.43796e-5 -0.08721995 0.9961891 -8.90459e-6 -0.08712512 0.9961975 8.5686e-5 -0.08708411 0.996201 -9.62041e-5 -0.0871213 0.9961978 -7.88272e-5 -0.08710449 0.9961992 -8.76979e-6 -0.08712577 0.9961974 -5.68578e-5 -0.08713597 0.9961965 1.51917e-5 -0.08717155 0.9961934 -6.39336e-5 -0.08717185 0.9961933 6.547e-5 -0.08701908 0.9962067 1.54365e-4 -0.08723258 0.996188 3.39473e-5 -0.08719414 0.9961914 -2.06172e-4 -0.08720731 0.9961902 1.05151e-4 -0.08711081 0.9961987 4.25488e-5 -0.08724027 0.9961874 4.13187e-5 -0.0871914 0.9961916 -8.913e-5 -0.08716493 0.9961939 5.95936e-6 -0.0869928 0.996209 -4.84871e-5 -0.08723199 0.9961881 3.50941e-5 -0.0870946 0.9962002 1.86665e-6 -0.08726084 0.9961856 6.23711e-5 -0.08718967 0.9961917 3.9582e-5 -0.0871427 0.9961959 -7.57325e-6 -0.08712023 0.9961978 9.25775e-6 -0.08716952 0.9961935 -5.58635e-6 -0.08722615 0.9961885 -1.17667e-5 -0.08723253 0.996188 -6.92712e-6 -0.0871669 0.9961937 0 -0.08716839 0.9961936 0.9977016 0.04835909 -0.04746729 0.9304901 0.3654494 0.02519977 0.5532054 0.8329019 0.01543515 0.6610985 0.7500883 -0.01778614 -0.1174404 0.9927143 0.02694636 0.113027 0.9934198 -0.01849639 -0.4933963 0.8689659 -0.03819012 -0.7455188 0.6654852 0.03648811 -0.9932082 0.1137763 -0.02433949 -0.989894 0.1412114 -0.0130074 0.04362094 -0.06124782 0.997169 -0.07638657 -0.08813762 0.9931752 0.1461553 -0.6933141 0.7056587 -0.8861996 0.09321939 0.4538287 0.2492365 -0.7248877 0.6421986 -0.3599447 -0.6461453 0.6730054 -0.3484211 -0.3305462 0.8771215 -0.04245918 0.003420531 0.9990924 -0.04420536 0.006306052 0.9990026 -0.0531907 -0.04277473 0.9976679 0.01922714 -0.09647053 0.9951502 0.04768878 -0.06078994 0.9970108 -0.5983613 -0.319139 0.7349246 -0.01812744 -0.8261228 0.5631986 0.9934726 0.1134713 0.01169115 0.7830885 0.6217316 0.0149151 0.916909 0.3989319 -0.01145565 0.5478766 0.8363118 -0.02034384 0.1636601 0.9862068 0.02473193 -0.1636564 0.9862074 -0.02473151 -0.5478775 0.8363113 0.02034461 -0.9169091 0.3989319 0.01145499 -0.7830868 0.6217335 -0.01491451 -0.9934739 0.1134586 -0.01169264 -0.460497 0.5585581 0.6898953 -0.852447 -0.2974129 0.4299764 0.2148637 -0.2799873 0.9356499 0.9306987 0.2354441 0.2799393 0.253157 -0.8346657 0.4891264 0.2337936 -0.8086472 0.539843 0.3707092 -0.9077184 0.1965243 -0.9345073 -0.3160915 0.1636532 0.3951461 -0.8623256 0.3166294 -0.8873562 -0.3977125 0.2332891 0.2531545 -0.7057045 0.6617356 0.4003773 -0.8684399 0.2924216 0.7870458 0.207488 0.5809542 -0.7616533 -0.4521535 0.4641568 0.8772034 0.4792339 0.02914208 -0.8698167 -0.4496058 0.2031589 -0.7476229 -0.4671066 0.4720926 0.4511883 -0.8431752 0.2923781 -0.6231312 -0.4807597 0.6169098 0.8421778 0.3316469 0.4251433 0.4729449 -0.8497313 0.2329807 0.7954583 0.3638788 0.4846013 -0.5242846 -0.5161883 0.6772557 -0.8395048 -0.5024303 0.2068707 0.8413414 0.4180415 0.3426166 0.5103306 -0.8186165 0.2634955 0.7636981 0.3929714 0.5121903 0.1982885 -0.1801181 0.9634516 0.4281622 -0.7080444 0.5615606 0.786106 0.5732236 0.2311975 0.4360752 -0.6749753 0.5951864 0.61899 0.3470216 0.7045761 -0.7049308 -0.6733509 0.2228703 0.4130276 0.575409 -0.7059128 -0.6638208 -0.7130103 0.2257395 0.6319007 0.5732187 0.5216529 -0.4813972 -0.6840924 0.5479729 -0.5995244 -0.7297276 0.3287372 -0.6095651 -0.7551713 0.2411365 0.6605517 -0.6368343 0.3976351 0.5663352 -0.6190139 0.5441381 -0.6567174 0.6786714 0.3288275 -0.6030253 0.494788 0.6257358 -0.5588282 -0.793137 0.2421669 -0.3733413 -0.6977252 0.6113885 0.5393661 -0.587363 0.6033979 0.5518045 -0.5536811 0.6236578 0.7973277 -0.5611053 0.2223274 -0.6761537 0.5316988 0.5100123 -0.7065178 0.6068428 0.3641079 -0.7552946 0.6220248 0.2064349 0.5045841 -0.5461959 0.6686292 -0.5109447 -0.8302569 0.2227311 0.6476363 -0.5327693 0.5447239 0.7830838 -0.529243 0.3266218 -0.7666644 0.5214864 0.3745368 -0.7887762 0.5730243 0.222431 -0.3993937 -0.7592753 0.5137954 -0.7602714 0.5026947 0.4114431 -0.4515249 -0.8580324 0.2447568 0.7709233 -0.4621901 0.4382439 -0.6736577 0.2868266 0.6811138 -0.8352912 0.5162541 0.1891312 -0.3454234 -0.8038367 0.4842824 -0.3974726 -0.8485165 0.3493356 -0.7987469 0.3651809 0.4781698 -0.7403389 0.2977601 0.6026917 0.7584668 -0.4554818 0.4661166 -0.8691569 0.4398126 0.2261223 -0.8169049 0.3482987 0.4597331 0.8702577 -0.4040454 0.281778 -0.2156292 -0.7903865 0.573405 -0.3069146 -0.9205459 0.2416584 -0.7412802 0.2132431 0.6364206 0.8435814 -0.3660961 0.3928666 -0.8993096 0.3817141 0.2133933 -0.8950043 0.355094 0.2699551 0.777257 -0.4007246 0.4850686 -0.8115599 0.2215304 0.5406431 -0.1177775 -0.6868467 0.717196 -0.9242484 0.3154265 0.2151072 -0.2881838 -0.9492985 0.1256282 0.7250258 -0.3634805 0.5849955 -0.9235054 0.3108614 0.2247291 0.7949888 -0.3210356 0.5147126 0.8946436 -0.2720757 0.3543838 -0.7372121 0.07322108 0.6716822 -0.9461375 0.2532947 0.2016572 -0.1081357 -0.7948259 0.5971251 -0.1563144 -0.8027755 0.575428 0.7305222 -0.3120732 0.6074107 0.9661928 -0.1579436 0.203778 -0.8640046 0.1004266 0.4933667 -0.1117117 -0.8761897 0.4688412 0.8212908 -0.2590177 0.5083221 -0.8149129 0.06649523 0.5757566 -0.9598909 0.1813864 0.2137959 -0.8800259 0.09185719 0.4659579 -0.07824337 -0.8734057 0.4806667 -0.0814625 -0.8482784 0.5232474 0.7145994 -0.240069 0.6570499 -0.7758337 -0.03095591 0.6301777 -0.975848 0.1255673 0.178756 0.9139604 -0.1139881 0.3894654 -0.03526449 -0.8599806 0.5091068 0.9150606 -0.1131396 0.3871222 -0.7905338 -0.06869798 0.6085532 -0.8567212 -0.03329938 0.5147039 0.961545 -0.05226105 0.2696295 -0.9622583 0.01942503 0.2714434 0.04688543 -0.864956 0.4996528 -0.83335 -0.08244109 0.5465633 -0.9957157 0.01283782 0.09157246 -0.9597623 -0.03906822 0.2780829 0.9637528 0.009041428 0.266644 0.03404283 -0.8744983 0.4838327 0.6455619 -0.1720211 0.7440824 0.9706503 0.06589752 0.2312912 -0.9894558 -0.05112409 0.1355122 0.04892152 -0.7825479 0.6206654 0.105982 -0.8688701 0.4835624 0.8483342 0.005480766 0.529433 -0.8919414 -0.1762181 0.4163987 0.1193053 -0.8245879 0.553011 -0.9783689 -0.1242597 0.1653903 0.1294754 -0.9057184 0.4036217 0.980453 0.177704 0.08445805 0.1440799 -0.8160056 0.5597999 -0.9491781 -0.2088853 0.2354314 0.1872004 -0.8408377 0.5078861 0.8932266 0.07561248 0.4432032 0.8823335 0.1334846 0.4512975 -0.859025 -0.2830641 0.4265573 0.1869827 -0.8164356 0.5463246 0.9144731 0.4043261 0.01611149 0.9807337 0.1947827 -0.01487022 0.4517566 0.8920778 0.01064741 0.58392 0.8115792 -0.01941394 -0.1060352 0.9943588 -0.002706289 -0.1140094 0.9934705 -0.00427252 -0.5946956 0.8039423 0.003759324 -0.6660174 0.7458514 -0.01125055 -0.9475283 0.3192372 0.01667255 -0.9880347 0.1537017 -0.01277965 0.9872285 0.1570909 -0.02650386 0.8940945 0.4454523 0.04655689 0.6295627 0.7761093 -0.0361275 0.1988293 0.9795354 0.03126519 0.05635821 0.9983565 -0.01039695 -0.5480505 0.8363807 0.01040059 -0.5954416 0.8033736 -0.006348252 -0.9301081 0.367231 0.006352245 -0.9497229 0.3129191 -0.0104019 0.1087419 -0.004365444 0.9940605 0.9157304 0.4017705 0.004292905 0.949701 0.3128864 -0.01304507 0.5568813 0.830513 0.01146155 0.6272734 0.7787879 -0.004222989 -0.06692099 0.9976107 0.01716703 0.1038169 0.9944517 -0.01696997 -0.5898844 0.8074666 -0.005864024 -0.5872735 0.8093727 -0.00509572 -0.9145323 0.4044908 0.004238367 -0.9326366 0.3607922 -0.004258155 0.9002705 -0.4304938 0.06471812 0.6720718 0.6971331 0.2496502 0.9106116 -0.4079752 0.06590139 0.8399976 -0.5038996 0.2012194 0.8792808 -0.47445 0.04198223 0.8783352 -0.4760474 0.04365849 0.6948216 0.7163676 0.06356596 0.6601303 0.7095329 -0.2465586 0.718699 0.6933503 0.05231934 0.6961555 0.7159805 0.05234134 0.6830025 0.7275791 0.06431418 0.7177813 0.6644255 0.2081562 0.8594496 -0.5085325 0.05235522 0.71878 0.6932618 0.05237865 0.7974607 -0.5610994 0.2218648 0.7770994 0.6279742 0.04201203 0.8408602 -0.5387167 0.05233055 0.7428314 0.667431 0.05232071 0.7652337 0.6416226 0.05232387 0.7426539 0.6676116 0.05253636 0.8397777 -0.5407332 0.04879575 0.7645715 0.5948526 0.248155 0.8199352 -0.5566868 0.1334392 0.8233011 -0.565186 0.05234754 0.7016843 0.6219813 -0.3475319 0.7875324 0.6140495 0.0523082 0.7723029 -0.6078224 0.1846631 0.8148497 -0.5639029 0.1342892 0.8043844 -0.59188 0.05141907 0.7995609 0.5141761 0.3103636 0.8047306 -0.5913296 0.05232656 0.8092185 0.585174 0.05231547 0.8092202 0.5851722 0.05230873 0.82945 0.5561265 0.0523079 0.8294754 0.5560861 0.05233389 0.8365148 0.4520219 0.3097081 0.8645005 0.5015369 0.03316038 0.7660494 -0.6407064 0.05161064 0.8615553 0.505778 0.04371929 0.8490794 0.5256665 0.05233722 0.8044814 0.515797 -0.2945559 0.8777646 0.4770541 0.04414594 0.7667237 -0.6397613 0.05329602 0.869098 0.388182 0.3065676 0.9007766 0.4311262 0.05226838 0.8847173 0.4631856 0.0522927 0.7125828 -0.7009807 0.02918905 0.7231821 -0.6886684 0.05237942 0.7828327 0.4770219 -0.3995286 0.9008219 0.4310239 0.05232971 0.9040721 0.3511334 0.2436367 0.9147292 0.4006643 0.05233353 0.925506 0.3729421 0.06597566 0.7997148 0.4262704 -0.4227884 0.9284627 0.3677212 0.05232816 0.940758 0.3350155 0.05233514 0.6780373 -0.7332789 0.05067062 0.9225911 0.3800923 0.06599748 0.9275927 0.2888464 0.236938 0.6477624 -0.7613381 0.02771931 0.9434577 0.3267118 0.05609709 0.9522376 0.3008414 0.05232745 0.962105 0.2676095 0.05233669 0.9472464 0.2218479 0.2313179 0.9511498 0.3036654 0.05569118 0.6286215 -0.7761022 0.0500046 0.9696348 0.240956 0.04181641 0.9702728 0.2391128 0.0373612 0.9722155 0.2303344 0.04174941 0.8944432 0.2910593 -0.3394936 0.573202 -0.8179967 0.04817539 0.5755882 -0.8160539 0.05248236 0.02354431 0.9983556 0.05226677 0.9042969 0.2349627 -0.3564262 0.5539678 -0.8228332 0.1267492 0.54803 -0.8348233 0.05228066 0.07441216 0.9958564 0.05227816 0.9849245 0.1648641 0.05237889 0.9897884 0.1325654 0.05239689 0.9831727 0.1717808 0.06215125 0.9686383 0.1137016 0.2209342 0.5162523 -0.8552503 0.0450623 0.5209596 -0.8519787 0.0522843 0.02487421 0.9982661 0.05334866 0.9922096 0.1103734 0.05777639 0.09157466 0.9934876 0.06779831 0.4849565 -0.8539365 0.1887057 0.1990933 0.9786455 -0.05113726 0.4908813 -0.869655 0.05230474 0.9938533 0.09761822 0.05221551 0.1686995 0.9842767 0.05234402 0.1186276 0.9903772 0.07127785 0.1384062 0.9288668 0.3435844 0.1328931 0.9897488 0.05231529 0.9932225 0.1009303 0.05763643 0.4573814 -0.8879415 0.04860246 0.9844534 0.04963111 0.1684882 0.7675173 0.6333186 0.09912002 0.929581 0.1645817 -0.3298366 0.9210008 0.1707088 -0.3501658 0.9965977 0.06365031 0.05236035 0.2060928 0.9772236 0.05059468 0.9966511 0.06294655 0.05219507 0.4599641 -0.8863972 0.05227875 0.9979914 0.03591758 0.05218476 0.1790901 0.9798328 0.08862578 0.4435368 -0.8876861 0.1236476 0.4315661 -0.9005608 0.05235379 0.274037 0.9603976 0.05040025 0.2713595 0.9609607 0.05402457 0.9980046 0.0309506 0.05503553 0.4028064 -0.9137882 0.05232733 0.9985907 0.008660376 0.05236154 0.3910496 -0.9196428 0.03656959 -0.3403139 0.7527414 0.5635308 0.4288522 -0.9026097 0.0371688 0.33801 0.9396899 0.05227023 0.3742788 -0.9098311 0.1792287 0.3326799 -0.9420354 0.0435127 0.3704321 -0.9273842 0.05233192 0.3406221 -0.9387436 0.05231744 0.9980334 -0.02758729 0.0562871 0.3380728 0.9396634 0.0523408 0.9975728 -0.03347957 0.06105381 0.4023779 0.9139534 0.05273836 0.9979857 -0.01721805 0.06105792 0.9797737 -0.1234135 0.1575203 0.9947696 -0.09126764 0.04586583 0.9946417 -0.09223389 0.04670053 0.4026706 0.9138485 0.05232089 0.3158401 -0.9330738 0.1720998 0.3069889 -0.9502595 0.05258101 0.9904616 -0.1274654 0.05233067 -0.238548 0.8702244 0.4310503 0.276499 -0.9595812 0.0524621 0.2762331 -0.9596722 0.05219846 0.9534725 -0.208197 0.2180463 0.9054229 0.01135253 -0.424359 0.4587326 0.8870339 0.05229914 0.4373282 0.8918182 -0.1157784 0.9857138 -0.1601004 0.05231022 0.2511272 -0.9529887 0.1695517 0.203693 -0.9779837 0.04535573 0.2122788 -0.9758065 0.05234116 0.2424967 -0.9687361 0.05239915 0.2427775 -0.9686598 0.05251032 0.9857084 -0.1601271 0.05232906 0.1908958 -0.9805615 0.04536646 0.9797887 -0.1930564 0.05237811 0.9797643 -0.1931881 0.05234962 0.9426644 -0.2634255 0.2049167 0.9421793 -0.09203636 -0.3222231 0.1803829 -0.9620873 0.2045729 0.1780925 -0.9826363 0.05204856 0.1464865 -0.9878038 0.05277806 0.9573212 -0.2857142 0.04362988 -0.1188752 0.8450973 0.5212287 0.9731243 -0.224249 0.05236005 0.1293735 -0.9907039 0.04205143 0.4482801 0.8929511 0.0410313 0.9703627 -0.2341539 0.05973386 0.1247574 -0.9727998 0.1951825 0.9709081 -0.2318863 0.05971878 0.9196221 -0.3300172 0.2130351 0.07136982 -0.9963449 0.04693776 0.4956516 0.8667427 0.05555862 0.4962148 0.8664013 0.0558542 0.959354 -0.2788185 0.04359191 0.1120697 -0.9923075 0.05259573 0.4796962 0.8537691 -0.2024099 0.5007212 0.8606271 -0.09273314 0.9555795 -0.2900528 0.05231946 0.08097273 -0.9953374 0.05240994 0.9452109 -0.3222376 0.0523377 0.06719774 -0.990306 0.1215673 0.02473556 -0.9983283 0.05223906 0.05698895 -0.9970437 0.05153864 0.02463024 -0.9983264 0.05232524 0.8906147 -0.3929113 0.2289679 0.5297426 0.8443623 0.08015716 0.9350695 -0.351651 0.04457288 0.5281621 0.8450858 0.08291476 0.5501834 0.8334016 0.0523464 0.9350039 -0.3518068 0.04471886 -0.5995061 -0.7997573 -0.03132086 0.5704534 0.8087672 0.1431033 0.585784 0.8093634 0.0422855 0.5676027 0.8216368 0.0523473 0.9213427 -0.385212 0.05233943 0.5988933 0.7990127 0.05390477 0.9082342 -0.4151782 0.0523234 0.8682501 -0.4468614 0.2155378 0.6465535 0.7617107 0.04201817 0.648818 0.7600125 0.03763192 -0.3845726 0.6640674 0.6411852 0.9169614 0.3989524 0.004343926 0.949666 0.3129944 -0.0130043 0.5569067 0.8304963 0.01144695 0.6272413 0.7788137 -0.004224598 -0.06691968 0.9976018 0.01767838 0.1478394 0.988906 -0.01444631 -0.5898255 0.8074514 0.0113312 -0.444381 0.8956623 -0.01774054 -0.9145026 0.4045659 -0.003399074 -0.9360233 0.351561 -0.0162903 -0.6253089 0.6603691 -0.4158142 -0.2307677 0.09296661 -0.9685575 -0.3544115 -0.0779711 0.9318332 -0.34791 -0.07003879 0.9349082 -0.1098471 -0.07620704 0.9910228 -0.3301656 -0.07000434 0.9413236 -0.2176781 0.1743258 0.9603264 -0.0376082 0.2071485 0.9775865 -0.03489887 -0.06255805 0.997431 0.04225724 -0.06427568 0.9970372 0.09800428 0.1252205 0.9872766 0.3877533 0.1525536 0.9090515 0.2201011 -0.05510085 0.9739196 0.1733962 -0.05560964 0.9832809 0.5400508 -0.0362854 0.8408499 0.220043 -0.08503544 0.9717768 0 -0.4582571 0.8888197 -0.219245 -0.08504664 0.9719561 -0.01048028 -0.7294192 0.6839868 -0.5775677 -0.2090934 0.7891106 0.2188229 0.130141 -0.967047 0.5379307 0.1753478 -0.8245506 0.00554651 -0.7839758 0.6207666 -0.3262508 -0.4069272 -0.8532121 0.1259861 -0.06582212 -0.9898459 0.7160338 -0.0788477 0.6935985 -0.3929542 0.0776332 -0.9162752 0.7787851 -0.3331998 0.5314809 0.4622436 -0.07614564 0.8834777 0.7268854 -0.06009167 0.6841248 0.7252044 -0.1266782 0.6767801 0.1169911 -0.0869224 0.9893218 0.2884809 -0.1910161 0.9382386 0.3017992 -0.4186496 0.8565337 0.09303522 -0.2555853 0.9622997 0.0408197 -0.3373721 0.940486 0.03333443 -0.1689971 0.9850527 -0.03229612 -0.4272562 0.9035537 -0.00804615 0.1615657 -0.9868292 -0.0421819 -0.2740799 0.9607814 -0.216055 -0.2436837 0.9454832 -0.8596166 -0.134225 0.4929939 -0.5628281 -0.3705488 0.7388628 -0.357855 -0.00293672 0.9337726 -0.4640295 0.05368125 0.8841917 -0.4903407 0.159765 0.856762 -0.2000492 0.04546904 0.9787303 -0.2955124 -0.3745946 0.8788353 -0.9194111 0.351898 0.1756452 -0.1412186 -0.2259947 0.9638381 0.2139542 0.1321055 0.9678698 0.2717269 0.1905837 0.9433146 0.01700073 0.1021485 -0.9946239 0.3343051 0.1151587 0.935403 0.1808562 -0.02399575 0.9832168 0.4909347 -0.05793362 0.8692681 0.2258671 -0.01851075 0.9739823 0.2777447 -0.213122 0.9367161 -0.1082757 0.1481169 -0.9830248 0.2247812 -0.09674668 -0.9695945 0.1687715 0.08271646 -0.9821783 -0.2559035 -0.408362 0.8762157 0.44992 0.8872671 0.1016326 -0.9915508 3.37488e-4 0.1297194 -0.8743549 -0.4740843 0.1036711 -0.8777893 -0.4662567 0.1099572 -0.5312967 -0.836269 0.1355658 -0.615983 -0.7824439 0.09135937 -0.1582315 -0.9847595 0.07219195 -0.03595662 -0.9903947 0.1335124 0.3173097 -0.9443174 0.08705943 0.4273713 -0.8902155 0.1577035 0.6429826 -0.7575767 0.1124778 -0.7225723 0.278451 0.6327356 0.006998002 0.02806621 0.9995816 -3.72273e-6 0 -1 -0.01213747 -0.07833045 0.9968537 -0.007145345 0.01258373 0.9998953 0.2769262 -0.1781302 0.944236 0.5138805 -0.4119129 0.752499 0.7294722 -0.5044972 0.4619016 -0.2505063 0.2535278 0.9343289 -0.5599478 0.1920788 0.8059554 -0.2049384 0.649043 0.7326278 -0.01640963 0.4180341 0.9082832 -0.01349598 0.001540005 0.9999077 -0.01300477 0.01031988 0.9998622 -0.0161159 0.01766717 0.9997141 9.01523e-5 0 1 -1.775e-6 0 1 -0.007638514 -0.02284872 0.9997099 -1.83786e-5 0 1 -0.0128448 -0.0188921 0.9997391 6.24262e-6 0 1 -0.05447649 -0.0218721 0.9982755 -0.02922648 -0.01731634 0.9994228 -0.0025388 0.01530754 0.9998797 -0.006376385 0.01137393 0.999915 -0.01750963 -0.3096526 0.9506886 -0.5343602 0.7079514 0.4618052 -0.5349892 0.7120702 0.4546896 -0.5662896 0.6663408 0.4850836 -0.7683402 -0.3981761 0.501108 -0.8669772 -0.3587183 0.3459359 -0.7137992 -0.3973448 0.5767217 0.9034222 0.2864115 0.3190562 0.9204474 0.3593389 0.1537923 0.2962032 -0.90894 0.2934144 -0.8989451 -0.3829215 0.2127647 0.7343716 0.1051675 0.6705507 0.8626132 0.3210703 0.3909125 0.2520391 -0.7050369 0.662872 -0.6431825 -0.444718 0.6233316 0.8821637 0.4108115 0.2302636 0.3503562 -0.8813237 0.3170476 -0.6902528 -0.4621652 0.5567356 -0.851219 -0.4547647 0.2619453 0.8067941 0.3485899 0.4770415 -0.8281425 -0.506786 0.2394744 0.4292935 -0.8766161 0.2173738 0.4221121 -0.8712802 0.2503842 0.2654908 -0.7265869 0.6337083 -0.8404495 -0.5028827 0.2018757 0.8186226 0.5377058 0.2018156 0.352609 -0.7631646 0.5415226 0.434297 -0.8240534 0.3637612 0.7492666 0.4599975 0.4764471 0.5709508 -0.8135564 -0.1101866 -0.6557425 -0.5188955 0.5484063 -0.5854749 -0.5619627 0.5843092 -0.8097999 -0.5522058 0.1982244 0.2544655 -0.6008321 0.7577917 -0.004371881 0.3274047 -0.9448742 0.4940643 -0.8028206 0.3337361 -0.5625205 -0.5746368 0.5944437 0.7653457 0.5494282 0.3352233 0.3686956 -0.7384055 0.5646424 -0.5763475 -0.5580871 0.5969609 0.5573727 -0.7801845 0.2839856 0.4036732 -0.6975579 0.5919976 0.5445274 -0.7729085 0.3257337 -0.7335851 -0.6155098 0.2880984 0.6915272 0.4727085 0.5462023 -0.5022545 -0.5791457 0.6421298 -0.5461032 -0.600711 0.5838816 -0.6904315 -0.661659 0.2924242 0.708012 0.5447652 0.4493883 0.6227034 -0.7420724 0.2481311 0.4710645 -0.7029481 0.5328811 0.6232919 0.5350489 0.5702893 -0.4906008 -0.6159352 0.6163886 -0.5229867 -0.6247276 0.579828 -0.635326 -0.7062411 0.3123849 -0.6993982 -0.662746 0.2676002 0.5116364 -0.6611005 0.5487936 0.5253698 -0.6722352 0.521619 -0.4459995 -0.6414173 0.6242343 -0.6067466 -0.7531348 0.254257 -0.6530196 0.5790007 0.4881841 -0.6217981 -0.7022681 0.3466796 0.5731782 -0.635598 0.5171867 -0.5034509 -0.7042734 0.5005359 -0.6574897 0.4657532 0.5922679 0.6797046 -0.6210562 0.3902447 -0.6344386 -0.7723942 -0.02991682 -0.7222452 0.4421563 0.5318455 -0.3864488 -0.7096329 0.5891337 -0.4381696 -0.773974 0.4571345 0.7511659 -0.5693553 0.3340425 0.783014 -0.5687922 0.2517234 -0.4612821 -0.8456169 0.2686091 -0.7743688 0.6152397 -0.1477601 -0.280728 -0.1243876 0.9516929 0.7532131 -0.5227285 0.3992809 -0.473581 -0.8535301 0.2172728 0.8393423 -0.4912176 0.2328302 -0.2753852 -0.6856937 0.6737857 -0.8308737 0.4429027 0.3368772 -0.3704043 -0.8465632 0.3822714 0.476945 -0.4868403 0.7317855 0.7429121 -0.4697514 0.4768809 -0.3977754 -0.8826974 0.2502401 -0.2885856 -0.7702735 0.5686804 -0.320411 -0.8712995 0.3717178 -0.8802129 0.4259631 0.2092389 0.8110001 -0.4479092 0.3763728 0.9522265 -0.3029586 -0.03848326 -0.2385975 -0.775265 0.5848378 -0.2481566 -0.8580533 0.4496254 -0.328419 -0.8848041 0.330549 0.6543247 -0.4120835 0.6340714 -0.7800213 0.1698884 0.6022498 -0.2674703 -0.9269158 0.2632237 -0.2140988 -0.9173184 0.3356913 -0.9111158 0.2506689 0.3271591 -0.8537443 0.1636136 0.494319 0.8697675 -0.3202224 0.3754495 -0.2203648 -0.9417218 0.2541642 -0.1561484 -0.9165151 0.3682634 -0.9497013 0.219479 0.2233753 0.8603751 -0.2714127 0.4313812 0.931478 -0.2487457 0.2654702 -0.161192 -0.9567857 0.2420298 -0.09869223 -0.918243 0.3835228 -0.09724682 -0.9636366 0.2488931 0.9441275 -0.1810742 0.2753826 0.883698 -0.2074627 0.4195678 -0.05266803 -0.9214859 0.3848246 -0.9364067 0.04555827 0.3479469 0.962098 -0.09714853 0.2548128 -0.02239835 -0.9996904 0.01083439 -0.7305502 -0.1497964 0.6662263 -0.9746329 0.0193147 0.2229747 -0.01358258 -0.9877653 0.1553558 0.988182 0.03463953 0.1493204 -0.7961893 -0.1681979 0.5811987 -0.008168995 -0.7484884 0.6630976 0.03364491 -0.9002644 0.4340416 0.7256279 -0.1671538 0.6674756 0.9112707 -0.008472263 0.4117208 -0.9022384 0.1287122 -0.4115815 -0.6559514 -0.2533531 0.7110133 0.08129924 -0.9488766 0.3049978 0.08030354 -0.9964191 0.02646416 0.9371131 0.02482724 0.3481417 -0.7957357 -0.2230166 0.5630882 0.9751503 0.1676777 0.1447972 0.1448316 -0.9443945 0.2951996 -0.7326872 -0.3084954 0.6066302 0.847309 0.02857041 0.5303312 0.9120398 0.1274212 0.3898044 -0.8137629 -0.3161848 0.4876651 0.9527975 0.2707941 0.1372862 0.8880501 0.1684069 0.4277923 0.6086674 0.7633057 0.2165372 0.5897967 0.5895287 0.5519021 0.5915217 0.5924658 0.5468881 0.5124495 0.6637911 0.5447726 0.01820194 -0.586648 0.8096375 0.1212779 -0.8146289 0.5671609 -2.31614e-5 0 1 0.003539144 9.68915e-4 0.9999933 1.2307e-5 0 1 5.79861e-4 0.005383789 0.9999853 0.002002894 0.002659022 0.9999945 0.04196327 -0.1111184 0.9929209 0.07526689 -0.06138938 0.995272 0.3847855 -0.7385595 0.5535972 0.2562583 0.09424287 0.9620031 0.08326804 0.2047482 0.9752664 0.03260475 0.1494825 0.9882267 0.08925938 -0.03883731 0.9952509 0.08444792 -0.1288911 0.9880565 0.9728177 0.001142323 0.2315694 0.02987313 0.1519179 0.9879416 0.9329378 -0.1019262 0.3453089 0.0779671 -0.0928986 0.9926183 0.9093557 -0.2353674 0.3430373 0.2470211 0.1218417 0.9613196 0.3714486 -0.3710615 0.8510813 0.621304 0.697807 0.3564363 -0.2356737 -0.9633432 0.128172 0.06493759 -0.9910102 0.1169695 -0.08155715 -0.9941946 0.07018256 0.37735 -0.918603 0.1173689 0.4419407 -0.8931955 0.08300775 0.740404 -0.6639687 0.1046308 0.7537553 -0.6464909 0.1179087 0.9353274 -0.3306654 0.1257899 -0.7084775 -0.5942453 0.3806996 -0.3885439 -0.3185617 0.864611 0.008285641 -0.009223043 0.9999232 -4.30271e-5 -9.0179e-4 0.9999996 0.1194887 0.01901423 0.9926535 0.006715953 -0.007440149 0.9999499 -0.02081853 -0.009454905 0.9997386 -5.53207e-4 -6.75807e-4 0.9999997 0.008424997 -0.002774596 0.9999607 0.008856594 -0.01194763 0.9998894 0.1220339 -0.06158477 0.9906135 0.675724 0.3284795 0.659923 -0.3768326 0.2637564 0.8879356 -0.7101239 0.4900404 0.5055538 0.05252194 0.06520533 0.9964888 -1.99817e-4 -3.99842e-5 1 -5.13442e-6 0 -1 -2.07353e-4 -4.13946e-5 1 -3.35659e-4 1.3354e-4 1 0.4595103 0.5991374 0.6556559 0.1641155 -0.07653129 0.9834679 0.1770933 -0.08406114 0.9805977 0.5458961 -0.2562419 0.7977078 0.8526244 -0.3862541 0.3519083 0.4196727 0.8795389 0.224246 0.4258344 0.8642581 0.2678114 0.3479205 0.8997524 0.263433 0.2939171 0.7211943 0.6272891 0.3017787 0.7656497 0.568076 0.2777598 0.9269002 0.2523999 0.2038445 0.9373043 0.28268 0.1715754 0.5690305 0.8042177 0.1107931 0.5290181 0.8413471 0.1142352 0.5427372 0.8320977 0.1360322 0.9121332 0.386663 0.06037777 0.5191841 0.852527 0.04018682 0.8236515 0.5656706 0.03806996 0.7947142 0.6057889 0.04044795 0.7367123 0.6749956 0.0393694 0.6933025 0.7195706 -0.03891998 0.9126083 0.4069781 -0.07256305 0.8129331 0.5778187 -0.069857 0.7099276 0.7008016 -0.09777837 0.4696854 0.8774024 -0.1404135 0.5969037 0.7899304 -0.1474746 0.4510741 0.8802179 -0.2085149 0.4807257 0.8517185 -0.2086133 0.6126461 0.7623289 -0.3002212 0.6113553 0.7321968 -0.2889093 0.6647049 0.6889839 -0.4363685 0.8628127 0.2552192 -0.3354096 0.6769423 0.6551715 -0.3859643 0.6423736 0.6621086 -0.3635185 0.6071351 0.7065702 -4.47109e-6 0 1 -0.01583874 -0.002746582 0.9998709 -0.02106022 -0.02883911 0.9993622 2.19645e-5 0 -1 -0.02310878 -0.009125232 0.9996914 1.05752e-5 0 1 -0.07021206 0.0263791 0.9971833 -0.2209432 -0.1076353 0.9693291 -0.286659 -0.1303036 0.94913 -0.6093264 -0.2826021 0.7408491 -0.8536407 -0.4112152 0.3196867 0.5209168 0.3721975 0.7681892 0.7106193 0.4967747 0.4982321 0.6281495 0.1169107 0.7692594 0.83815 0.006048798 0.5454064 0.83939 -0.04735064 0.5414631 0.8943415 0.3928188 0.2141188 -0.005500793 -0.7914492 0.6112102 0.007548928 -0.6591265 0.7519943 0.8942577 0.44606 0.03651845 -0.7548912 -0.05690556 0.6533766 -0.83322 -0.04819715 0.5508371 -0.8246157 -0.1907557 0.5325613 0.6850571 -0.4959655 0.533587 -0.2803295 0.1936325 0.9401712 -0.2480255 -0.5763248 0.778674 -0.5986036 0.5315639 0.599261 0.5914908 0.5478633 0.5915949 -0.8872671 0.3593784 0.2891442 -0.8957616 -0.3765685 0.2362357 -0.8231019 -0.5663379 0.04200923 -0.5657368 -0.7916536 0.2307088 -0.2299419 -0.9571362 0.1761168 -0.5209249 -0.6808994 0.5147945 0.2622784 -0.73817 0.6215426 -0.4759835 -0.6773325 0.5609461 0.05790024 -0.7376785 0.6726649 0.08857303 -0.856468 0.5085443 -0.2112085 -0.3775751 0.9015698 -0.492641 -0.5959005 0.6341983 0.05080056 -0.9688263 0.2424769 -0.1232625 -0.4940571 0.8606474 -0.7609187 -0.2931438 0.5788519 -0.2039147 -0.7926735 0.5745324 -0.6657938 -0.3785789 0.6429593 -0.1267225 -0.5094035 0.851146 -0.2286135 -0.6027687 0.7644644 0.1372246 -0.3191159 0.9377284 -0.3584556 -0.5428058 0.7595206 -0.7042264 0.1698199 0.6893667 -0.6451901 -0.1637813 0.746261 -0.07149368 -0.144155 0.9869691 -0.07388353 -0.1478354 0.9862484 -0.2064635 -0.4166668 0.8853032 -0.2454556 -0.4711238 0.8472272 -0.3275091 -0.6483222 0.6873253 0.07899022 -0.6813938 0.7276422 0.3971875 -0.4996945 0.7697712 -0.2660716 -0.4496219 0.85267 0.2265863 -0.2452969 0.9425966 -0.6498958 0.1563298 0.7437718 -0.1196046 -0.01955235 0.9926291 -0.2201454 0.7000634 0.6792991 -0.3273997 -0.01706612 0.9447319 -0.5189006 -0.01873409 0.8546293 -0.4054452 0.3558194 0.8420255 -0.3363539 0.4375439 0.8339193 -0.5099737 0.3857724 0.7688345 -0.5501399 0.4212323 0.7210476 -0.6229016 0.7567086 0.1984587 -0.2326356 0.2474852 0.9405487 -0.6441691 -0.1073389 0.757314 -0.6150282 -0.1472909 0.7746263 -0.2516804 0.2148973 0.9436504 -0.6946426 -0.104314 0.7117516 -0.2334614 -0.04015171 0.9715368 -0.1525365 -0.01915848 0.9881121 -0.4888657 -0.05372524 0.8707031 -0.395316 -0.6342341 0.6644339 -0.3563942 -0.6169853 0.7016497 -0.05228638 -0.8518993 0.5210891 -0.348754 -0.6398696 0.6847901 -0.5481612 -0.3075708 0.7777659 -0.1463086 -0.2576137 0.9551068 -0.1795839 -0.1945292 0.9643175 -0.382808 -0.5842807 0.7155936 -0.5544594 -0.4859785 0.6755737 -0.3873627 -0.5827387 0.7143989 -0.4752155 -0.3053301 0.8251932 -0.2399218 -0.2771615 0.9303866 -0.4317404 -0.2107947 0.8770211 0.2167717 0.4991282 0.8389762 0.1170418 0.3597705 0.9256708 0.1814041 0.1677399 0.9689974 0.7111537 0.4787803 0.5148106 0.3761205 0.2424311 0.8942933 -0.100272 0.5240818 0.8457444 0.08658647 0.586227 0.8055066 -0.362522 0.5648714 0.7412815 -0.4871927 0.8606971 0.1477966 -0.3847907 0.669016 0.6358882 -0.2198427 0.3332522 0.9168491 -0.1101448 0.198419 0.9739087 -0.5538542 0.5899691 0.5875219 0.3879489 -0.9173938 0.08879309 0.5275257 -0.8258243 0.1993265 0.839918 -0.5308706 0.112758 0.6564599 0.7543609 4.41524e-4 0.6576901 -0.6929642 -0.2953719 0.166357 -0.9829374 0.07848328 0.8017995 -0.5910033 0.08850306 0.135575 -0.987652 0.07850551 0.784769 -0.6148778 0.07786494 0.06812363 -0.9945526 0.07889533 0.1002192 -0.991869 0.07843559 0.1367159 -0.9874988 0.07845407 0.1708962 -0.9652674 0.1976193 0.7268988 0.6709598 0.1463937 0.7331367 0.6771902 0.06264215 0.7707974 0.6296759 0.09684991 0.1001677 -0.991875 0.07842576 0.572786 -0.6003404 -0.5581287 0.7794642 0.6241532 0.05355858 0.8119626 0.5724574 0.114059 0.8160739 0.5713005 0.08740323 0.7610834 -0.6432614 0.08346837 0.8664155 0.4941033 0.07201647 0.8616516 0.4922889 0.1233218 0.1053009 -0.9844603 0.1405339 0.8545157 0.5153136 0.06522887 0.8843379 0.4583461 0.08868712 0.8922284 0.4394029 0.1041805 0.8873135 0.4553517 0.07300472 0.9060669 0.414696 0.08408367 0.643517 -0.7614019 0.07844197 0.03565311 -0.989102 0.1428505 0.9144488 0.3880686 0.1148315 0.2533383 0.4096481 -0.8763608 0.9196197 0.3844017 0.08083873 0.03190678 -0.9964116 0.07839661 0.9322876 0.3531098 0.07844257 0.918779 0.3868975 0.07845771 0.9439848 0.3205361 0.07841855 0.02062773 0.3356626 -0.9417564 0.9322445 0.3532135 0.07848906 0.7144787 -0.6951321 0.07944619 0.9395755 0.3226962 0.1143032 0.7168794 -0.6927559 0.07856976 0.9440357 0.3203527 0.07855564 0.5311489 -0.6719816 -0.5160635 0.9544736 0.28779 0.07846736 0.9543799 0.2880709 0.07857567 0.6697332 -0.7385295 0.07766318 0.964742 0.2512379 0.0784384 0.9602757 0.2526382 0.1185103 0.6191945 -0.7813132 0.07840937 0.9783836 0.1928716 0.07460814 0.9650979 0.2494683 0.07969707 0.9750383 0.208305 0.07687193 0.9789754 0.1870488 0.08136272 0.6645093 -0.7430901 0.07902288 0.9792541 0.1868835 0.07833313 0.9850146 0.1536412 0.07836347 0.9868078 0.1582552 -0.03414142 0.9890782 0.1267772 0.07517933 0.5096362 -0.8568082 0.07842725 0.9880548 0.1128591 0.1049318 0.4464527 -0.6003666 -0.6635059 0.9931595 0.08521181 0.07983326 -0.9734998 -1.87336e-4 -0.2286879 0.4785846 -0.87453 0.07844889 0.9923972 0.08149158 0.09223365 0.9901404 0.1126276 0.08328896 0.9953538 0.05387586 0.07980102 -0.99647 0.04662084 -0.06981468 0.9944455 -0.05185085 0.09159582 0.5646567 -0.8215904 0.07843476 0.5365215 -0.8402313 0.07846164 0.996778 -0.01600074 0.07859826 0.9956659 -0.04969555 0.0786122 0.618789 -0.7816237 0.07851499 0.9952666 0.05741703 0.07840681 0.9937261 -0.01063728 0.1113349 0.9967579 0.01793771 0.0784353 0.5918831 -0.8022814 0.07758265 0.9927179 -0.09457033 0.0746178 0.3839188 -0.5976737 -0.7038413 0.995725 -0.04875004 0.0784555 0.9906976 -0.07780426 0.1116464 0.9933746 -0.08398938 0.07843989 -0.1157261 -0.3128998 0.9427096 0.9898759 -0.118276 0.07846361 0.9848433 -0.1546936 0.07844489 0.9588018 -0.2417507 0.1491837 0.9829737 -0.1461206 0.1114073 0.3798102 -0.9057279 -0.1881526 0.9787711 -0.1893393 0.07847118 0.9735763 -0.2118483 0.08526253 0.9723643 -0.2198908 0.07845979 0.4505384 -0.8893048 0.07843631 0.9382992 -0.3333842 0.09192258 0.9140962 -0.3480129 0.208123 0.3586303 -0.9301792 0.07842957 0.9838665 -0.1599149 0.08021241 0.9713048 -0.2241259 0.0795902 0.9643263 -0.25291 0.07817608 0.5632218 -0.8226101 0.0780636 0.5521872 -0.8233635 0.1310034 0.9713563 -0.2144073 0.1024534 0.9805158 -0.179359 0.08012092 0.4180498 -0.9050307 0.07844686 0.3318906 -0.9400181 0.07883322 0.5367203 -0.8401052 0.07845222 0.9554218 -0.2846586 0.07835072 0.9471179 -0.3111686 0.0783711 0.3284856 -0.9412472 0.07842755 0.289665 -0.9569239 0.01977342 0.2895829 -0.9538562 0.07937383 0.9549326 -0.2813556 0.09456592 0.9367595 -0.3410903 0.07835257 0.9465472 -0.3128746 0.07847303 0.9103824 -0.4066522 0.07640635 0.2765303 -0.9577158 0.07944595 0.5096389 -0.8568065 0.07842779 0.4959355 -0.8602268 0.1185663 0.9251503 -0.3712034 0.07940441 0.9361433 -0.3381621 0.09634393 0.2980816 -0.9513148 0.07840687 0.9368639 -0.3407813 0.07844948 0.3866627 0.1256912 -0.9136158 0.4784762 -0.8745884 0.07845836 0.2222741 -0.9713262 0.08437746 0.9127 -0.4010386 0.07840126 0.2669855 -0.9605058 0.07840591 0.2222667 -0.9717076 0.07988607 0.2234311 -0.9714242 0.08008557 0.911532 -0.398251 0.1024972 0.3888392 -0.9179608 0.07843559 0.4505915 -0.8892771 0.07844549 0.4313206 -0.892275 0.133446 0.8982856 -0.4323512 0.07845753 0.3586964 -0.9301531 0.07843679 0.8082857 -0.5836881 0.07734686 0.8859685 -0.4570629 0.07844376 0.1510558 -0.9855097 -0.07715523 0.4179584 -0.905072 0.07845693 0.1555048 -0.9845692 0.08026093 0.8833349 -0.4570689 0.1039596 0.8040202 -0.5892888 0.07931226 0.7528033 -0.6530982 0.08215796 0.7839381 -0.6154932 0.08129709 0.3691206 -0.9195066 0.1351204 0.3889848 -0.9178975 0.07845515 0.8688777 -0.4887806 0.0783922 0.8786457 -0.470479 0.08143424 0.8409679 -0.5353724 0.07841813 0.8585485 -0.5065991 0.07906866 0.8235747 -0.5617566 0.0784493 0.8779085 -0.4718546 0.08142548 0.8616597 -0.4948382 0.1125959 0.840928 -0.5354384 0.078395 0.03209614 -0.9964085 0.07835775 0.7614812 -0.6433794 0.07880055 0.3071266 -0.939808 0.1497807 0.8345506 -0.5408975 0.1046675 0.2326912 -0.9693821 0.07844233 0.2399496 -0.9535778 0.1819717 0.2019062 -0.9762577 0.07845258 0.5086638 0.6430608 0.5724806 0.170722 0.2492364 0.953276 0.3392645 0.5674601 0.7502591 0.33445 0.5334813 0.776879 0.08204734 0.139958 0.9867523 0.4685389 0.7754529 0.4232541 0.2803928 0.5311886 0.7995115 -0.08059298 0.1255732 0.9888055 -0.1294311 -0.2000774 -0.9711935 -0.2808352 0.1597381 0.9463697 0.1035119 0.3287838 0.9387153 0.1745002 0.3411954 0.9236534 -0.04556536 0.5425458 0.8387896 -0.4941847 0.5108123 0.7034574 -0.5060009 0.5052692 0.6990466 0.32425 0.6378106 0.6986126 0.4303994 -0.5045136 0.7484802 0.3019142 -0.3902854 0.8697845 0.4263685 -0.5071177 0.7490271 0.5264532 0.2372689 0.8164255 0.5682666 -0.05832654 0.8207747 0.3385217 -0.5776935 0.7427471 0.4370142 -0.5541498 0.7084749 0.1124287 -0.9324403 0.3433875 0.1993912 -0.03851419 0.9791629 0.4183597 -0.04499942 0.9071661 0.6452902 -0.09109491 0.7584869 0.2640572 0.01727086 0.9643524 0.5140675 0.02819222 0.8572863 0.3811989 0.3456704 0.8574377 0.3367763 0.4793354 0.8104439 0.09506595 0.7802349 0.61822 0.5073825 0.3100908 0.8039942 0.508978 0.3176815 0.8000125 0.6969326 0.6877736 0.2031069 0.3879451 0.1598072 0.9077226 0.4606064 0.08883827 0.8831476 0.3890874 -0.7462466 0.5401175 0.02487808 -0.6559501 0.7543943 -0.1425297 -0.6862028 0.71331 0.011568 -0.7467696 0.6649823 0.1826986 -0.3683506 0.9115586 0.3296056 -0.8409817 0.4290805 0.9865111 0.03211802 0.1605129 0.6926994 -0.2864828 0.6618875 0.7942916 -0.3214236 0.5155461 0.1483198 -0.4174804 0.8964995 0.6977241 -0.4980012 0.5149524 0.3106896 -0.4449923 0.8399131 0.03629451 -0.8350347 0.548999 -0.2660076 -0.9131535 0.308854 0.1071899 -0.6315782 0.7678668 0.6822428 -0.6632752 0.3075889 0.636315 -0.5847222 0.5031932 -0.5676191 -0.819208 0.08189666 -0.629416 -0.7229092 0.2850224 0.3190647 -0.4694188 0.8233127 -0.2297353 -0.7809482 0.5808112 0.1332536 -0.5662559 0.8133866 -0.1947659 -0.681259 0.7056574 -0.6141638 -0.3849136 0.6889445 -0.1811451 -0.7213069 0.6685079 0.2138103 -0.3988913 0.8917235 0.1823354 -0.362505 0.9139716 0.4135913 -0.2942398 0.8616061 0.2309109 0.00827986 0.9729397 0.7100639 -0.13793 0.690496 -0.2798438 0.160961 0.946456 -0.7105435 0.479461 0.5150196 0.4573019 0.3650769 0.8109216 -0.2952044 0.6355778 0.713369 -0.01627981 0.0688163 0.9974966 0.06921279 0.02280336 0.9973413 -0.4537434 0.3343635 0.8260254 -0.03060781 0.04217875 0.9986412 -0.03764265 0.03911179 0.9985256 0.1022561 0.08267253 0.9913168 0.6797078 0.2788723 0.6784008 0.006188154 0.05934441 0.9982184 0.7994757 0.2700775 0.5365601 -0.3134242 0.8102025 0.4953152 0.07065713 0.04320907 0.9965645 0.1717548 -0.01141774 0.9850736 0.4320119 -0.2355058 0.8705761 0.4631708 -0.2025838 0.8628051 0.6345841 0.1657 0.7548818 -0.2667885 0.1600115 -0.950379 0.7496303 0.06755888 0.6583998 0.4687965 0.209734 0.8580451 0.8011541 -0.3030202 0.5160726 0.6144267 0.5281729 0.5861001 0.2552834 0.06966841 0.964353 0.4065081 0.7113421 0.5733618 0.3713973 0.6472388 0.6656921 0.3035106 0.6691856 0.6782861 0.2473583 0.6298592 0.7362686 0.170665 0.6161122 0.7689469 -0.01567965 0.9995822 0.02428036 0.003578186 0.4745057 0.8802452 -0.06812518 0.6944378 0.7163206 -0.07739973 0.6439746 0.7611216 -0.1440225 0.7017861 0.6976775 -0.1319355 0.7866615 0.6031224 -0.1969014 0.831129 0.5200526 -0.2078459 0.647617 0.7330706 -0.2614122 0.7693749 0.5828603 -0.2658311 0.6457305 0.7157975 -0.3325797 0.7648271 0.551752 -0.3305962 0.8153516 0.4752978 -0.4285427 0.8274915 0.3627795 -0.6403356 -0.5191211 0.5661129 -0.5118662 0.5148628 0.6876841 -0.5522125 0.01510542 0.8335666 -0.6975609 -0.3365976 0.6325433 -0.5069572 0.2670732 0.8195525 0.4560426 -0.7309602 -0.5076637 -0.4762603 0.1490261 0.8665837 -0.8096653 -0.1142851 0.575657 0.4562749 -0.7350496 -0.5015132 -0.6749791 0.2618412 0.6898133 -0.460917 -0.03315657 0.8868237 0.5844274 0.5723694 0.5751851 0.5300404 0.4895118 0.6924127 0.4911242 0.7471884 0.4477795 -0.01692306 0.03410744 0.999275 0.0602532 0.05902379 0.9964366 -0.8788449 0.02737754 0.4763215 0.05187487 0.06442683 0.9965732 0.01450896 0.7467125 0.6649888 0.07348245 0.2958946 0.95239 -0.7932072 -0.3335825 0.5094558 -0.1918732 0.05153393 0.9800658 0.008404374 0.05613487 0.9983879 0.03571498 0.09284144 0.9950402 -0.200631 0.05596154 0.9780672 -0.07111185 0.02642333 0.9971184 -0.8685898 0.01166152 0.4953947 0.2278478 0.147178 0.9625093 0.4091423 0.2194049 0.8856998 -0.4336256 0.03126025 0.9005507 -0.2871764 -0.08030527 0.9545056 -0.6715027 -0.05422943 0.7390152 -0.5423986 0.09227347 0.8350385 -0.7974241 -0.04319113 0.6018716 -0.8010733 -0.05356156 0.5961652 -0.2894048 -0.08290708 0.9536096 -0.01802343 0.08858585 -0.9959055 -0.2581365 0.15066 -0.9542888 -0.3845446 0.3565536 -0.8514664 -0.6229246 0.4127739 -0.6645168 -0.301398 0.802568 0.5148242 -0.02868527 0.5544415 -0.8317283 0.06365585 0.2843919 -0.9565926 0.02786421 0.6188953 0.7849792 0.1169125 0.7075525 0.6969224 -0.3338742 0.9388079 0.08466339 0.6179059 0.2033976 0.7594879 0.6141763 0.2128936 0.7599104 0.6512955 0.230333 0.7230221 0.649074 0.6534638 0.3894714 -6.69949e-4 -0.9961186 -0.08801853 0.4455534 -0.08876562 0.8908439 0.7945957 0.1640484 0.5845561 0.6857362 0.1348949 0.7152406 -0.2085518 -0.02401858 -0.9777164 0.709483 0.2483832 -0.6594997 0.8292131 -0.04867011 0.5568097 0.5672375 -0.07173436 0.8204242 0.6657894 -0.06510519 0.743294 0.2063944 -0.0852881 0.9747447 0.2615556 -0.08421266 0.9615077 0 -0.3049467 0.9523694 -0.3807364 -0.08085197 0.9211422 -0.2387297 -0.08464401 0.9673901 -0.3781837 -0.1971008 0.9045045 -0.2992854 -0.196326 0.9337474 -0.05096369 -0.4061269 0.9123945 0.1457877 -0.6516156 0.7444079 0.801474 -0.1845593 0.5688387 0.1503198 -0.09057503 0.9844797 0.8087857 -0.05125415 0.5858657 0.3172038 -0.08293187 0.9447243 0.7240968 -0.05953335 0.6871242 0.2203361 -0.08498585 0.9717147 0.07072103 0.0600478 -0.9956871 0.7225458 -0.1260979 0.6797257 0.9327332 -0.2262905 0.2807163 0.2199892 -0.1251951 0.9674353 0.06496864 -0.1181403 -0.9908693 0.1275486 -0.8893953 -0.4389846 -0.07835471 -0.0651561 -0.9947941 0.00955069 -0.2943498 -0.9556501 -0.5030289 -0.7109694 -0.4914108 -0.9871243 -0.08944052 0.1326124 -0.8177509 0.5226487 -0.2410847 -0.4899501 0.273616 0.8276976 -0.2910981 0.09226346 0.9522339 -0.4493579 -0.3056316 0.8394443 -0.5338408 -0.3683459 0.7611408 0.2987943 0.371527 -0.8790277 -0.4021835 -0.4700239 0.7857009 -0.4220507 -0.4887886 0.7635176 -0.1449542 -0.2296176 0.9624261 -0.2003037 0.1104407 0.9734893 0.08645594 3.62246e-4 0.9962557 0.1866672 0.1033756 0.9769693 0.3733805 0.3025544 0.8769538 0.09927779 -0.01250576 0.9949812 0.3442765 0.330892 0.8786264 0.7753091 0.6087825 -0.1681656 0.7488061 -0.2374669 0.6187883 0.3469464 -0.3544921 0.8683108 0.3987001 -0.1903805 -0.8971029 0.1085453 -0.09075516 0.9899402 0.3268377 -0.0088104 0.9450395 0.3290829 -0.1822601 0.9265451 0.6139715 -0.4035424 0.678375 -0.02772009 0.284201 0.958364 -0.03336256 0.0897662 0.995404 -0.6193487 0.1010686 0.7785837 -0.2778029 -0.08675807 0.9567125 -0.9786259 0.2034164 0.03021788 -0.2315619 -0.09835451 0.9678355 -0.805751 -0.2529649 0.5355129 0.4779508 -0.3442656 0.8081117 0.5732029 -0.5134252 0.6386181 0.3610681 -0.2830598 0.8885421 0.3343685 -0.2575908 0.9065566 0.7014819 -0.02835166 0.7121232 0.70592 0.3426976 0.6198673 0.6811011 -0.62969 0.37362 0.1784176 -0.9823451 0.05626231 0.186851 -0.9774178 0.09869688 0.5218477 -0.4747936 0.7086932 0.714481 -0.6936132 0.0917474 0.9033104 -0.1921749 0.3835353 0.3734928 -0.3492121 0.8593918 0.521504 -0.4722741 0.710627 0.1769871 -0.6587473 0.7312508 0.5616371 0.1806164 0.807429 0.6708985 -0.1815277 0.7189874 0.9551741 -0.2282738 0.1885036 0.8989989 0.301603 0.3175482 0.6982889 -0.6972344 0.1620395 0.8457717 -0.2029225 0.4934499 0.3708821 0.1101306 0.9221268 0.2176308 0.0277732 0.975636 0.6866242 -0.1855936 0.7029241 0.6974237 -0.6858459 0.2078835 0.1955477 -0.7756326 0.6001293 0.6403356 -0.6041442 0.4743209 0.6428275 -0.6090965 0.4645151 0.4463604 -0.4166259 0.7919504 0.1856774 -0.5753927 0.7965219 0.0642181 -0.245134 0.96736 0.2133968 -0.8615741 0.4605998 0.7594762 0.2617157 0.5955677 0.4885258 -0.1696342 0.8559013 0.08156216 -0.04406374 0.9956938 0.8999354 -0.2576353 0.3517674 0.2150101 -0.7401951 0.6370887 0.5463972 0.7821316 0.299534 0.2015002 -0.2351731 0.9508372 0.6096172 -0.5931513 0.5258691 0.5521982 0.7661918 0.3286751 0.1841709 -0.9611556 0.2055753 0.1812267 -0.9626263 0.2012647 0.6643514 -0.6953822 0.2740092 0.9517241 -0.2862977 0.1107029 0.1607166 -0.5022025 0.849684 0.6191703 0.7679276 0.1640598 0.6577726 0.7389503 0.1459028 0.1613181 -0.4908376 0.8561863 0.5817117 0.7457821 0.3246854 0.5911649 -0.560029 0.5804238 0.8918185 -0.2471305 0.3789277 0.1268668 -0.1773459 0.9759372 0.1266732 -0.1770298 0.9760197 0.752824 -0.2348867 0.6148856 0.248453 0.03362923 0.96806 0.9560208 0.2689313 0.1170485 0.9618873 0.272124 -0.02685755 0.4144735 -0.4139291 0.8104779 0.9394051 -0.3351843 0.07190012 0.202539 -0.7739754 0.5999501 0.6708698 0.7131989 0.2031776 0.8367328 -0.2441117 0.4901917 0.9384893 -0.2908243 0.1861693 0.1737135 -0.6567386 0.7338379 0.3570137 -0.3725913 0.8565728 0.02515178 -0.1501411 0.9883447 0.8705683 0.255016 0.4208061 0.9222589 -0.3097407 0.2312992 0.1950079 -0.8563795 0.4781069 0.5874609 0.6450222 0.4887085 0.8139328 -0.2568557 0.5210939 0.1083389 -0.4067628 0.907087 0.6383783 0.6553999 0.4036389 0.511084 -0.5479221 0.6622496 0.6406634 -0.6696766 0.3756111 0.6406233 -0.6726308 0.3703643 0.2791787 -0.135927 0.95057 0.5535365 -0.2059327 0.806963 0.5676015 0.1462096 0.8102169 0.06580245 -0.1326498 0.9889764 0.1519723 -0.5902286 0.7928018 0.3272601 -0.3681837 0.8702538 0.6559969 -0.7467706 0.1095525 0.2790783 -0.3082192 0.9094593 0.4847848 0.5739708 0.6599556 0.4014286 0.07450646 0.9128548 0.727103 0.198834 0.6571046 0.4989621 -0.5007379 0.7073178 0.6485337 -0.7437576 0.1619533 0.2974862 -0.34889 0.8886945 0.1727421 -0.8019812 0.5718272 0.9490877 0.2390331 0.2051723 0.6215971 -0.7811921 0.05793297 0.8915789 -0.2917421 0.3463722 0.1688318 -0.9144703 0.3677499 0.1671819 -0.9161022 0.364427 0.638407 -0.7470672 0.1852758 0.1205539 -0.5447655 0.8298779 0.6009124 -0.6529076 0.4611032 0.5997118 -0.6484489 0.4688922 0.1072366 -0.9909226 0.08107298 0.1128997 -0.9880602 0.1048361 0.9276772 -0.3539564 0.1188697 0.8662614 0.2405836 0.437848 0.5847111 0.6476205 0.48857 0.05055975 -0.1250036 0.9908672 0.009423673 -0.09427958 0.9955012 0.0703442 -0.05443346 0.9960365 0.6265109 -0.7325082 0.2663006 0.1858974 -0.2458865 0.9513054 0.5750129 -0.6284626 0.5238274 0.7059894 0.6779806 0.204747 0.700344 -0.2561566 0.6662597 0.1945492 -0.2592061 0.9460247 0.7261697 0.680233 0.09980368 0.6107028 0.1410568 0.7791953 0.03597187 -0.9937598 0.1055824 0.5339905 -0.6215478 0.5731776 0.6088775 -0.7347709 0.2989649 0.1544743 -0.8756941 0.4574907 0.1580162 -0.8695635 0.4678572 0.3964729 -0.4462302 0.8023017 0.4406857 0.0691114 0.8949972 0.02779018 -0.9897847 0.1398357 0.9568721 0.2163214 0.19391 0.6576279 -0.2332855 0.7163124 0.3737927 -0.4445008 0.8140627 0.2462257 0.01830643 0.9690397 0.8469942 -0.3250899 0.4206156 0.5832521 0.4851413 0.6515021 0.6647562 0.598508 0.4470878 0.1123007 -0.9732055 0.2006483 0.1048808 -0.9717726 0.2113243 0.1238175 -0.5464654 0.8282783 0.4018419 -0.186522 0.8965114 0.04410493 -0.2525447 0.9665795 0.9498872 0.2257868 0.2161822 0.04555249 -0.09539139 0.9943971 0.2369336 0.01130867 0.9714601 0.5923574 -0.7141906 0.3728868 0.1096605 -0.185917 0.976427 0.1111289 -0.1881043 0.9758418 0.1380272 -0.6855626 0.7148094 0.1421543 -0.8054392 0.575378 0.4586917 0.4525187 0.7647411 0.9046176 -0.3612806 0.226149 0.7887034 0.1629944 0.592773 0.5969057 -0.7924556 0.1253702 0.0420891 -0.9762368 0.2125802 0.1157798 -0.7922962 0.5990508 0.5487679 -0.2289829 0.804003 0.5042966 -0.5553247 0.6612864 0.9145066 -0.3876187 0.1158854 0.9165927 -0.3943337 0.06602251 0.8961254 -0.3731518 0.2402436 0.8812737 0.1777657 0.4378996 0.7998483 -0.3004925 0.5195644 0.271294 -0.1378988 0.9525669 0.02105373 -0.1475772 0.9888265 0.5633578 -0.7105823 0.4215458 0.5568819 -0.6889384 0.4639467 0.6967488 0.60525 0.3849853 0.922747 0.1809201 0.3403026 0.06881558 -0.3219813 0.9442418 0.02428889 -0.9642916 0.2637265 0.06268388 -0.1446809 0.9874908 0.4405838 0.08590734 0.8935915 0.7687466 0.6163673 0.1706461 0.5603814 -0.2226373 0.7977503 0.4303444 -0.5495353 0.7161108 0.5840539 0.1045739 0.8049505 0.08752286 -0.4479008 0.8897891 0.278985 -0.346392 0.895645 0.7268787 -0.3270474 0.6038936 0.5770749 0.4919673 0.6518841 0.3388197 0.3079745 0.8890179 0.9651449 0.1700336 0.1989576 0.5880928 -0.7873008 0.1852148 0.574178 -0.7908373 0.2118872 0.0850256 -0.4029904 0.9112461 0.08930492 -0.05269449 0.9946095 0.9576181 0.1406834 0.2513477 0.005826711 -0.09202796 0.9957394 0.8473705 -0.3736694 0.3772724 0.7886342 0.1522743 0.5957086 0.7152886 0.5786203 0.3918685 0.6912391 -0.2927294 0.6606799 0.1009911 -0.9246455 0.3671944 0.1009582 -0.9246774 0.3671228 0.8803057 0.178231 0.439654 0.7250825 -0.3389385 0.5994799 0.04425317 -0.241817 0.9693122 0.7698292 0.6110962 0.1841861 0.5380405 -0.7315048 0.4188235 0.7788019 0.1377164 0.6119656 0.8944061 -0.3976025 0.2048168 0.5230375 -0.6698757 0.5269709 0.1868818 -0.2642 0.9461889 0.1294253 -0.6846533 0.7172858 0.8364605 -0.3523684 0.4197264 0.8916485 -0.384366 0.239219 0.5608954 -0.8260136 0.05566048 0.07273447 -0.3750708 0.9241383 0.5257603 -0.8205873 0.2240819 0.9756842 0.1498889 0.1599178 0.5197866 -0.6342649 0.5723023 0.1273567 -0.04029577 0.9910382 0.4008271 -0.1917102 0.895871 0.02447038 -0.9242041 0.3811144 0.03123486 -0.9315037 0.3623884 0.571604 -0.7645657 0.2978394 0.4217844 -0.2105934 0.8819004 0.09598743 -0.6451876 0.7579707 0.4843764 0.07353687 0.8717637 0.02030855 -0.1644493 0.9861765 0.3790022 -0.438803 0.8147448 0.9266461 0.1632509 0.3386386 0.09689521 -0.8781451 0.468479 0.7940991 -0.3704368 0.4818539 0.09384095 -0.8844888 0.4570268 0.8486681 -0.4093946 0.3349009 0.102068 -0.5637497 0.8196149 0.8373288 -0.3979386 0.3748673 0.02132588 -0.8704947 0.4917156 0.03110051 -0.8892387 0.4563851 0.04574829 -0.09550988 0.9943767 0.08672165 -0.850532 0.5187243 0.9900832 0.1176899 0.07670992 0.5533534 -0.2765616 0.7856932 0.6582019 0.07479131 0.7491173 0.2635518 -0.1795035 0.9477969 0.1020932 -0.7944934 0.5986296 0.1008397 -0.7936309 0.5999845 0.02691203 -0.8555417 0.5170342 0.5761814 0.4759868 0.6644182 0.3279227 2.29425e-4 0.9447046 0.5266642 -0.8386146 0.1391059 0.3108268 0.2353366 0.920871 0.5321761 -0.7585465 0.3760263 0.8839058 0.09061044 0.4588031 0.05875694 -0.5083554 0.8591406 0.8143703 0.575809 0.07242304 0.7858874 -0.3901084 0.4797878 0.7978999 -0.4061503 0.4454187 0.269904 -0.177767 0.9463354 0.449293 0.02849215 0.89293 0.3998542 -0.5743058 0.7143456 0.8832235 -0.4559954 0.1094745 0.4616674 -0.5656231 0.6833255 0.6015923 0.07206714 0.7955458 0.5491846 -0.2781242 0.7880629 0.01769965 -0.8032794 0.5953392 0.511403 -0.8328303 0.2118034 0.7105897 0.5347595 0.4572688 0.3045309 -0.4255403 0.8521599 0.1529415 -0.1352347 0.9789385 0.02268022 -0.7938462 0.6076955 0.2606129 -0.3663793 0.8932229 0.6318904 0.4582629 0.6250677 0.1730746 -0.147082 0.9738646 0.3573846 -0.009745001 0.9339064 0.2738932 -0.0098719 0.9617096 0.8672275 -0.4582679 0.1946977 0.4152973 -0.2224686 0.8820635 0.06375283 -0.7310071 0.6793851 0.8803555 0.08249735 0.4670849 0.02271103 -0.7383941 0.6739869 0.7896968 0.5751847 0.2134049 0.5031607 -0.7315163 0.4601232 0.5428777 0.03357654 0.8391403 0.0143159 -0.7211794 0.6926004 0.05746006 -0.07191354 0.9957545 0.1867731 0.1094503 0.9762871 0.2640843 -0.3833724 0.885034 0.8525001 -0.4715715 0.2255308 0.03820621 -0.1342682 0.9902083 0.8642708 -0.4917979 0.1056932 0.09180605 -0.1166093 0.9889256 0.5225402 -0.7624402 0.3816241 0.1874917 -0.04912483 0.981037 0.4392889 0.2822771 0.8528451 0.5064622 -0.862044 0.01940011 0.8706603 0.07460176 0.4861947 0.06597453 -0.6499361 0.7571198 0.07189106 -0.642058 0.763278 0.4533415 -0.258429 0.8530511 0.7804962 0.06700915 0.6215591 0.4837208 -0.6989269 0.5267974 0.001565456 -0.09555989 0.9954225 0.6937069 -0.3828045 0.6101078 0.3333845 0.2288882 0.9145847 0.6921502 -0.3740394 0.6172704 0.9588106 0.07338565 0.2744028 0.02539646 -0.2500782 0.9678925 0.02078634 -0.6479914 0.761364 0.04168689 -0.3487026 0.9363059 0.009220242 -0.6300689 0.7764845 0.1706708 -0.2665399 0.9485927 0.8240846 -0.4612232 0.3288735 0.1507402 0.05856859 0.9868369 0.4490016 -0.6923519 0.5648419 0.9819167 0.04841822 0.1830172 0.05080419 -0.5074735 0.8601685 0.8095769 -0.4666047 0.3561813 0.02344042 -0.3923314 0.9195253 0.4907385 -0.7576274 0.4303215 0.03150916 -0.5307639 0.8469339 0.02347433 -0.521971 0.8526403 0.002413868 -0.09547406 0.995429 0.4964728 -0.8387132 0.2237745 0.7886208 0.5118216 0.3407578 0.8401954 -0.5053899 0.196603 0.9877804 0.04242599 0.1499665 0.8528361 0.514253 0.09063535 0.1771922 -0.2942606 0.9391558 0.03044462 -0.3664231 0.9299501 -0.001310706 -0.4044606 0.9145545 0.3354961 -0.5193827 0.7859288 0.8044576 0.0544185 0.5915122 0.3122289 -0.209057 0.9267191 0.01671278 -0.3614379 0.9322464 0.4402571 0.3100115 0.8426545 0.5414103 0.02736335 0.8403131 0.003362774 -0.2935835 0.9559276 0.9446116 0.06530714 0.3216271 0.02867817 -0.2567923 0.966041 -0.003639638 -0.2818984 0.9594374 0.07032412 -0.01950788 0.9973335 0.006519913 -0.1530675 0.9881942 0.004741787 -0.1685442 0.9856827 0.01153844 -0.1464212 0.9891551 0.6649462 0.04501676 0.7455335 0.105053 -0.204838 0.973142 0.09202015 -0.1855876 0.9783096 0.3961287 -0.6340308 0.6641438 0.3510234 -0.01780921 0.9361974 0.8424415 0.5241727 0.1246417 0.4178062 -0.6403255 0.6445319 0.8416047 -0.5372438 0.05541384 0.8403537 -0.5323337 0.1021096 0.776047 -0.449988 0.4418845 0.7457721 0.4945363 0.4463831 0.4362564 -0.2699192 0.8583845 0.2865368 -0.1961388 0.9377774 0.3129141 -0.486148 0.8159319 0.4618245 -0.8783208 0.1235742 0.7539836 -0.4554036 0.4734094 0.4350112 -0.02041828 0.9001936 0.6295429 0.3973414 0.6676793 0.5500158 -0.3280192 0.7680405 0.5459564 -0.3236963 0.7727563 0.2353783 -0.3786548 0.8951076 0.04247653 -0.05284911 0.9976988 0.4556668 -0.7674741 0.4509449 0.983112 0.04273498 0.1779456 0.03899151 -0.1367631 0.9898361 0.6017526 0.007268965 0.7986494 0.4317881 -0.7431546 0.511156 0.342465 -0.2476106 0.9063149 0.350945 -0.01775127 0.9362279 0.8191552 -0.5368393 0.2019611 0.8198121 -0.5333875 0.208341 0.4469618 -0.2983927 0.843319 0.2648801 0.1070582 0.95832 0.2499424 -0.4458131 0.8595228 0.01094704 -0.0924344 0.9956587 0.6842214 -0.4294184 0.5894413 0.8785942 -0.01023751 0.4774597 0.3340233 -0.5535737 0.7628792 0.4454937 -0.8948423 -0.02815246 0.8152421 0.4767982 0.3287004 0.6783269 -0.4132962 0.6075022 0.1816241 -0.04594331 0.9822942 0.2293955 -0.3870193 0.8930812 0.1893373 -0.04718452 0.9807778 0.6872444 0.3997419 0.606549 0.4485815 -0.8731217 0.1908752 0.5913474 -0.009528756 0.8063607 0.2185277 -0.1946616 0.9562178 0.33535 0.1655904 0.9274266 0.8172755 0.4512665 0.3583565 0.9957101 -0.007300138 0.09223926 0.43388 -0.8039399 0.4067292 0.4283815 -0.3056163 0.8503458 0.517125 0.2903305 0.8051646 0.8842557 0.4613302 0.07257157 0.5166166 0.2894226 0.8058176 0.3811641 -0.9075185 0.1764203 0.7681391 -0.5303324 0.3587617 0.7668876 -0.5255587 0.3683362 0.4289644 -0.8797638 0.2049518 0.06540513 -0.07267856 0.9952086 0.2428892 0.1257707 0.9618662 0.1451169 -0.1574597 0.9768049 0.8766332 0.4633689 0.1296288 0.1454499 -0.2818223 0.9483779 0.3889015 -0.2978692 0.8717967 0.3390004 -0.2519942 0.9064092 0.04692518 -0.1392531 0.9891444 0.1085914 -0.2336455 0.9662389 0.8009293 -0.5901463 0.1011919 0.767445 -0.02041411 0.6407898 0.554163 -0.3953458 0.7325334 0.3815635 -0.909324 0.1659495 0.8781336 -0.01107138 0.4782875 0.3890943 -0.8191993 0.4213292 0.3536557 -0.2743937 0.8942236 0.3557788 -0.7310606 0.582213 0.7246223 -0.5108179 0.462588 0.8768727 -0.006183087 0.4806829 0.9589905 -0.08510905 0.2703588 0.9904019 0.01667845 0.1372079 0.2324763 -0.2117394 0.9492741 0.3821469 -0.8567965 0.3462133 0.4495943 -0.03579252 0.8925155 0.5277239 -0.3755457 0.7618879 0.3498684 -0.7378715 0.5771809 0.3595911 0.1608393 0.9191436 0.4027306 -0.8025944 0.4400574 0.5013573 -0.3680992 0.783035 0.7166765 -0.5122064 0.473307 0.01110064 -0.09252578 0.9956485 0.07356816 -0.1222856 0.9897646 0.3721153 -0.9221022 0.1061039 0.1298883 -0.2986744 0.9454749 0.7892034 -0.5936978 0.1571027 0.7881814 -0.5845329 0.1925912 0.7400558 -0.03708612 0.6715222 0.1443724 -0.3232184 0.9352468 0.8513234 0.4071043 0.3309299 0.1299602 -0.1529245 0.9796553 0.3337006 -0.7348459 0.590462 0.3492535 0.1569601 0.9237888 0.6986377 0.3624212 0.6168925 0.8398073 0.4117993 0.3537585 0.6602602 -0.491209 0.5681288 0.9421685 -0.05918055 0.329873 0.3536931 -0.9108505 0.2127263 0.004746258 -0.09519714 0.9954472 0.6530475 -0.4760143 0.5890157 0.1638619 0.0279355 0.9860877 0.5138062 0.2304074 0.8263871 0.9182993 -0.05504333 0.3920416 0.9129284 0.4081062 0.003351569 0.9076021 0.4025841 0.1190994 0.5256853 0.2294747 0.8191437 0.7494669 -0.599544 0.2807962 0.07411903 -0.2094478 0.9750068 0.6902716 0.3378398 0.6398355 0.9793992 -0.08197242 0.1845481 0.5983254 -0.02111041 0.8009752 0.1394451 -0.3204156 0.9369573 0.04769837 -0.1664099 0.9849023 0.08326685 -0.1290037 0.9881421 0.2954793 -0.06945031 0.9528215 0.1705259 -0.06985002 0.9828744 0.346413 -0.8721297 0.3455256 0.9810295 -0.08783233 0.1728204 0.4640051 -0.3715916 0.8041262 0.7325289 -0.5772486 0.3608121 0.4349079 -0.04844319 0.899171 0.08565706 -0.2432519 0.9661736 0.3130033 -0.94616 0.08252358 0.9411898 -0.08308333 0.3275042 0.07303202 -0.0383808 0.9965909 0.2991203 -0.9467809 0.118883 0.3275708 -0.868477 0.3720823 0.04431921 -0.08533489 0.9953662 0.5703026 -0.4512653 0.686378 0.5004343 -0.0746572 0.8625497 0.8930485 -0.07659536 0.4433932 0.8677152 -0.06769752 0.49243 0.298444 -0.9310338 0.2100178 0.7198599 -0.5800299 0.3812704 0.2476151 -0.9658538 0.07624572 0.3152291 -0.826342 0.4666794 0.9826515 -0.06271797 0.1745353 0.2773218 -0.9299903 0.2412691 0.2496841 -0.963832 0.09319782 0.3905975 0.1568083 0.9071079 0.2297146 -0.6285608 0.743063 0.6834568 -0.5623873 0.4654108 0.9868934 -0.1471312 0.06628715 0.8444688 -0.08680236 0.5285242 0.2929528 -0.8102053 0.507687 0.9075371 0.3692297 0.2001151 0.7553505 -0.6470993 0.1034802 0.2232552 0.05118668 0.9734152 0.2770254 -0.06467872 0.9586834 0.7130717 -0.07635152 0.6969212 0.8999614 -0.1154399 0.4204083 0.3754966 -0.3228026 0.8687927 0.2241846 -0.2240175 0.94845 0.9208547 0.351116 0.169541 0.7496337 -0.6436328 0.1542276 0.2915875 -0.752065 0.5910795 0.2751138 -0.8871971 0.3703966 0.6745467 -0.5632615 0.4772037 0.8882567 -0.1297441 0.4406433 0.1739221 -0.07042354 0.9822382 0.8561885 -0.1328321 0.4992965 0.7262803 -0.653966 0.2117677 0.7337667 0.2952915 0.6118737 0.1401907 -0.1772121 0.9741368 0.249732 -0.9362073 0.2472848 0.6322623 -0.5357427 0.5596644 0.8812981 0.3461275 0.3217289 0.2717861 -0.8866183 0.3742196 0.2511473 -0.7274572 0.6385384 0.2675186 -0.2625914 0.9270812 0.8762091 0.3483697 0.3330106 0.03822636 -0.08701509 0.9954733 0.02591383 -0.1430872 0.9893708 0.5082103 0.1738939 0.8434947 0.7085419 0.2966524 0.6402857 0.4383299 -0.3896697 0.8099535 0.9676195 -0.1606466 0.1946924 0.9391611 0.3212029 0.1216766 0.8876937 -0.1796475 0.4239422 0.6059504 -0.5405615 0.5836244 0.7220897 -0.6332376 0.2785619 0.1019076 -0.3366494 0.9360995 0.2387625 -0.2435771 0.9400334 0.9566812 -0.1812064 0.2278715 0.3608074 -0.3269441 0.8734562 0.9499275 -0.2204228 0.2214758 0.9289698 0.3171692 0.1908375 0.2226427 -0.9060599 0.3598411 0.2483705 -0.8257988 0.5063285 0.2420604 -0.8473111 0.4727271 0.06056118 -0.09252101 0.9938674 0.2182936 -0.9553567 0.1991024 0.2270387 -0.6420677 0.7322586 0.7790275 -0.1835972 0.5995069 0.1086781 -0.3071746 0.9454273 0.6801468 -0.6299676 0.3748883 0.7202527 -0.1791219 0.6701876 0.06785255 -0.2502656 0.9657967 0.2285237 -0.8992719 0.3729437 0.7599596 -0.1873122 0.622395 0.2965316 -0.2943872 0.9085182 0.8329687 0.2916578 0.4702118 0.4279471 0.1234635 0.8953313 0.9261035 0.3075321 0.2185326 0.6565983 0.5168199 0.5493413 -0.07261544 -0.2279958 0.9709506 0.3097561 0.3040081 0.9009053 -0.3969302 0.5635638 0.7244601 -0.4210671 0.2051451 0.8835259 0.001744687 0.8646906 0.5023018 0.7763878 0.2391738 0.5831106 -0.07284194 0.2401847 0.9679905 0.3273343 0.06008714 0.9429962 -0.076276 0.6878615 0.7218231 0.1014821 -0.1522147 -0.9831237 0.02105516 0.1245942 0.9919844 0.1699683 0.400554 0.9003706 0.9264063 -0.3143643 0.2072361 0.4057518 0.7695022 0.4931856 0.4298923 0.005693793 0.9028622 0.517871 -0.3448214 0.7828843 0.3555818 -0.1108939 0.9280432 0.252502 -0.1762578 0.9514074 -0.005685091 0.2621326 0.9650152 -0.1308522 0.1953973 0.9719555 -0.03157216 0.2186197 0.9752992 -0.2842284 0.6240335 0.7278711 -0.01215106 0.009392261 0.9998821 -0.00202471 -0.02326285 0.9997274 -0.2848804 0.6246914 0.7270514 -0.2382434 0.4943941 0.8359513 -0.04296147 0.01948809 0.9988867 -0.2955524 0.8425605 0.4502671 -0.2926172 0.8378394 0.4608691 -0.2752121 0.7618221 0.5864174 -0.2381197 0.699058 0.6742529 -0.2420186 0.6402693 0.7290283 -0.1950949 0.4693945 0.861166 -0.1718468 0.4233409 0.8895231 -0.1559273 0.3447462 0.9256548 -0.1038762 0.20515 0.9732027 -0.1051826 0.2127937 0.9714193 -0.2355765 0.8549618 0.4621082 -0.04168218 0.02709567 0.9987635 -0.2119293 0.7964919 0.5662921 -0.2170947 0.705209 0.6749445 -0.1885358 0.658162 0.7288876 -0.1439417 0.4854545 0.8623309 -0.1459937 0.4311711 0.8903805 -0.08121073 0.2189495 0.9723508 -0.07072573 0.1990964 0.9774245 -0.02872633 0.01550269 0.9994671 -0.01849502 -0.01042711 0.9997746 -0.1527261 0.8000005 0.580236 -0.3782564 0.7173315 0.5851134 -0.1522464 0.8013341 0.5785194 -0.1337952 0.670403 0.7298347 -0.1339819 0.670801 0.7294347 -0.1057056 0.4936825 0.8631941 -0.3066604 0.6141663 0.7271584 -0.1038156 0.4899627 0.8655397 -0.2568585 0.3452755 0.9026675 -0.2662494 0.4647534 0.8444617 -0.1145996 0.1268834 0.9852754 -0.06156116 0.2535452 0.9653627 -0.02490091 0.8197962 0.5721138 -0.08986163 0.8083372 0.5818212 -0.0891093 0.8103816 0.5790866 -0.3395131 0.8318076 0.4391208 -0.02791404 0.8128452 0.5818105 -0.07917684 0.6782404 0.7305622 -0.07898443 0.6791326 0.7297537 -0.02061319 0.6661593 0.7455246 -0.02593362 0.6822878 0.7306237 -0.0622785 0.4991483 0.8642756 -0.168821 0.2998827 0.9389196 -0.3362348 0.7576438 0.5593943 -0.06048309 0.4950954 0.8667309 -0.009864091 0.4672038 0.8840946 -0.0213238 0.5020041 0.8646025 -0.3235275 0.7439075 0.5847492 -0.0347315 0.2283877 0.9729506 -0.5508961 -0.3302168 0.7664663 -0.4568747 -0.2506917 0.8534748 -0.1646417 -0.1781786 0.9701266 -0.1612796 0.8590733 0.4857798 -0.1331869 0.2678755 0.9542034 -0.05880975 0.1388934 0.9885597 -0.6559569 -0.1371193 0.7422391 0.08205306 -0.6825516 -0.7262167 -0.2197603 -0.1000103 0.9704141 -0.5112276 -0.0203461 0.8592047 0.2486643 0.09595286 -0.9638253 0.2146509 0.6362599 0.7410117 0.0367062 0.01084357 0.9992673 0.2404516 0.2697267 0.9324326 -0.7831242 0.4271293 0.4519702 -0.4332237 -0.2020003 0.8783583 -0.5469282 0.5373809 0.6419433 -0.3503934 0.2734643 0.8957912 -0.4639144 -0.464555 -0.7543024 -0.2607011 0.134118 0.9560583 -0.001547396 -0.08560556 0.9963279 2.26924e-5 -0.08717697 0.9961929 -1.32595e-5 -0.08713787 0.9961963 -6.00891e-5 -0.08706986 0.9962022 1.14863e-5 -0.0871644 0.996194 -6.31762e-5 -0.08705532 0.9962036 -5.08247e-5 -0.08720862 0.9961901 1.90282e-5 -0.08716368 0.9961941 3.13129e-5 -0.08717036 0.9961934 -1.0569e-4 -0.08713215 0.9961968 1.36228e-5 -0.08715736 0.9961946 -3.48781e-5 -0.08714032 0.996196 -1.33902e-4 -0.08728301 0.9961836 -0.00746423 -0.08824807 0.9960706 1.7739e-5 -0.08713561 0.9961965 1.37939e-5 -0.08713603 0.9961965 1.03888e-5 -0.08714342 0.9961959 4.34562e-6 -0.08718001 0.9961926 2.52988e-5 -0.08717119 0.9961934 4.39296e-5 -0.08713042 0.9961969 -1.07829e-4 -0.08681446 0.9962246 3.70891e-5 -0.0871309 0.9961969 -4.35676e-5 -0.08718591 0.9961922 1.6735e-4 -0.08701956 0.9962067 5.50105e-6 -0.08713185 0.9961969 -3.27004e-4 -0.08759558 0.9961562 -3.25202e-6 -0.08713293 0.9961967 1.54333e-5 -0.0871306 0.9961969 4.28383e-6 -0.0871343 0.9961966 -3.73901e-4 -0.08706152 0.9962029 3.69726e-5 -0.08714574 0.9961956 -6.59782e-6 -0.08714425 0.9961958 -3.31692e-5 -0.08714026 0.9961961 8.75144e-5 -0.08727246 0.9961845 -6.24555e-6 -0.08713275 0.9961967 7.19394e-6 -0.08717888 0.9961927 3.77576e-5 -0.08711349 0.9961985 2.37876e-5 -0.08715796 0.9961946 3.70411e-5 -0.08711498 0.9961984 -8.4629e-5 -0.08712375 0.9961976 6.33518e-5 -0.08703225 0.9962055 -1.59215e-6 -0.08718162 0.9961925 -9.17431e-5 -0.08716481 0.996194 -6.12708e-6 -0.08713442 0.9961966 -6.23401e-6 -0.0871489 0.9961954 -4.4737e-6 -0.08715236 0.996195 -1.49242e-4 -0.08726757 0.996185 5.22222e-6 -0.08718889 0.9961918 4.05536e-6 -0.08721196 0.9961898 1.77859e-4 -0.08707612 0.9962017 4.57018e-5 -0.08713191 0.9961968 2.65803e-5 -0.08716106 0.9961943 0.002895474 -0.09138822 0.9958112 -6.60706e-6 -0.08716768 0.9961937 -6.26243e-5 -0.08728915 0.9961831 -3.92015e-6 -0.0871517 0.9961951 3.99692e-5 -0.08715254 0.996195 -2.52042e-5 -0.08722096 0.9961891 -2.2757e-5 -0.08721917 0.9961892 8.49728e-5 -0.0870496 0.996204 -0.01818263 -0.08912634 0.9958544 -1.39704e-4 -0.08708375 0.996201 4.87041e-6 -0.08715331 0.996195 -2.33367e-5 -0.0871632 0.9961941 -2.39468e-4 -0.08727079 0.9961847 -6.64123e-5 -0.08695185 0.9962126 -9.86826e-5 -0.08711755 0.9961981 0 -0.08709287 0.9962002 -1.45628e-4 -0.08721655 0.9961894 2.28143e-5 -0.08717721 0.9961928 -1.19058e-6 -0.08718258 0.9961925 7.52707e-5 -0.08710521 0.9961991 -3.00865e-6 -0.08714944 0.9961953 1.58074e-5 -0.08714985 0.9961953 -1.45423e-4 -0.08705198 0.9962038 -1.42711e-5 -0.0871281 0.9961972 2.71455e-5 -0.0871967 0.9961912

10 0 17 0 19 0 3538 1 20 1 21 1 10 2 19 2 22 2 1 3 3538 3 21 3 3902 4 15 4 3957 4 3535 5 1 5 21 5 15 6 3902 6 3535 6 11 7 0 7 18 7 11 8 18 8 17 8 4 9 3542 9 3545 9 4 10 3545 10 3924 10 11 11 17 11 10 11 4 12 3924 12 2 12 20 13 3539 13 3540 13 5 14 4 14 2 14 5 15 2 15 3930 15 3 16 3543 16 3542 16 20 17 3540 17 0 17 3 18 3542 18 4 18 6 19 3930 19 7 19 3 20 4 20 5 20 15 21 10 21 22 21 8 22 3 22 5 22 14 23 6 23 7 23 8 24 5 24 3930 24 15 25 11 25 10 25 13 26 8 26 3930 26 9 27 3543 27 3 27 6 28 13 28 3930 28 12 29 9 29 3 29 21 30 20 30 0 30 21 31 0 31 11 31 3957 32 15 32 22 32 16 33 3 33 8 33 16 34 12 34 3 34 19 35 16 35 8 35 15 36 21 36 11 36 19 37 8 37 13 37 3541 38 9 38 12 38 20 39 3534 39 3539 39 18 40 3541 40 12 40 19 41 13 41 6 41 18 42 12 42 16 42 22 43 6 43 14 43 22 44 14 44 3957 44 22 45 19 45 6 45 17 46 18 46 16 46 17 47 16 47 19 47 3540 48 3541 48 18 48 3538 49 3534 49 20 49 0 50 3540 50 18 50 3535 51 21 51 15 51 54 52 3413 52 23 52 33 53 261 53 268 53 35 54 294 54 275 54 31 55 47 55 29 55 50 56 36 56 257 56 35 57 275 57 284 57 41 58 260 58 289 58 63 59 292 59 3394 59 272 60 67 60 219 60 75 61 339 61 281 61 26 62 254 62 270 62 70 63 25 63 24 63 36 64 287 64 251 64 88 65 68 65 269 65 45 66 271 66 28 66 31 67 29 67 30 67 41 68 289 68 230 68 88 69 269 69 27 69 32 70 33 70 268 70 38 71 315 71 37 71 38 72 49 72 303 72 38 73 303 73 315 73 53 74 290 74 294 74 293 75 35 75 284 75 61 76 299 76 277 76 43 77 39 77 40 77 54 78 23 78 26 78 73 79 270 79 287 79 70 80 24 80 267 80 73 81 26 81 270 81 293 82 34 82 35 82 93 83 41 83 230 83 44 84 43 84 40 84 49 85 38 85 37 85 62 86 49 86 37 86 31 87 30 87 68 87 50 88 249 88 288 88 62 89 37 89 39 89 65 90 45 90 28 90 46 91 62 91 39 91 71 92 27 92 56 92 107 93 70 93 267 93 46 94 39 94 43 94 74 95 54 95 26 95 48 96 329 96 49 96 75 97 281 97 82 97 52 98 293 98 280 98 89 99 42 99 47 99 89 100 47 100 31 100 85 101 53 101 294 101 55 102 46 102 43 102 85 103 294 103 35 103 290 104 53 104 83 104 82 105 281 105 25 105 93 106 230 106 51 106 67 107 50 107 288 107 329 108 48 108 81 108 57 109 280 109 292 109 57 110 52 110 280 110 48 111 49 111 62 111 72 112 48 112 62 112 34 113 85 113 35 113 111 114 71 114 56 114 60 115 3742 115 3756 115 108 116 41 116 93 116 59 117 43 117 44 117 107 118 267 118 286 118 71 119 88 119 27 119 59 120 55 120 43 120 97 121 57 121 292 121 99 122 61 122 277 122 97 123 292 123 63 123 69 124 72 124 62 124 34 125 293 125 52 125 69 126 62 126 46 126 59 127 44 127 58 127 99 128 277 128 42 128 64 129 82 129 25 129 119 130 28 130 285 130 119 131 65 131 28 131 64 132 25 132 70 132 94 133 59 133 58 133 122 134 3756 134 66 134 125 135 129 135 36 135 91 136 83 136 53 136 74 137 3413 137 54 137 125 138 36 138 50 138 108 139 3387 139 41 139 33 140 119 140 285 140 129 141 73 141 287 141 129 142 287 142 36 142 89 143 31 143 68 143 74 144 26 144 73 144 107 145 286 145 45 145 87 146 81 146 48 146 98 147 33 147 32 147 115 148 64 148 70 148 126 149 336 149 75 149 105 150 34 150 52 150 80 151 46 151 55 151 80 152 69 152 46 152 136 153 85 153 34 153 90 154 66 154 76 154 90 155 122 155 66 155 95 156 90 156 76 156 139 157 74 157 73 157 87 158 48 158 72 158 122 159 60 159 3756 159 3412 160 3413 160 74 160 78 161 87 161 72 161 94 162 58 162 3742 162 160 163 3387 163 108 163 77 164 63 164 3397 164 77 165 97 165 63 165 91 166 53 166 85 166 3412 167 3243 167 3413 167 190 168 93 168 51 168 79 169 55 169 59 169 32 170 283 170 112 170 96 171 115 171 70 171 96 172 70 172 107 172 79 173 80 173 55 173 67 174 125 174 50 174 81 175 121 175 92 175 123 176 95 176 76 176 110 177 107 177 45 177 175 178 98 178 32 178 100 179 89 179 68 179 126 180 75 180 82 180 78 181 72 181 69 181 60 182 94 182 3742 182 121 183 81 183 87 183 118 184 52 184 57 184 118 185 105 185 52 185 159 186 125 186 67 186 99 187 42 187 89 187 166 188 94 188 60 188 86 189 79 189 59 189 173 190 3388 190 3387 190 110 191 45 191 65 191 83 192 104 192 84 192 104 193 83 193 91 193 91 194 85 194 136 194 86 195 59 195 94 195 118 196 57 196 97 196 117 197 100 197 68 197 138 198 110 198 65 198 136 199 34 199 105 199 128 200 126 200 82 200 134 201 88 201 71 201 122 202 90 202 95 202 166 203 60 203 122 203 117 204 68 204 88 204 78 205 69 205 80 205 112 206 175 206 32 206 149 207 136 207 105 207 119 208 33 208 98 208 111 209 134 209 71 209 101 210 111 210 56 210 139 211 73 211 129 211 120 212 99 212 89 212 109 213 302 213 61 213 121 214 87 214 78 214 109 215 61 215 99 215 145 216 129 216 125 216 119 217 138 217 65 217 100 218 120 218 89 218 103 219 82 219 64 219 103 220 128 220 82 220 102 221 121 221 78 221 146 222 96 222 107 222 164 223 108 223 93 223 77 224 3397 224 3388 224 106 225 78 225 80 225 158 226 118 226 97 226 116 227 77 227 3388 227 149 228 105 228 118 228 110 229 146 229 107 229 131 230 74 230 139 230 140 231 67 231 272 231 127 232 336 232 126 232 190 233 164 233 93 233 131 234 3412 234 74 234 157 235 146 235 110 235 147 236 127 236 126 236 163 237 111 237 101 237 158 238 97 238 77 238 130 239 64 239 115 239 130 240 103 240 64 240 165 241 119 241 98 241 134 242 117 242 88 242 173 243 116 243 3388 243 124 244 80 244 79 244 124 245 106 245 80 245 113 246 122 246 95 246 166 247 86 247 94 247 157 248 110 248 132 248 156 249 104 249 91 249 163 250 101 250 3944 250 149 251 118 251 158 251 130 252 115 252 96 252 156 253 91 253 136 253 165 254 98 254 175 254 145 255 125 255 159 255 135 256 92 256 121 256 92 257 135 257 352 257 3953 258 123 258 3767 258 152 259 139 259 129 259 124 260 79 260 86 260 102 261 78 261 106 261 153 262 124 262 86 262 132 263 110 263 138 263 168 264 158 264 77 264 3408 265 131 265 139 265 180 266 132 266 138 266 177 267 109 267 99 267 141 268 102 268 106 268 151 269 126 269 128 269 147 270 114 270 127 270 137 271 117 271 134 271 130 272 96 272 148 272 160 273 173 273 3387 273 84 274 133 274 176 274 133 275 84 275 104 275 167 276 147 276 126 276 152 277 129 277 145 277 168 278 77 278 116 278 159 279 67 279 140 279 209 280 100 280 117 280 177 281 99 281 120 281 113 282 166 282 122 282 304 283 302 283 109 283 150 284 156 284 136 284 141 285 106 285 124 285 167 286 126 286 151 286 163 287 3944 287 234 287 142 288 152 288 145 288 151 289 128 289 103 289 144 290 135 290 121 290 143 291 134 291 111 291 144 292 121 292 102 292 155 293 114 293 147 293 3408 294 152 294 142 294 148 295 96 295 146 295 3408 296 139 296 152 296 184 297 160 297 108 297 229 298 138 298 119 298 229 299 180 299 138 299 169 300 141 300 124 300 159 301 142 301 145 301 177 302 120 302 100 302 172 303 304 303 109 303 179 304 180 304 229 304 250 305 175 305 112 305 169 306 124 306 153 306 154 307 133 307 104 307 150 308 136 308 149 308 154 309 104 309 156 309 170 310 155 310 147 310 229 311 119 311 165 311 162 312 144 312 102 312 137 313 209 313 117 313 166 314 153 314 86 314 184 315 108 315 164 315 161 316 151 316 103 316 3409 317 140 317 272 317 161 318 103 318 130 318 148 319 146 319 157 319 214 320 148 320 157 320 183 321 150 321 149 321 163 322 143 322 111 322 142 323 159 323 140 323 3409 324 142 324 140 324 178 325 147 325 167 325 205 326 154 326 156 326 247 327 229 327 165 327 171 328 157 328 132 328 174 329 168 329 116 329 352 330 135 330 185 330 202 331 161 331 130 331 232 332 209 332 137 332 174 333 116 333 173 333 172 334 109 334 177 334 170 335 147 335 178 335 221 336 165 336 175 336 191 337 185 337 135 337 182 338 172 338 177 338 202 339 130 339 148 339 234 340 143 340 163 340 187 341 162 341 102 341 209 342 177 342 100 342 183 343 149 343 158 343 187 344 102 344 141 344 179 345 171 345 132 345 213 346 172 346 182 346 161 347 167 347 151 347 174 348 173 348 160 348 179 349 132 349 180 349 203 350 160 350 184 350 216 351 166 351 113 351 143 352 137 352 134 352 189 353 183 353 158 353 214 354 171 354 179 354 181 355 153 355 166 355 186 356 205 356 156 356 186 357 156 357 150 357 221 358 175 358 250 358 181 359 169 359 153 359 204 360 95 360 123 360 191 361 135 361 144 361 189 362 158 362 168 362 195 363 167 363 161 363 210 364 176 364 133 364 223 365 187 365 141 365 218 366 305 366 304 366 211 367 203 367 184 367 196 368 191 368 144 368 199 369 170 369 178 369 218 370 304 370 172 370 211 371 184 371 164 371 223 372 141 372 169 372 194 373 150 373 183 373 194 374 186 374 150 374 196 375 144 375 162 375 191 376 198 376 185 376 208 377 223 377 169 377 164 378 190 378 283 378 226 379 189 379 168 379 193 380 210 380 133 380 193 381 133 381 154 381 197 382 174 382 160 382 192 383 210 383 193 383 202 384 195 384 161 384 195 385 178 385 167 385 206 386 194 386 183 386 197 387 160 387 203 387 199 388 155 388 170 388 266 389 191 389 196 389 200 390 193 390 154 390 227 391 113 391 95 391 226 392 168 392 174 392 200 393 154 393 205 393 232 394 201 394 209 394 207 395 172 395 213 395 207 396 218 396 172 396 204 397 123 397 3953 397 231 398 181 398 166 398 217 399 232 399 137 399 182 400 177 400 209 400 283 401 211 401 164 401 214 402 157 402 171 402 217 403 137 403 143 403 215 404 206 404 183 404 196 405 162 405 187 405 200 406 205 406 186 406 242 407 169 407 181 407 242 408 208 408 169 408 212 409 306 409 305 409 220 410 196 410 187 410 231 411 166 411 216 411 201 412 182 412 209 412 240 413 231 413 216 413 215 414 183 414 189 414 214 415 202 415 148 415 241 416 226 416 174 416 227 417 95 417 204 417 238 418 199 418 178 418 192 419 331 419 210 419 230 420 227 420 204 420 219 421 217 421 143 421 201 422 282 422 182 422 256 423 200 423 186 423 225 424 155 424 199 424 191 425 258 425 198 425 241 426 174 426 197 426 212 427 305 427 218 427 216 428 113 428 227 428 236 429 214 429 179 429 220 430 187 430 223 430 239 431 197 431 203 431 221 432 247 432 165 432 188 433 155 433 225 433 258 434 222 434 198 434 228 435 215 435 189 435 228 436 189 436 226 436 192 437 193 437 200 437 224 438 186 438 194 438 224 439 256 439 186 439 202 440 3404 440 195 440 219 441 143 441 234 441 195 442 238 442 178 442 258 443 191 443 266 443 208 444 220 444 223 444 243 445 212 445 218 445 248 446 192 446 200 446 231 447 242 447 181 447 239 448 203 448 211 448 252 449 196 449 220 449 235 450 179 450 229 450 235 451 236 451 179 451 241 452 228 452 226 452 32 453 239 453 211 453 265 454 331 454 192 454 265 455 333 455 331 455 282 456 213 456 182 456 224 457 194 457 206 457 252 458 266 458 196 458 257 459 201 459 232 459 235 460 229 460 247 460 289 461 216 461 227 461 3391 462 242 462 231 462 233 463 199 463 238 463 233 464 225 464 199 464 278 465 224 465 206 465 243 466 218 466 207 466 238 467 195 467 3404 467 274 468 306 468 212 468 246 469 188 469 225 469 249 470 232 470 217 470 3396 471 220 471 208 471 283 472 32 472 211 472 272 473 219 473 234 473 249 474 257 474 232 474 262 475 246 475 225 475 245 476 247 476 221 476 3396 477 208 477 255 477 32 478 268 478 239 478 28 479 228 479 241 479 243 480 207 480 213 480 246 481 237 481 188 481 250 482 245 482 221 482 289 483 227 483 230 483 233 484 238 484 3404 484 255 485 208 485 242 485 259 486 237 486 246 486 248 487 200 487 256 487 278 488 206 488 215 488 244 489 231 489 240 489 244 490 3391 490 231 490 262 491 225 491 233 491 257 492 282 492 201 492 264 493 248 493 256 493 245 494 235 494 247 494 261 495 241 495 197 495 253 496 265 496 192 496 251 497 243 497 213 497 288 498 249 498 217 498 261 499 197 499 239 499 3609 500 245 500 250 500 273 501 233 501 3404 501 259 502 297 502 237 502 254 503 274 503 212 503 244 504 240 504 216 504 28 505 241 505 261 505 26 506 274 506 254 506 24 507 248 507 264 507 253 508 192 508 248 508 3396 509 252 509 220 509 271 510 278 510 215 510 275 511 222 511 258 511 288 512 217 512 219 512 264 513 256 513 224 513 339 514 333 514 265 514 51 515 204 515 3953 515 51 516 230 516 204 516 291 517 297 517 259 517 291 518 298 518 297 518 276 519 258 519 266 519 267 520 264 520 224 520 275 521 310 521 222 521 261 522 239 522 268 522 263 523 262 523 233 523 279 524 259 524 246 524 260 525 244 525 216 525 254 526 212 526 243 526 279 527 246 527 262 527 271 528 215 528 228 528 260 529 216 529 289 529 267 530 224 530 278 530 263 531 233 531 273 531 47 532 291 532 259 532 269 533 273 533 3405 533 24 534 264 534 267 534 251 535 213 535 282 535 276 536 284 536 258 536 339 537 265 537 253 537 281 538 339 538 253 538 3394 539 252 539 3396 539 276 540 266 540 252 540 270 541 254 541 243 541 269 542 263 542 273 542 253 543 248 543 24 543 279 544 47 544 259 544 50 545 257 545 249 545 279 546 262 546 263 546 28 547 271 547 228 547 30 548 279 548 263 548 284 549 275 549 258 549 260 550 3391 550 244 550 286 551 267 551 278 551 252 552 280 552 276 552 277 553 299 553 298 553 27 554 269 554 3405 554 287 555 270 555 243 555 29 556 279 556 30 556 277 557 298 557 291 557 26 558 23 558 274 558 257 559 251 559 282 559 36 560 251 560 257 560 30 561 263 561 269 561 45 562 286 562 278 562 285 563 28 563 261 563 293 564 284 564 276 564 45 565 278 565 271 565 292 566 280 566 252 566 68 567 30 567 269 567 287 568 243 568 251 568 294 569 310 569 275 569 47 570 279 570 29 570 3394 571 292 571 252 571 67 572 288 572 219 572 42 573 277 573 291 573 25 574 281 574 253 574 25 575 253 575 24 575 280 576 293 576 276 576 33 577 285 577 261 577 42 578 291 578 47 578 357 579 297 579 298 579 357 580 237 580 297 580 310 581 348 581 3276 581 3296 582 357 582 298 582 3296 583 298 583 299 583 299 584 300 584 3296 584 3274 585 348 585 310 585 61 586 300 586 299 586 61 587 3289 587 300 587 310 588 3276 588 222 588 3038 589 3289 589 61 589 334 590 185 590 198 590 3038 591 61 591 302 591 3038 592 302 592 304 592 304 593 3283 593 3038 593 303 594 3305 594 308 594 305 595 3278 595 3283 595 305 596 3283 596 304 596 306 597 3278 597 305 597 313 598 3274 598 310 598 306 599 3275 599 3278 599 3273 600 3275 600 306 600 274 601 3273 601 306 601 3273 602 274 602 23 602 307 603 3273 603 23 603 307 604 23 604 3413 604 316 605 3298 605 309 605 83 606 301 606 311 606 3265 607 301 607 83 607 3272 608 3274 608 313 608 301 609 3272 609 314 609 303 610 308 610 315 610 309 611 3305 611 303 611 319 612 3294 612 3298 612 312 613 324 613 3265 613 290 614 301 614 314 614 311 615 301 615 290 615 313 616 310 616 294 616 316 617 309 617 49 617 3272 618 313 618 294 618 314 619 294 619 290 619 314 620 3272 620 294 620 317 621 3257 621 324 621 329 622 3298 622 316 622 323 623 3265 623 83 623 320 624 3294 624 319 624 49 625 309 625 303 625 311 626 290 626 83 626 312 627 3265 627 323 627 84 628 312 628 323 628 318 629 326 629 3257 629 324 630 312 630 84 630 321 631 322 631 3291 631 323 632 83 632 84 632 325 633 324 633 84 633 319 634 3298 634 329 634 3291 635 3294 635 320 635 331 636 327 636 326 636 317 637 324 637 325 637 325 638 84 638 176 638 330 639 3257 639 317 639 331 640 326 640 318 640 352 641 321 641 3291 641 329 642 316 642 49 642 322 643 321 643 352 643 317 644 325 644 176 644 328 645 3285 645 322 645 318 646 3257 646 330 646 352 647 3291 647 349 647 337 648 327 648 333 648 337 649 3248 649 327 649 330 650 176 650 210 650 330 651 317 651 176 651 81 652 319 652 329 652 81 653 320 653 319 653 332 654 3248 654 337 654 318 655 330 655 210 655 331 656 318 656 210 656 333 657 327 657 331 657 3248 658 3311 658 335 658 334 659 3281 659 3285 659 92 660 349 660 3291 660 92 661 3291 661 320 661 338 662 3248 662 332 662 351 663 322 663 352 663 320 664 81 664 92 664 337 665 333 665 339 665 336 666 3068 666 3311 666 335 667 3248 667 338 667 342 668 3311 668 335 668 345 669 3068 669 336 669 332 670 337 670 339 670 338 671 339 671 75 671 338 672 332 672 339 672 336 673 3311 673 342 673 328 674 322 674 351 674 3068 675 340 675 344 675 340 676 3068 676 345 676 341 677 3276 677 3281 677 335 678 338 678 75 678 343 679 344 679 340 679 342 680 335 680 75 680 342 681 75 681 336 681 349 682 92 682 352 682 185 683 3285 683 328 683 114 684 344 684 343 684 345 685 336 685 127 685 354 686 344 686 114 686 354 687 347 687 344 687 347 688 3304 688 346 688 346 689 347 689 354 689 340 690 345 690 127 690 348 691 3274 691 3276 691 198 692 3281 692 334 692 343 693 340 693 127 693 343 694 127 694 114 694 334 695 3285 695 185 695 350 696 3304 696 346 696 351 697 352 697 185 697 353 698 3304 698 350 698 3304 699 353 699 355 699 222 700 341 700 3281 700 3276 701 341 701 222 701 354 702 114 702 155 702 346 703 354 703 155 703 237 704 357 704 355 704 350 705 346 705 155 705 350 706 155 706 188 706 353 707 350 707 188 707 356 708 355 708 353 708 237 709 355 709 356 709 222 710 3281 710 198 710 328 711 351 711 185 711 356 712 188 712 237 712 356 713 353 713 188 713 3242 714 358 714 360 714 360 715 358 715 361 715 358 716 3233 716 361 716 361 717 3233 717 362 717 364 718 2928 718 440 718 364 719 366 719 2928 719 370 720 439 720 365 720 364 721 440 721 3353 721 366 722 364 722 3353 722 370 723 365 723 3014 723 367 724 3352 724 3121 724 3048 725 2901 725 363 725 369 726 363 726 3050 726 3352 727 366 727 3353 727 3352 728 367 728 366 728 369 729 3050 729 3048 729 370 730 3014 730 368 730 370 731 368 731 439 731 3048 732 363 732 369 732 371 733 2901 733 3048 733 3040 734 2901 734 371 734 3040 735 2902 735 2901 735 2904 736 368 736 3014 736 371 737 3048 737 3040 737 373 738 2902 738 3040 738 373 739 3040 739 374 739 2904 740 3014 740 372 740 374 741 375 741 2902 741 3012 742 2905 742 2904 742 374 743 2902 743 373 743 3012 744 2904 744 372 744 376 745 375 745 374 745 376 746 374 746 3035 746 379 747 377 747 375 747 378 748 377 748 379 748 3035 749 379 749 375 749 3035 750 378 750 379 750 3035 751 375 751 376 751 3004 752 2908 752 380 752 378 753 3035 753 3029 753 381 754 377 754 378 754 381 755 2909 755 377 755 380 756 2905 756 3012 756 3029 757 381 757 378 757 3008 758 380 758 3012 758 382 759 2909 759 381 759 382 760 381 760 3029 760 3004 761 380 761 3008 761 382 762 3029 762 3025 762 3025 763 383 763 2909 763 3004 764 2911 764 2908 764 3025 765 2909 765 382 765 384 766 383 766 3025 766 384 767 3025 767 385 767 386 768 2915 768 383 768 386 769 383 769 384 769 386 770 384 770 385 770 388 771 2910 771 2911 771 385 772 2915 772 386 772 387 773 388 773 2911 773 3021 774 2915 774 385 774 2917 775 2915 775 3021 775 387 776 2911 776 3004 776 394 777 2917 777 3021 777 394 778 3021 778 3019 778 393 779 389 779 2917 779 3350 780 3121 780 3352 780 390 781 2919 781 3054 781 400 782 2920 782 389 782 390 783 392 783 391 783 400 784 389 784 395 784 393 785 2917 785 394 785 392 786 390 786 3054 786 393 787 394 787 3019 787 395 788 389 788 393 788 396 789 401 789 2910 789 395 790 393 790 3019 790 399 791 401 791 396 791 398 792 396 792 2910 792 398 793 2910 793 388 793 397 794 391 794 392 794 398 795 399 795 396 795 3051 796 397 796 392 796 403 797 2924 797 2920 797 402 798 397 798 3051 798 400 799 395 799 3244 799 399 800 398 800 3245 800 404 801 401 801 399 801 402 802 3051 802 3042 802 405 803 2918 803 401 803 405 804 401 804 404 804 397 805 402 805 407 805 3042 806 407 806 402 806 403 807 2920 807 400 807 404 808 399 808 3245 808 3244 809 403 809 400 809 406 810 2924 810 403 810 405 811 3245 811 410 811 405 812 404 812 3245 812 408 813 2918 813 405 813 410 814 408 814 405 814 417 815 2924 815 406 815 406 816 403 816 3244 816 413 817 407 817 3042 817 413 818 3042 818 3036 818 411 819 421 819 409 819 412 820 413 820 3036 820 412 821 3036 821 415 821 414 822 413 822 412 822 414 823 416 823 413 823 3066 824 2922 824 408 824 3066 825 408 825 410 825 415 826 416 826 414 826 415 827 414 827 412 827 419 828 2922 828 3066 828 417 829 409 829 2924 829 417 830 411 830 409 830 2990 831 421 831 411 831 418 832 2922 832 419 832 2923 833 416 833 415 833 419 834 3066 834 418 834 3031 835 2923 835 415 835 420 836 2925 836 2922 836 420 837 2922 837 418 837 2990 838 411 838 417 838 2927 839 2923 839 3031 839 426 840 2925 840 420 840 426 841 425 841 2925 841 420 842 418 842 422 842 421 843 2990 843 3177 843 422 844 426 844 420 844 2927 845 3031 845 3026 845 424 846 421 846 3177 846 3056 847 423 847 425 847 428 848 425 848 426 848 3356 849 424 849 3177 849 427 850 2927 850 3026 850 428 851 3056 851 425 851 428 852 426 852 422 852 429 853 427 853 3026 853 428 854 422 854 3056 854 2929 855 424 855 3356 855 3355 856 2929 856 3356 856 430 857 2900 857 423 857 432 858 423 858 3056 858 427 859 429 859 3018 859 433 860 436 860 2929 860 435 861 431 861 427 861 433 862 2929 862 3355 862 437 863 431 863 435 863 432 864 3056 864 3053 864 433 865 3355 865 3354 865 438 866 2900 866 430 866 430 867 423 867 432 867 434 868 433 868 3354 868 435 869 427 869 3018 869 434 870 3354 870 440 870 434 871 436 871 433 871 430 872 432 872 3053 872 363 873 2900 873 438 873 437 874 3018 874 365 874 437 875 435 875 3018 875 3050 876 363 876 438 876 2928 877 434 877 440 877 2928 878 436 878 434 878 439 879 431 879 437 879 438 880 3053 880 3050 880 365 881 439 881 437 881 438 882 430 882 3053 882 448 883 450 883 442 883 444 884 443 884 450 884 3329 885 444 885 450 885 441 886 3329 886 450 886 443 887 446 887 450 887 448 888 441 888 450 888 447 889 445 889 3329 889 450 890 446 890 449 890 449 891 3513 891 442 891 441 892 3330 892 3329 892 450 893 449 893 442 893 3329 894 445 894 444 894 3219 895 3220 895 3601 895 451 896 3219 896 3601 896 3601 897 3217 897 451 897 452 898 451 898 3217 898 453 899 3217 899 3218 899 454 900 2817 900 2303 900 2814 901 2818 901 2303 901 2817 902 2814 902 2303 902 2891 903 659 903 2889 903 2889 904 659 904 455 904 2650 905 2889 905 455 905 659 906 2887 906 455 906 455 907 2887 907 2888 907 455 908 2888 908 456 908 2651 909 455 909 456 909 2888 910 660 910 456 910 2649 911 2651 911 456 911 2427 912 551 912 2832 912 461 913 2427 913 2832 913 460 914 549 914 2824 914 460 915 457 915 549 915 462 916 459 916 2427 916 462 917 2427 917 461 917 460 918 2824 918 2827 918 467 919 463 919 458 919 467 920 458 920 460 920 2418 921 459 921 462 921 467 922 460 922 2827 922 466 923 2411 923 463 923 464 924 2418 924 462 924 465 925 2414 925 2418 925 465 926 2418 926 464 926 470 927 2414 927 465 927 2839 928 465 928 464 928 466 929 463 929 467 929 470 930 2839 930 471 930 466 931 467 931 2827 931 470 932 465 932 2839 932 468 933 2827 933 2829 933 469 934 2405 934 2414 934 469 935 2414 935 470 935 468 936 2411 936 466 936 468 937 466 937 2827 937 474 938 2404 938 2411 938 474 939 2411 939 468 939 469 940 470 940 471 940 472 941 2405 941 469 941 472 942 469 942 471 942 474 943 468 943 2829 943 473 944 2395 944 2404 944 471 945 2406 945 2405 945 471 946 2405 946 472 946 473 947 474 947 2829 947 473 948 2404 948 474 948 475 949 473 949 2829 949 475 950 2395 950 473 950 475 951 2829 951 2833 951 477 952 2406 952 471 952 477 953 2401 953 2406 953 2390 954 2401 954 477 954 478 955 2395 955 475 955 478 956 475 956 2833 956 478 957 2833 957 2835 957 480 958 486 958 476 958 479 959 477 959 2847 959 479 960 2390 960 477 960 479 961 476 961 2390 961 479 962 480 962 476 962 480 963 479 963 2847 963 2386 964 478 964 2835 964 2699 965 2433 965 485 965 485 966 2699 966 482 966 484 967 2699 967 483 967 481 968 480 968 2847 968 481 969 486 969 480 969 484 970 482 970 2699 970 488 971 482 971 484 971 488 972 485 972 482 972 2706 973 2314 973 2386 973 2847 974 486 974 481 974 2706 975 2386 975 2835 975 488 976 2433 976 485 976 2433 977 2429 977 487 977 2834 978 2433 978 488 978 2834 979 487 979 2433 979 489 980 2379 980 493 980 2429 981 492 981 490 981 491 982 487 982 2834 982 491 983 2429 983 487 983 491 984 2834 984 496 984 496 985 2429 985 491 985 496 986 490 986 2429 986 492 987 2420 987 497 987 494 988 486 988 2847 988 495 989 490 989 496 989 495 990 492 990 490 990 494 991 493 991 486 991 495 992 497 992 492 992 495 993 496 993 2830 993 494 994 2847 994 499 994 2830 995 2420 995 497 995 2830 996 497 996 495 996 2420 997 2415 997 501 997 498 998 2420 998 2830 998 499 999 489 999 493 999 498 1000 2830 1000 2828 1000 499 1001 493 1001 494 1001 2828 1002 2420 1002 498 1002 2828 1003 501 1003 2420 1003 2379 1004 489 1004 499 1004 508 1005 2380 1005 2379 1005 500 1006 504 1006 2415 1006 502 1007 501 1007 2828 1007 502 1008 2828 1008 506 1008 502 1009 2415 1009 501 1009 506 1010 2415 1010 502 1010 506 1011 500 1011 2415 1011 503 1012 499 1012 512 1012 503 1013 2379 1013 499 1013 504 1014 2410 1014 505 1014 507 1015 504 1015 500 1015 507 1016 506 1016 510 1016 507 1017 500 1017 506 1017 510 1018 505 1018 504 1018 510 1019 504 1019 507 1019 509 1020 2410 1020 505 1020 509 1021 505 1021 510 1021 512 1022 2380 1022 508 1022 512 1023 508 1023 2379 1023 512 1024 2379 1024 503 1024 509 1025 2398 1025 2410 1025 514 1026 517 1026 2380 1026 509 1027 510 1027 2825 1027 511 1028 2380 1028 512 1028 511 1029 512 1029 516 1029 515 1030 2397 1030 2398 1030 513 1031 2398 1031 509 1031 513 1032 515 1032 2398 1032 513 1033 509 1033 2825 1033 516 1034 2380 1034 511 1034 516 1035 514 1035 2380 1035 2397 1036 515 1036 513 1036 525 1037 2436 1037 517 1037 519 1038 2391 1038 2397 1038 518 1039 2397 1039 513 1039 519 1040 2397 1040 518 1040 519 1041 518 1041 513 1041 520 1042 514 1042 516 1042 520 1043 517 1043 514 1043 520 1044 525 1044 517 1044 520 1045 516 1045 526 1045 529 1046 2436 1046 525 1046 522 1047 2387 1047 2391 1047 521 1048 519 1048 513 1048 521 1049 2391 1049 519 1049 521 1050 522 1050 2391 1050 523 1051 2387 1051 522 1051 525 1052 520 1052 526 1052 528 1053 536 1053 2436 1053 524 1054 2384 1054 2387 1054 528 1055 2436 1055 529 1055 524 1056 2387 1056 523 1056 523 1057 522 1057 521 1057 524 1058 523 1058 521 1058 527 1059 2384 1059 524 1059 529 1060 525 1060 526 1060 530 1061 2383 1061 2384 1061 530 1062 2384 1062 527 1062 527 1063 524 1063 521 1063 530 1064 527 1064 521 1064 528 1065 529 1065 526 1065 531 1066 2383 1066 530 1066 533 1067 532 1067 536 1067 535 1068 2377 1068 2383 1068 535 1069 2383 1069 531 1069 531 1070 521 1070 2826 1070 531 1071 530 1071 521 1071 535 1072 531 1072 2826 1072 537 1073 526 1073 543 1073 537 1074 528 1074 526 1074 537 1075 536 1075 528 1075 2826 1076 538 1076 2377 1076 534 1077 2826 1077 2377 1077 534 1078 2377 1078 535 1078 534 1079 535 1079 2826 1079 533 1080 537 1080 543 1080 533 1081 536 1081 537 1081 541 1082 2422 1082 532 1082 539 1083 538 1083 2826 1083 540 1084 533 1084 543 1084 540 1085 532 1085 533 1085 540 1086 541 1086 532 1086 539 1087 2826 1087 544 1087 538 1088 539 1088 544 1088 541 1089 540 1089 543 1089 544 1090 542 1090 538 1090 545 1091 543 1091 2824 1091 545 1092 541 1092 543 1092 545 1093 2422 1093 541 1093 550 1094 457 1094 2422 1094 547 1095 551 1095 546 1095 550 1096 2422 1096 545 1096 548 1097 542 1097 544 1097 548 1098 546 1098 542 1098 548 1099 547 1099 546 1099 548 1100 544 1100 2832 1100 547 1101 548 1101 2832 1101 550 1102 545 1102 2824 1102 549 1103 550 1103 2824 1103 2832 1104 551 1104 547 1104 549 1105 457 1105 550 1105 460 1106 458 1106 457 1106 1966 1107 552 1107 2811 1107 552 1108 2649 1108 456 1108 2811 1109 552 1109 456 1109 553 1110 1964 1110 1966 1110 554 1111 1498 1111 560 1111 1813 1112 1801 1112 555 1112 630 1113 557 1113 555 1113 1780 1114 554 1114 560 1114 1498 1115 556 1115 558 1115 555 1116 557 1116 559 1116 555 1117 559 1117 1813 1117 1816 1118 1813 1118 559 1118 557 1119 1528 1119 561 1119 1498 1120 558 1120 1780 1120 560 1121 1498 1121 1780 1121 559 1122 557 1122 561 1122 1780 1123 558 1123 556 1123 1816 1124 559 1124 561 1124 1780 1125 556 1125 564 1125 1819 1126 1816 1126 561 1126 561 1127 1528 1127 562 1127 563 1128 1780 1128 564 1128 561 1129 562 1129 1819 1129 562 1130 568 1130 1819 1130 1760 1131 1819 1131 568 1131 568 1132 562 1132 566 1132 564 1133 556 1133 563 1133 556 1134 565 1134 563 1134 565 1135 567 1135 563 1135 568 1136 566 1136 1760 1136 565 1137 570 1137 567 1137 1760 1138 566 1138 569 1138 1767 1139 1760 1139 569 1139 569 1140 566 1140 574 1140 571 1141 563 1141 567 1141 567 1142 570 1142 573 1142 569 1143 574 1143 1767 1143 1781 1144 1767 1144 574 1144 571 1145 567 1145 573 1145 574 1146 572 1146 1781 1146 578 1147 571 1147 573 1147 573 1148 570 1148 577 1148 573 1149 577 1149 578 1149 1781 1150 572 1150 575 1150 1796 1151 1781 1151 575 1151 572 1152 576 1152 579 1152 579 1153 576 1153 582 1153 575 1154 572 1154 579 1154 578 1155 577 1155 581 1155 575 1156 579 1156 1796 1156 1796 1157 579 1157 582 1157 594 1158 1796 1158 582 1158 576 1159 580 1159 582 1159 577 1160 583 1160 586 1160 586 1161 578 1161 581 1161 581 1162 577 1162 586 1162 584 1163 1499 1163 1509 1163 582 1164 580 1164 594 1164 584 1165 1509 1165 1769 1165 1769 1166 1509 1166 1513 1166 1513 1167 587 1167 585 1167 1769 1168 1513 1168 585 1168 585 1169 587 1169 588 1169 587 1170 1518 1170 588 1170 586 1171 583 1171 596 1171 590 1172 588 1172 1518 1172 594 1173 580 1173 1481 1173 1518 1174 1522 1174 589 1174 596 1175 583 1175 1521 1175 1518 1176 589 1176 590 1176 590 1177 589 1177 591 1177 589 1178 1522 1178 591 1178 1522 1179 592 1179 591 1179 1787 1180 590 1180 591 1180 1790 1181 1787 1181 592 1181 1787 1182 591 1182 592 1182 1790 1183 592 1183 593 1183 597 1184 1790 1184 593 1184 593 1185 595 1185 597 1185 594 1186 1481 1186 601 1186 1798 1187 597 1187 595 1187 595 1188 1533 1188 1798 1188 1800 1189 1798 1189 1533 1189 596 1190 1521 1190 606 1190 1533 1191 598 1191 1800 1191 613 1192 596 1192 606 1192 598 1193 1539 1193 1803 1193 1481 1194 599 1194 608 1194 1800 1195 598 1195 1803 1195 1803 1196 1539 1196 600 1196 603 1197 600 1197 1477 1197 1539 1198 1477 1198 600 1198 1477 1199 1484 1199 602 1199 606 1200 1521 1200 1431 1200 601 1201 1481 1201 608 1201 604 1202 603 1202 1484 1202 1477 1203 602 1203 603 1203 602 1204 1484 1204 603 1204 1484 1205 605 1205 604 1205 604 1206 605 1206 610 1206 606 1207 1431 1207 613 1207 604 1208 610 1208 609 1208 599 1209 607 1209 1812 1209 608 1210 599 1210 1812 1210 1431 1211 1432 1211 1711 1211 613 1212 1431 1212 1711 1212 1711 1213 1432 1213 1440 1213 610 1214 1491 1214 612 1214 609 1215 610 1215 612 1215 612 1216 1491 1216 611 1216 607 1217 614 1217 618 1217 1766 1218 612 1218 611 1218 617 1219 1766 1219 615 1219 1766 1220 611 1220 615 1220 1812 1221 607 1221 618 1221 615 1222 611 1222 616 1222 617 1223 615 1223 616 1223 1777 1224 617 1224 622 1224 617 1225 616 1225 622 1225 618 1226 614 1226 621 1226 1296 1227 619 1227 1440 1227 616 1228 1504 1228 620 1228 1289 1229 1296 1229 1440 1229 622 1230 616 1230 620 1230 1762 1231 618 1231 621 1231 1711 1232 1440 1232 619 1232 614 1233 1497 1233 625 1233 1777 1234 622 1234 620 1234 1777 1235 620 1235 623 1235 1789 1236 1777 1236 623 1236 620 1237 1504 1237 623 1237 623 1238 1504 1238 624 1238 621 1239 614 1239 1762 1239 625 1240 1497 1240 1762 1240 614 1241 625 1241 1762 1241 623 1242 624 1242 1789 1242 1789 1243 624 1243 628 1243 629 1244 1789 1244 628 1244 1762 1245 1497 1245 626 1245 628 1246 624 1246 629 1246 1497 1247 1498 1247 627 1247 554 1248 1762 1248 626 1248 629 1249 624 1249 631 1249 626 1250 1497 1250 554 1250 1497 1251 627 1251 554 1251 631 1252 630 1252 1801 1252 554 1253 627 1253 1498 1253 1801 1254 630 1254 555 1254 629 1255 631 1255 1801 1255 921 1256 1110 1256 1384 1256 632 1257 1384 1257 638 1257 632 1258 638 1258 923 1258 921 1259 1384 1259 632 1259 929 1260 634 1260 633 1260 635 1261 634 1261 929 1261 636 1262 634 1262 635 1262 636 1263 928 1263 634 1263 923 1264 638 1264 924 1264 699 1265 641 1265 700 1265 698 1266 642 1266 641 1266 697 1267 1383 1267 642 1267 697 1268 642 1268 698 1268 696 1269 1383 1269 697 1269 645 1270 1383 1270 696 1270 644 1271 1383 1271 645 1271 643 1272 644 1272 645 1272 646 1273 647 1273 644 1273 646 1274 644 1274 643 1274 695 1275 647 1275 646 1275 648 1276 647 1276 695 1276 648 1277 649 1277 647 1277 930 1278 649 1278 648 1278 930 1279 633 1279 649 1279 1061 1280 650 1280 1343 1280 1343 1281 879 1281 1061 1281 981 1282 1159 1282 1061 1282 1159 1283 1547 1283 650 1283 1159 1284 650 1284 1061 1284 1066 1285 651 1285 653 1285 652 1286 782 1286 879 1286 1343 1287 652 1287 879 1287 653 1288 652 1288 1343 1288 651 1289 714 1289 653 1289 2962 1290 3183 1290 3563 1290 2960 1291 2962 1291 3361 1291 2962 1292 3563 1292 3361 1292 2963 1293 2960 1293 3363 1293 2960 1294 3361 1294 3363 1294 2963 1295 3363 1295 654 1295 2964 1296 2963 1296 654 1296 2964 1297 654 1297 655 1297 2959 1298 2964 1298 655 1298 3178 1299 2959 1299 3362 1299 2959 1300 655 1300 3362 1300 2896 1301 3079 1301 3336 1301 2896 1302 3336 1302 656 1302 2894 1303 2896 1303 656 1303 2893 1304 2894 1304 3334 1304 2894 1305 656 1305 3334 1305 2897 1306 2893 1306 3335 1306 2893 1307 3334 1307 3335 1307 657 1308 2897 1308 3335 1308 657 1309 3335 1309 3338 1309 658 1310 657 1310 3338 1310 658 1311 3338 1311 3333 1311 2814 1312 658 1312 3333 1312 659 1313 2891 1313 3076 1313 659 1314 3076 1314 3330 1314 2887 1315 659 1315 441 1315 659 1316 3330 1316 441 1316 2887 1317 441 1317 448 1317 2888 1318 2887 1318 448 1318 660 1319 2888 1319 661 1319 2888 1320 448 1320 661 1320 663 1321 2879 1321 3315 1321 2879 1322 662 1322 3315 1322 663 1323 3315 1323 3317 1323 2886 1324 663 1324 3317 1324 2886 1325 3317 1325 3326 1325 2883 1326 2886 1326 3326 1326 2883 1327 3326 1327 664 1327 665 1328 2883 1328 664 1328 665 1329 664 1329 3321 1329 2885 1330 665 1330 3321 1330 666 1331 2988 1331 2989 1331 666 1332 2989 1332 667 1332 2877 1333 666 1333 667 1333 2877 1334 667 1334 668 1334 2874 1335 2877 1335 3239 1335 2877 1336 668 1336 3239 1336 2871 1337 2874 1337 669 1337 2874 1338 3239 1338 669 1338 2875 1339 2871 1339 670 1339 2871 1340 669 1340 670 1340 2875 1341 670 1341 2985 1341 2868 1342 2866 1342 3231 1342 2868 1343 3231 1343 671 1343 2775 1344 2868 1344 2982 1344 2868 1345 671 1345 2982 1345 2776 1346 2775 1346 2982 1346 2776 1347 2982 1347 3232 1347 2974 1348 2776 1348 3232 1348 2860 1349 2861 1349 2447 1349 2861 1350 672 1350 2447 1350 2860 1351 2447 1351 2448 1351 2859 1352 2860 1352 2449 1352 2860 1353 2448 1353 2449 1353 2858 1354 2859 1354 2450 1354 2859 1355 2449 1355 2450 1355 673 1356 2858 1356 2443 1356 2858 1357 2450 1357 2443 1357 673 1358 2443 1358 2445 1358 2857 1359 673 1359 2444 1359 673 1360 2445 1360 2444 1360 2857 1361 2444 1361 674 1361 2857 1362 674 1362 2442 1362 2442 1363 675 1363 2856 1363 2855 1364 2856 1364 2441 1364 2768 1365 2855 1365 1544 1365 2855 1366 2441 1366 1544 1366 676 1367 2310 1367 677 1367 2820 1368 677 1368 2305 1368 677 1369 2304 1369 2305 1369 2821 1370 2820 1370 2306 1370 2820 1371 2305 1371 2306 1371 2821 1372 2306 1372 2819 1372 2818 1373 2819 1373 2307 1373 2818 1374 2307 1374 2303 1374 454 1375 2303 1375 2302 1375 2817 1376 454 1376 678 1376 454 1377 2302 1377 678 1377 681 1378 1965 1378 1964 1378 681 1379 1960 1379 1965 1379 681 1380 1964 1380 1404 1380 681 1381 687 1381 1960 1381 679 1382 1962 1382 1961 1382 680 1383 681 1383 1404 1383 680 1384 687 1384 681 1384 680 1385 1929 1385 687 1385 1963 1386 1157 1386 685 1386 686 1387 1158 1387 1963 1387 682 1388 1961 1388 1959 1388 682 1389 679 1389 1961 1389 683 1390 1959 1390 684 1390 683 1391 682 1391 1959 1391 683 1392 1903 1392 682 1392 1904 1393 684 1393 685 1393 1904 1394 683 1394 684 1394 1904 1395 1903 1395 683 1395 1157 1396 1904 1396 685 1396 1157 1397 1898 1397 2235 1397 1157 1398 2235 1398 1904 1398 687 1399 1963 1399 1960 1399 687 1400 1406 1400 686 1400 687 1401 686 1401 1963 1401 687 1402 1133 1402 1406 1402 687 1403 1929 1403 1133 1403 688 1404 1589 1404 1588 1404 688 1405 1373 1405 1371 1405 688 1406 1371 1406 1589 1406 689 1407 1588 1407 1590 1407 689 1408 1865 1408 1373 1408 689 1409 1373 1409 688 1409 689 1410 688 1410 1588 1410 691 1411 689 1411 1590 1411 691 1412 1590 1412 1587 1412 689 1413 1881 1413 1865 1413 690 1414 1587 1414 802 1414 690 1415 691 1415 1587 1415 691 1416 1881 1416 689 1416 691 1417 1091 1417 1881 1417 690 1418 1090 1418 1091 1418 690 1419 1091 1419 691 1419 692 1420 1592 1420 1179 1420 692 1421 1179 1421 1611 1421 692 1422 1611 1422 1610 1422 692 1423 1610 1423 693 1423 848 1424 694 1424 695 1424 847 1425 848 1425 646 1425 848 1426 695 1426 646 1426 845 1427 847 1427 643 1427 847 1428 646 1428 643 1428 839 1429 845 1429 645 1429 845 1430 643 1430 645 1430 838 1431 839 1431 696 1431 839 1432 645 1432 696 1432 835 1433 838 1433 697 1433 838 1434 696 1434 697 1434 832 1435 835 1435 698 1435 835 1436 697 1436 698 1436 829 1437 832 1437 641 1437 832 1438 698 1438 641 1438 828 1439 829 1439 699 1439 829 1440 641 1440 699 1440 849 1441 828 1441 700 1441 828 1442 699 1442 700 1442 844 1443 849 1443 640 1443 701 1444 844 1444 640 1444 849 1445 700 1445 640 1445 836 1446 840 1446 639 1446 840 1447 701 1447 639 1447 701 1448 640 1448 639 1448 833 1449 836 1449 702 1449 836 1450 639 1450 702 1450 703 1451 833 1451 824 1451 833 1452 702 1452 824 1452 703 1453 824 1453 704 1453 834 1454 703 1454 704 1454 825 1455 834 1455 704 1455 715 1456 716 1456 705 1456 716 1457 715 1457 706 1457 1355 1458 717 1458 716 1458 728 1459 719 1459 727 1459 719 1460 730 1460 731 1460 719 1461 728 1461 730 1461 731 1462 733 1462 734 1462 707 1463 752 1463 745 1463 749 1464 707 1464 745 1464 749 1465 745 1465 708 1465 709 1466 749 1466 708 1466 752 1467 742 1467 753 1467 752 1468 753 1468 745 1468 710 1469 758 1469 712 1469 758 1470 711 1470 712 1470 711 1471 713 1471 712 1471 712 1472 713 1472 766 1472 713 1473 759 1473 766 1473 759 1474 760 1474 766 1474 753 1475 754 1475 770 1475 712 1476 755 1476 756 1476 755 1477 712 1477 651 1477 651 1478 712 1478 714 1478 715 1479 705 1479 1072 1479 717 1480 1355 1480 718 1480 727 1481 719 1481 877 1481 724 1482 719 1482 731 1482 724 1483 731 1483 734 1483 792 1484 724 1484 734 1484 724 1485 720 1485 723 1485 724 1486 795 1486 720 1486 724 1487 723 1487 721 1487 724 1488 722 1488 795 1488 724 1489 792 1489 722 1489 735 1490 799 1490 798 1490 735 1491 716 1491 724 1491 735 1492 726 1492 799 1492 735 1493 798 1493 801 1493 735 1494 725 1494 726 1494 705 1495 716 1495 735 1495 789 1496 716 1496 717 1496 789 1497 1353 1497 716 1497 728 1498 729 1498 730 1498 728 1499 727 1499 729 1499 731 1500 730 1500 732 1500 731 1501 732 1501 733 1501 734 1502 733 1502 1071 1502 894 1503 792 1503 734 1503 894 1504 734 1504 1071 1504 897 1505 1072 1505 705 1505 897 1506 705 1506 735 1506 818 1507 737 1507 757 1507 818 1508 736 1508 737 1508 857 1509 736 1509 818 1509 818 1510 738 1510 739 1510 818 1511 740 1511 738 1511 818 1512 757 1512 740 1512 751 1513 752 1513 834 1513 834 1514 825 1514 752 1514 750 1515 751 1515 834 1515 709 1516 750 1516 834 1516 752 1517 742 1517 825 1517 987 1518 854 1518 775 1518 854 1519 855 1519 775 1519 746 1520 743 1520 744 1520 746 1521 748 1521 855 1521 743 1522 708 1522 745 1522 855 1523 746 1523 775 1523 745 1524 747 1524 743 1524 743 1525 747 1525 744 1525 748 1526 743 1526 746 1526 709 1527 708 1527 741 1527 707 1528 749 1528 750 1528 749 1529 709 1529 750 1529 707 1530 750 1530 751 1530 707 1531 751 1531 752 1531 753 1532 742 1532 825 1532 754 1533 753 1533 825 1533 770 1534 754 1534 769 1534 754 1535 825 1535 769 1535 756 1536 755 1536 804 1536 712 1537 756 1537 739 1537 756 1538 804 1538 739 1538 710 1539 712 1539 738 1539 712 1540 739 1540 738 1540 758 1541 710 1541 740 1541 710 1542 738 1542 740 1542 711 1543 758 1543 757 1543 758 1544 740 1544 757 1544 713 1545 711 1545 737 1545 711 1546 757 1546 737 1546 759 1547 713 1547 736 1547 713 1548 737 1548 736 1548 760 1549 759 1549 857 1549 759 1550 736 1550 857 1550 766 1551 767 1551 714 1551 852 1552 768 1552 761 1552 766 1553 714 1553 712 1553 762 1554 852 1554 763 1554 852 1555 761 1555 763 1555 861 1556 762 1556 765 1556 762 1557 763 1557 765 1557 765 1558 763 1558 764 1558 765 1559 767 1559 766 1559 765 1560 764 1560 767 1560 852 1561 762 1561 983 1561 852 1562 983 1562 768 1562 1039 1563 937 1563 770 1563 769 1564 1039 1564 770 1564 771 1565 769 1565 823 1565 771 1566 1039 1566 769 1566 745 1567 753 1567 770 1567 745 1568 770 1568 772 1568 745 1569 772 1569 865 1569 747 1570 745 1570 865 1570 744 1571 865 1571 866 1571 744 1572 747 1572 865 1572 744 1573 866 1573 773 1573 746 1574 773 1574 774 1574 746 1575 744 1575 773 1575 775 1576 774 1576 867 1576 775 1577 746 1577 774 1577 776 1578 777 1578 933 1578 729 1579 727 1579 877 1579 729 1580 877 1580 788 1580 779 1581 778 1581 719 1581 719 1582 778 1582 876 1582 719 1583 876 1583 877 1583 874 1584 872 1584 781 1584 872 1585 724 1585 791 1585 874 1586 719 1586 872 1586 781 1587 780 1587 874 1587 872 1588 719 1588 724 1588 761 1589 768 1589 853 1589 761 1590 853 1590 879 1590 761 1591 879 1591 782 1591 763 1592 761 1592 782 1592 764 1593 782 1593 652 1593 764 1594 763 1594 782 1594 767 1595 652 1595 653 1595 767 1596 764 1596 652 1596 767 1597 653 1597 714 1597 784 1598 804 1598 755 1598 784 1599 755 1599 651 1599 784 1600 783 1600 804 1600 920 1601 784 1601 651 1601 785 1602 786 1602 919 1602 729 1603 788 1603 888 1603 876 1604 778 1604 788 1604 875 1605 788 1605 787 1605 788 1606 787 1606 778 1606 787 1607 779 1607 875 1607 875 1608 1057 1608 779 1608 717 1609 1352 1609 789 1609 1351 1610 1264 1610 1353 1610 1264 1611 781 1611 791 1611 1264 1612 890 1612 781 1612 791 1613 724 1613 716 1613 1070 1614 730 1614 729 1614 729 1615 888 1615 1070 1615 722 1616 792 1616 894 1616 795 1617 722 1617 793 1617 722 1618 894 1618 793 1618 720 1619 795 1619 794 1619 723 1620 720 1620 796 1620 720 1621 794 1621 796 1621 721 1622 723 1622 893 1622 723 1623 796 1623 893 1623 725 1624 892 1624 797 1624 726 1625 725 1625 797 1625 799 1626 726 1626 797 1626 799 1627 797 1627 800 1627 798 1628 799 1628 800 1628 735 1629 801 1629 897 1629 735 1630 724 1630 721 1630 725 1631 735 1631 721 1631 1014 1632 803 1632 802 1632 905 1633 803 1633 1013 1633 905 1634 1013 1634 863 1634 804 1635 783 1635 805 1635 805 1636 739 1636 804 1636 811 1637 827 1637 808 1637 816 1638 819 1638 822 1638 926 1639 821 1639 810 1639 818 1640 822 1640 857 1640 810 1641 821 1641 813 1641 815 1642 810 1642 813 1642 806 1643 820 1643 811 1643 811 1644 808 1644 813 1644 809 1645 816 1645 822 1645 926 1646 810 1646 807 1646 694 1647 827 1647 931 1647 806 1648 811 1648 814 1648 811 1649 813 1649 814 1649 812 1650 931 1650 827 1650 809 1651 822 1651 922 1651 807 1652 810 1652 925 1652 827 1653 859 1653 808 1653 925 1654 810 1654 815 1654 812 1655 827 1655 817 1655 818 1656 922 1656 822 1656 815 1657 822 1657 819 1657 925 1658 815 1658 819 1658 927 1659 814 1659 821 1659 814 1660 813 1660 821 1660 820 1661 817 1661 811 1661 817 1662 827 1662 811 1662 823 1663 769 1663 825 1663 824 1664 704 1664 825 1664 825 1665 824 1665 823 1665 828 1666 849 1666 830 1666 848 1667 826 1667 694 1667 694 1668 826 1668 827 1668 826 1669 859 1669 827 1669 849 1670 846 1670 830 1670 828 1671 830 1671 829 1671 741 1672 856 1672 709 1672 829 1673 830 1673 832 1673 832 1674 830 1674 831 1674 830 1675 846 1675 842 1675 831 1676 830 1676 842 1676 834 1677 709 1677 856 1677 703 1678 834 1678 833 1678 832 1679 831 1679 835 1679 833 1680 834 1680 837 1680 834 1681 856 1681 837 1681 835 1682 831 1682 838 1682 838 1683 831 1683 843 1683 836 1684 833 1684 837 1684 840 1685 836 1685 837 1685 843 1686 831 1686 842 1686 856 1687 841 1687 837 1687 839 1688 838 1688 843 1688 840 1689 837 1689 841 1689 843 1690 842 1690 860 1690 839 1691 843 1691 845 1691 701 1692 840 1692 844 1692 843 1693 860 1693 845 1693 840 1694 841 1694 846 1694 847 1695 845 1695 826 1695 845 1696 860 1696 826 1696 860 1697 859 1697 826 1697 844 1698 840 1698 849 1698 849 1699 840 1699 846 1699 848 1700 847 1700 826 1700 1361 1701 850 1701 1155 1701 1361 1702 1075 1702 850 1702 850 1703 1075 1703 944 1703 686 1704 851 1704 1158 1704 768 1705 985 1705 853 1705 987 1706 775 1706 868 1706 775 1707 867 1707 868 1707 855 1708 854 1708 748 1708 813 1709 842 1709 846 1709 815 1710 813 1710 846 1710 995 1711 743 1711 748 1711 822 1712 815 1712 841 1712 815 1713 846 1713 841 1713 708 1714 743 1714 858 1714 741 1715 708 1715 858 1715 760 1716 857 1716 858 1716 766 1717 760 1717 858 1717 765 1718 766 1718 858 1718 822 1719 841 1719 856 1719 1000 1720 765 1720 858 1720 995 1721 1000 1721 858 1721 743 1722 995 1722 858 1722 857 1723 741 1723 858 1723 822 1724 856 1724 741 1724 857 1725 822 1725 741 1725 808 1726 859 1726 860 1726 983 1727 762 1727 861 1727 813 1728 808 1728 842 1728 808 1729 860 1729 842 1729 861 1730 765 1730 1000 1730 802 1731 803 1731 905 1731 863 1732 862 1732 1119 1732 863 1733 1119 1733 939 1733 1013 1734 862 1734 863 1734 803 1735 1014 1735 1013 1735 777 1736 776 1736 938 1736 936 1737 938 1737 1037 1737 864 1738 936 1738 1037 1738 771 1739 937 1739 1039 1739 772 1740 770 1740 937 1740 1033 1741 866 1741 1207 1741 774 1742 773 1742 866 1742 774 1743 866 1743 1033 1743 1031 1744 865 1744 772 1744 866 1745 865 1745 1031 1745 868 1746 867 1746 774 1746 1005 1747 868 1747 774 1747 870 1748 2089 1748 2090 1748 1032 1749 870 1749 2090 1749 1036 1750 1042 1750 871 1750 869 1751 1040 1751 1041 1751 935 1752 1041 1752 1038 1752 935 1753 869 1753 1041 1753 932 1754 935 1754 1038 1754 933 1755 932 1755 776 1755 1048 1756 1219 1756 1042 1756 871 1757 1053 1757 870 1757 871 1758 870 1758 1032 1758 871 1759 1032 1759 1035 1759 871 1760 1042 1760 1054 1760 872 1761 791 1761 781 1761 1237 1762 890 1762 1055 1762 1237 1763 781 1763 890 1763 873 1764 1237 1764 1055 1764 1237 1765 873 1765 1235 1765 781 1766 1237 1766 1236 1766 781 1767 1236 1767 780 1767 1241 1768 887 1768 875 1768 1057 1769 1238 1769 875 1769 1056 1770 1057 1770 779 1770 778 1771 779 1771 787 1771 877 1772 876 1772 788 1772 779 1773 719 1773 874 1773 1056 1774 874 1774 1239 1774 1056 1775 779 1775 874 1775 1241 1776 1243 1776 887 1776 1254 1777 878 1777 1250 1777 1250 1778 878 1778 887 1778 879 1779 853 1779 985 1779 920 1780 651 1780 1066 1780 920 1781 783 1781 784 1781 881 1782 1060 1782 880 1782 882 1783 881 1783 880 1783 919 1784 882 1784 785 1784 786 1785 785 1785 883 1785 915 1786 883 1786 884 1786 885 1787 915 1787 884 1787 878 1788 917 1788 886 1788 788 1789 875 1789 887 1789 886 1790 887 1790 878 1790 1062 1791 788 1791 887 1791 1353 1792 789 1792 1352 1792 1261 1793 891 1793 1262 1793 890 1794 1264 1794 889 1794 1055 1795 890 1795 1263 1795 791 1796 716 1796 1353 1796 791 1797 1353 1797 1264 1797 1261 1798 1351 1798 790 1798 1261 1799 1264 1799 1351 1799 891 1800 1261 1800 790 1800 732 1801 730 1801 1070 1801 1070 1802 1069 1802 732 1802 893 1803 892 1803 725 1803 893 1804 725 1804 721 1804 794 1805 795 1805 793 1805 796 1806 794 1806 793 1806 1841 1807 796 1807 895 1807 796 1808 793 1808 895 1808 793 1809 1069 1809 895 1809 797 1810 892 1810 1841 1810 800 1811 797 1811 1841 1811 897 1812 801 1812 1841 1812 801 1813 798 1813 1841 1813 798 1814 800 1814 1841 1814 1075 1815 1074 1815 944 1815 1359 1816 898 1816 649 1816 941 1817 649 1817 898 1817 941 1818 898 1818 942 1818 1356 1819 1080 1819 900 1819 899 1820 1079 1820 898 1820 1079 1821 1356 1821 898 1821 903 1822 901 1822 1082 1822 1087 1823 1085 1823 913 1823 904 1824 903 1824 1082 1824 1088 1825 1087 1825 1082 1825 1087 1826 913 1826 1082 1826 901 1827 1088 1827 1082 1827 905 1828 909 1828 906 1828 905 1829 906 1829 912 1829 911 1830 905 1830 863 1830 905 1831 911 1831 907 1831 905 1832 907 1832 909 1832 908 1833 906 1833 909 1833 910 1834 909 1834 907 1834 910 1835 908 1835 909 1835 910 1836 907 1836 911 1836 939 1837 911 1837 863 1837 939 1838 910 1838 911 1838 1157 1839 1963 1839 1158 1839 851 1840 1156 1840 1095 1840 1095 1841 1156 1841 914 1841 1095 1842 914 1842 1094 1842 914 1843 1107 1843 1094 1843 1385 1844 786 1844 883 1844 1385 1845 883 1845 915 1845 1385 1846 915 1846 885 1846 1385 1847 885 1847 918 1847 916 1848 918 1848 917 1848 916 1849 1385 1849 918 1849 1230 1850 1385 1850 916 1850 783 1851 920 1851 1110 1851 783 1852 1110 1852 805 1852 1385 1853 882 1853 919 1853 1110 1854 882 1854 1385 1854 1110 1855 881 1855 882 1855 1110 1856 1060 1856 881 1856 1110 1857 920 1857 1060 1857 1110 1858 921 1858 805 1858 739 1859 805 1859 818 1859 922 1860 818 1860 921 1860 818 1861 805 1861 921 1861 809 1862 922 1862 632 1862 922 1863 921 1863 632 1863 816 1864 809 1864 923 1864 819 1865 816 1865 923 1865 809 1866 632 1866 923 1866 925 1867 819 1867 924 1867 819 1868 923 1868 924 1868 807 1869 925 1869 638 1869 925 1870 924 1870 638 1870 926 1871 807 1871 637 1871 807 1872 638 1872 637 1872 821 1873 926 1873 928 1873 926 1874 637 1874 928 1874 927 1875 821 1875 636 1875 821 1876 928 1876 636 1876 814 1877 927 1877 635 1877 927 1878 636 1878 635 1878 806 1879 814 1879 929 1879 814 1880 635 1880 929 1880 820 1881 806 1881 633 1881 806 1882 929 1882 633 1882 820 1883 633 1883 930 1883 817 1884 820 1884 930 1884 812 1885 817 1885 930 1885 812 1886 930 1886 648 1886 931 1887 812 1887 648 1887 931 1888 648 1888 695 1888 694 1889 931 1889 695 1889 934 1890 932 1890 933 1890 934 1891 935 1891 932 1891 934 1892 869 1892 935 1892 934 1893 1040 1893 869 1893 1040 1894 934 1894 1047 1894 823 1895 938 1895 936 1895 823 1896 936 1896 864 1896 823 1897 864 1897 937 1897 823 1898 937 1898 771 1898 823 1899 777 1899 938 1899 644 1900 912 1900 906 1900 644 1901 908 1901 910 1901 910 1902 939 1902 644 1902 644 1903 906 1903 908 1903 1082 1904 913 1904 912 1904 1082 1905 912 1905 940 1905 1084 1906 940 1906 941 1906 1084 1907 1082 1907 940 1907 942 1908 1084 1908 941 1908 649 1909 943 1909 1076 1909 1074 1910 943 1910 649 1910 1076 1911 1073 1911 649 1911 943 1912 945 1912 1076 1912 946 1913 914 1913 951 1913 947 1914 951 1914 955 1914 947 1915 946 1915 951 1915 1123 1916 950 1916 949 1916 1123 1917 955 1917 950 1917 948 1918 949 1918 952 1918 948 1919 1123 1919 949 1919 1156 1920 950 1920 955 1920 1156 1921 955 1921 951 1921 1156 1922 951 1922 914 1922 1156 1923 952 1923 949 1923 1156 1924 949 1924 950 1924 953 1925 962 1925 1122 1925 1123 1926 960 1926 959 1926 954 1927 947 1927 955 1927 954 1928 955 1928 961 1928 954 1929 946 1929 947 1929 954 1930 961 1930 1123 1930 1123 1931 952 1931 1122 1931 956 1932 946 1932 954 1932 957 1933 954 1933 1123 1933 958 1934 948 1934 952 1934 946 1935 957 1935 1123 1935 946 1936 954 1936 957 1936 959 1937 948 1937 958 1937 1123 1938 948 1938 959 1938 1123 1939 958 1939 952 1939 1123 1940 959 1940 958 1940 961 1941 955 1941 1123 1941 1405 1942 1137 1942 962 1942 1139 1943 962 1943 964 1943 1137 1944 1408 1944 964 1944 962 1945 1137 1945 964 1945 1407 1946 1405 1946 962 1946 965 1947 979 1947 971 1947 1141 1948 965 1948 1121 1948 1141 1949 979 1949 965 1949 1140 1950 1141 1950 1121 1950 1122 1951 1140 1951 1121 1951 962 1952 1140 1952 1122 1952 966 1953 1148 1953 1146 1953 1147 1954 1144 1954 1143 1954 1411 1955 966 1955 1146 1955 1146 1956 1147 1956 1143 1956 967 1957 1150 1957 1154 1957 968 1958 1154 1958 1152 1958 968 1959 967 1959 1154 1959 974 1960 1152 1960 1153 1960 974 1961 968 1961 1152 1961 968 1962 971 1962 978 1962 969 1963 967 1963 968 1963 969 1964 968 1964 978 1964 969 1965 978 1965 967 1965 967 1966 970 1966 1150 1966 972 1967 1153 1967 971 1967 971 1968 1149 1968 970 1968 973 1969 971 1969 968 1969 975 1970 971 1970 973 1970 975 1971 972 1971 971 1971 975 1972 1153 1972 972 1972 975 1973 968 1973 974 1973 975 1974 973 1974 968 1974 975 1975 974 1975 1153 1975 976 1976 1153 1976 975 1976 978 1977 977 1977 971 1977 1153 1978 850 1978 1074 1978 978 1979 971 1979 970 1979 978 1980 970 1980 967 1980 1149 1981 971 1981 1143 1981 979 1982 1143 1982 971 1982 1361 1983 1155 1983 1928 1983 1361 1984 1928 1984 980 1984 851 1985 686 1985 1156 1985 1158 1986 851 1986 1095 1986 986 1987 768 1987 983 1987 1162 1988 986 1988 991 1988 984 1989 1162 1989 1161 1989 1161 1990 1162 1990 991 1990 991 1991 986 1991 983 1991 986 1992 985 1992 768 1992 1061 1993 879 1993 985 1993 1061 1994 985 1994 981 1994 981 1995 1568 1995 982 1995 1571 1996 1568 1996 981 1996 748 1997 854 1997 1004 1997 988 1998 1163 1998 1001 1998 1001 1999 1004 1999 854 1999 988 2000 1001 2000 987 2000 987 2001 1001 2001 854 2001 989 2002 1000 2002 995 2002 1167 2003 989 2003 993 2003 990 2004 1161 2004 991 2004 993 2005 989 2005 995 2005 991 2006 983 2006 992 2006 992 2007 983 2007 861 2007 1160 2008 990 2008 992 2008 990 2009 991 2009 992 2009 993 2010 995 2010 994 2010 1165 2011 1166 2011 994 2011 1166 2012 993 2012 994 2012 992 2013 861 2013 997 2013 1160 2014 992 2014 998 2014 998 2015 992 2015 997 2015 1165 2016 994 2016 999 2016 994 2017 995 2017 999 2017 1165 2018 999 2018 996 2018 997 2019 861 2019 1000 2019 1003 2020 998 2020 1002 2020 998 2021 997 2021 1002 2021 999 2022 995 2022 748 2022 997 2023 1000 2023 1002 2023 996 2024 999 2024 1004 2024 1167 2025 1003 2025 989 2025 1003 2026 1002 2026 989 2026 989 2027 1002 2027 1000 2027 1001 2028 996 2028 1004 2028 999 2029 748 2029 1004 2029 1169 2030 1168 2030 1005 2030 1169 2031 1005 2031 774 2031 690 2032 802 2032 1090 2032 1369 2033 1090 2033 905 2033 905 2034 1090 2034 802 2034 1119 2035 862 2035 1012 2035 1007 2036 1012 2036 1006 2036 1007 2037 1119 2037 1012 2037 1016 2038 1006 2038 1011 2038 1016 2039 1007 2039 1006 2039 1010 2040 1009 2040 1008 2040 1010 2041 1022 2041 1009 2041 1184 2042 1010 2042 1008 2042 1013 2043 1011 2043 1006 2043 1013 2044 1006 2044 1012 2044 862 2045 1013 2045 1012 2045 1013 2046 1008 2046 1009 2046 1013 2047 1009 2047 1022 2047 1013 2048 1022 2048 1011 2048 692 2049 693 2049 1014 2049 1183 2050 1013 2050 1014 2050 1592 2051 692 2051 1014 2051 1118 2052 1024 2052 1023 2052 1015 2053 1024 2053 1118 2053 1017 2054 1016 2054 1011 2054 1017 2055 1011 2055 1019 2055 1017 2056 1007 2056 1016 2056 1118 2057 1019 2057 1024 2057 1018 2058 1007 2058 1017 2058 1018 2059 1119 2059 1007 2059 1018 2060 1120 2060 1119 2060 1018 2061 1118 2061 1120 2061 1118 2062 1017 2062 1019 2062 1021 2063 1010 2063 1184 2063 1021 2064 1029 2064 1118 2064 1118 2065 1018 2065 1017 2065 1021 2066 1184 2066 1029 2066 1020 2067 1018 2067 1118 2067 1023 2068 1010 2068 1021 2068 1023 2069 1021 2069 1118 2069 1024 2070 1022 2070 1010 2070 1024 2071 1010 2071 1023 2071 1019 2072 1011 2072 1022 2072 1019 2073 1022 2073 1024 2073 1186 2074 1025 2074 1026 2074 1192 2075 1614 2075 1026 2075 1614 2076 1028 2076 1026 2076 1028 2077 1186 2077 1026 2077 1029 2078 1189 2078 1614 2078 1029 2079 1614 2079 1118 2079 1118 2080 1614 2080 1192 2080 1206 2081 1030 2081 1031 2081 1031 2082 1207 2082 866 2082 1618 2083 1209 2083 1032 2083 1207 2084 1208 2084 1033 2084 1032 2085 1034 2085 1035 2085 774 2086 1033 2086 1169 2086 1035 2087 1205 2087 1036 2087 1036 2088 1206 2088 772 2088 772 2089 1206 2089 1031 2089 772 2090 937 2090 1036 2090 937 2091 938 2091 776 2091 1036 2092 937 2092 776 2092 938 2093 937 2093 1037 2093 1037 2094 937 2094 864 2094 1036 2095 776 2095 1042 2095 776 2096 932 2096 1042 2096 932 2097 1038 2097 1042 2097 1042 2098 1038 2098 1041 2098 1035 2099 1036 2099 871 2099 2090 2100 1618 2100 1032 2100 1040 2101 1047 2101 1042 2101 1041 2102 1040 2102 1042 2102 934 2103 1112 2103 1043 2103 1043 2104 1217 2104 934 2104 1217 2105 1216 2105 934 2105 2089 2106 870 2106 1044 2106 1223 2107 1222 2107 1212 2107 1223 2108 1212 2108 1214 2108 1045 2109 1223 2109 1214 2109 1220 2110 1219 2110 1212 2110 1219 2111 1217 2111 1212 2111 1046 2112 1220 2112 1212 2112 1222 2113 1046 2113 1212 2113 1047 2114 934 2114 1216 2114 1047 2115 1216 2115 1048 2115 1042 2116 1047 2116 1048 2116 1219 2117 1048 2117 1218 2117 1220 2118 1049 2118 1219 2118 1049 2119 1050 2119 1219 2119 1050 2120 1221 2120 1226 2120 1219 2121 1050 2121 1226 2121 1226 2122 1221 2122 1052 2122 1053 2123 1044 2123 870 2123 1224 2124 1053 2124 871 2124 1224 2125 871 2125 1054 2125 1226 2126 1224 2126 1054 2126 1042 2127 1226 2127 1054 2127 1219 2128 1226 2128 1042 2128 1253 2129 1231 2129 1230 2129 916 2130 1255 2130 1253 2130 1230 2131 916 2131 1253 2131 1055 2132 1235 2132 873 2132 1055 2133 1263 2133 1235 2133 1056 2134 1239 2134 1238 2134 1056 2135 1238 2135 1057 2135 875 2136 1238 2136 1241 2136 1238 2137 1242 2137 1241 2137 1243 2138 1250 2138 887 2138 917 2139 878 2139 1254 2139 1255 2140 917 2140 1254 2140 916 2141 917 2141 1255 2141 1246 2142 1248 2142 1250 2142 1246 2143 1250 2143 1243 2143 1244 2144 1246 2144 1243 2144 1245 2145 1244 2145 1243 2145 1682 2146 1245 2146 1243 2146 1248 2147 1251 2147 1250 2147 1250 2148 1252 2148 1254 2148 1253 2149 1249 2149 1247 2149 1058 2150 1059 2150 1257 2150 1247 2151 1058 2151 1257 2151 1059 2152 1245 2152 1260 2152 1257 2153 1059 2153 1260 2153 886 2154 883 2154 785 2154 920 2155 886 2155 785 2155 886 2156 917 2156 883 2156 883 2157 917 2157 884 2157 1343 2158 1066 2158 653 2158 884 2093 917 2093 885 2093 920 2159 785 2159 882 2159 920 2160 882 2160 880 2160 920 2161 880 2161 1060 2161 885 2093 917 2093 918 2093 1062 2162 887 2162 886 2162 886 2163 1347 2163 1062 2163 650 2164 1063 2164 1343 2164 1064 2165 1340 2165 1062 2165 1343 2166 1065 2166 1066 2166 788 2167 1062 2167 1340 2167 1066 2168 1067 2168 1346 2168 920 2169 1066 2169 1346 2169 886 2170 920 2170 1346 2170 1068 2171 1262 2171 891 2171 1068 2172 891 2172 790 2172 1070 2173 888 2173 2198 2173 2198 2174 1833 2174 1070 2174 1833 2175 1069 2175 1070 2175 1069 2176 733 2176 732 2176 1071 2177 733 2177 1069 2177 1071 2178 1069 2178 793 2178 1071 2179 793 2179 894 2179 893 2180 796 2180 1841 2180 893 2181 1841 2181 892 2181 897 2182 1841 2182 1072 2182 1841 2183 896 2183 1072 2183 716 2184 706 2184 896 2184 706 2185 715 2185 896 2185 715 2186 1072 2186 896 2186 1073 2187 1359 2187 649 2187 1076 2188 1361 2188 1073 2188 1361 2189 945 2189 943 2189 1361 2190 943 2190 1074 2190 1361 2191 1074 2191 1075 2191 1361 2192 1076 2192 945 2192 898 2193 1359 2193 1077 2193 899 2194 898 2194 1358 2194 898 2195 1077 2195 1358 2195 1079 2196 899 2196 1078 2196 899 2197 1358 2197 1078 2197 1356 2198 1079 2198 1081 2198 1079 2199 1078 2199 1081 2199 900 2200 1080 2200 1357 2200 1080 2201 1356 2201 1357 2201 900 2202 1357 2202 1363 2202 1364 2203 1082 2203 1083 2203 904 2204 1082 2204 1364 2204 1083 2205 1082 2205 1084 2205 1083 2206 1084 2206 942 2206 900 2207 1364 2207 1083 2207 900 2208 1083 2208 1356 2208 1356 2209 1083 2209 898 2209 1083 2210 942 2210 898 2210 1365 2211 902 2211 913 2211 1086 2212 1365 2212 1085 2212 1365 2213 913 2213 1085 2213 1366 2214 1086 2214 1087 2214 1086 2215 1085 2215 1087 2215 1367 2216 1366 2216 1088 2216 1366 2217 1087 2217 1088 2217 1089 2218 1367 2218 901 2218 1367 2219 1088 2219 901 2219 1089 2220 901 2220 903 2220 1368 2221 903 2221 904 2221 912 2222 913 2222 902 2222 912 2223 902 2223 1369 2223 905 2224 912 2224 1369 2224 1091 2225 1090 2225 1370 2225 1095 2226 1092 2226 1093 2226 1095 2227 1094 2227 1097 2227 1095 2228 1097 2228 1096 2228 1095 2229 1096 2229 1092 2229 1099 2230 1093 2230 1092 2230 1099 2231 1092 2231 1096 2231 1101 2232 1096 2232 1097 2232 1101 2233 1099 2233 1096 2233 1101 2234 1097 2234 1094 2234 1108 2235 1101 2235 1094 2235 1107 2236 1108 2236 1094 2236 1098 2237 1102 2237 1099 2237 1099 2238 1102 2238 1093 2238 1100 2239 1102 2239 1098 2239 1099 2240 1100 2240 1098 2240 1103 2241 1102 2241 1100 2241 1103 2242 1105 2242 1102 2242 1104 2243 1100 2243 1099 2243 1101 2244 1104 2244 1099 2244 1106 2245 1100 2245 1104 2245 1106 2246 1103 2246 1100 2246 1106 2247 1105 2247 1103 2247 1106 2248 1104 2248 1101 2248 1106 2249 1101 2249 1108 2249 1106 2250 1108 2250 1107 2250 1106 2251 1107 2251 1105 2251 1102 2252 1127 2252 1379 2252 1102 2253 1379 2253 1399 2253 1128 2254 1102 2254 1399 2254 1109 2255 1232 2255 1385 2255 1109 2256 1233 2256 1232 2256 1109 2257 1385 2257 1230 2257 786 2258 1385 2258 919 2258 639 2259 640 2259 1111 2259 824 2260 1111 2260 823 2260 702 2261 1111 2261 824 2261 702 2262 639 2262 1111 2262 934 2263 933 2263 777 2263 934 2264 777 2264 823 2264 934 2265 1387 2265 1112 2265 1388 2266 1619 2266 1387 2266 1114 2267 1170 2267 1389 2267 1115 2268 1389 2268 642 2268 1116 2269 1115 2269 642 2269 1117 2270 1116 2270 642 2270 1117 2271 642 2271 644 2271 644 2272 1118 2272 1117 2272 1120 2273 1118 2273 939 2273 1118 2274 644 2274 939 2274 1119 2275 1120 2275 939 2275 644 2276 940 2276 912 2276 941 2277 940 2277 649 2277 940 2278 644 2278 649 2278 944 2279 1074 2279 850 2279 649 2280 965 2280 971 2280 1153 2281 649 2281 971 2281 1121 2282 965 2282 1122 2282 965 2283 649 2283 1122 2283 1122 2284 946 2284 1123 2284 946 2285 633 2285 1102 2285 946 2286 1102 2286 1105 2286 914 2287 946 2287 1107 2287 946 2288 1105 2288 1107 2288 1102 2289 1386 2289 1127 2289 1102 2290 633 2290 1386 2290 1380 2291 1126 2291 1382 2291 1382 2292 1124 2292 1380 2292 1126 2293 1125 2293 1386 2293 1129 2294 1399 2294 1131 2294 1132 2295 1131 2295 1401 2295 1400 2296 1129 2296 1131 2296 1130 2297 1128 2297 1399 2297 952 2298 1409 2298 1134 2298 952 2299 1156 2299 1409 2299 1122 2300 952 2300 953 2300 1134 2301 953 2301 952 2301 1135 2302 963 2302 964 2302 1136 2303 1135 2303 964 2303 1136 2304 964 2304 1408 2304 1138 2305 1407 2305 962 2305 1134 2306 1138 2306 962 2306 1134 2307 962 2307 953 2307 1139 2308 1143 2308 979 2308 1411 2309 1146 2309 1139 2309 962 2310 1139 2310 1140 2310 1139 2311 979 2311 1141 2311 1146 2312 1143 2312 1139 2312 1139 2313 1141 2313 1140 2313 963 2314 1411 2314 1139 2314 963 2315 1139 2315 964 2315 1151 2316 1149 2316 1143 2316 1142 2317 1151 2317 1143 2317 1145 2318 1142 2318 1144 2318 1142 2319 1143 2319 1144 2319 1413 2320 1145 2320 1147 2320 1145 2321 1144 2321 1147 2321 1415 2322 1413 2322 1146 2322 1413 2323 1147 2323 1146 2323 1416 2324 1415 2324 1148 2324 1415 2325 1146 2325 1148 2325 1418 2326 1416 2326 966 2326 1416 2327 1148 2327 966 2327 1417 2328 1418 2328 1411 2328 1418 2329 966 2329 1411 2329 1149 2330 1150 2330 970 2330 1151 2331 1150 2331 1149 2331 1150 2332 1151 2332 1412 2332 1155 2333 1153 2333 1152 2333 1155 2334 1154 2334 1150 2334 1155 2335 850 2335 1153 2335 1155 2336 1152 2336 1154 2336 686 2337 1406 2337 1409 2337 1409 2338 1156 2338 686 2338 1095 2339 1130 2339 1158 2339 1130 2340 1898 2340 1158 2340 1158 2341 1898 2341 1157 2341 982 2342 1159 2342 981 2342 982 2343 1341 2343 1159 2343 982 2344 1568 2344 1755 2344 1562 2345 1566 2345 1167 2345 1566 2346 1570 2346 1003 2346 1167 2347 1566 2347 1003 2347 1570 2348 1573 2348 998 2348 1003 2349 1570 2349 998 2349 1573 2350 1557 2350 1160 2350 998 2351 1573 2351 1160 2351 1557 2352 1558 2352 990 2352 1160 2353 1557 2353 990 2353 990 2354 1558 2354 1161 2354 1558 2355 1571 2355 984 2355 1161 2356 1558 2356 984 2356 1571 2357 981 2357 985 2357 984 2358 1571 2358 1162 2358 1571 2359 985 2359 1162 2359 1162 2360 985 2360 986 2360 1005 2361 1168 2361 868 2361 868 2362 1168 2362 1561 2362 1561 2363 1565 2363 988 2363 987 2364 868 2364 988 2364 868 2365 1561 2365 988 2365 988 2366 1565 2366 1163 2366 1565 2367 1569 2367 1001 2367 1163 2368 1565 2368 1001 2368 1569 2369 1576 2369 996 2369 1001 2370 1569 2370 996 2370 1576 2371 1164 2371 1165 2371 996 2372 1576 2372 1165 2372 1164 2373 1560 2373 1166 2373 1165 2374 1164 2374 1166 2374 1560 2375 1562 2375 993 2375 1166 2376 1560 2376 993 2376 993 2377 1562 2377 1167 2377 1580 2378 1168 2378 1169 2378 1170 2379 1599 2379 1113 2379 1193 2380 1170 2380 1114 2380 1193 2381 1599 2381 1170 2381 1199 2382 1114 2382 1389 2382 1199 2383 1193 2383 1114 2383 1171 2384 1389 2384 1115 2384 1171 2385 1199 2385 1389 2385 1195 2386 1115 2386 1116 2386 1195 2387 1171 2387 1115 2387 1194 2388 1116 2388 1117 2388 1194 2389 1195 2389 1116 2389 1192 2390 1194 2390 1117 2390 1192 2391 1117 2391 1118 2391 1172 2392 1388 2392 1113 2392 1388 2393 1596 2393 1598 2393 1388 2394 1170 2394 1113 2394 1388 2395 1172 2395 1596 2395 1175 2396 1173 2396 1601 2396 1174 2397 1177 2397 1601 2397 1176 2398 1175 2398 1601 2398 1599 2399 1177 2399 1113 2399 1172 2400 1177 2400 1606 2400 1177 2401 1605 2401 1606 2401 1608 2402 1181 2402 1178 2402 1181 2403 1594 2403 1178 2403 1593 2404 2010 2404 1613 2404 1180 2405 1591 2405 1594 2405 1180 2406 1181 2406 1609 2406 1180 2407 1609 2407 2010 2407 1180 2408 1593 2408 1591 2408 1180 2409 1594 2409 1181 2409 1180 2410 2010 2410 1593 2410 1182 2411 1611 2411 1179 2411 1182 2412 1613 2412 1612 2412 1182 2413 1612 2413 1611 2413 1182 2414 1179 2414 1593 2414 1182 2415 1593 2415 1613 2415 1014 2416 693 2416 1183 2416 1008 2417 1183 2417 1029 2417 1008 2418 1013 2418 1183 2418 1029 2419 1184 2419 1008 2419 1185 2420 2063 2420 1027 2420 1615 2421 1185 2421 1026 2421 1185 2422 1027 2422 1026 2422 1187 2423 1615 2423 1025 2423 1615 2424 1026 2424 1025 2424 1188 2425 1187 2425 1186 2425 1187 2426 1025 2426 1186 2426 1614 2427 1188 2427 1028 2427 1188 2428 1186 2428 1028 2428 1191 2429 1192 2429 1026 2429 1190 2430 1171 2430 1195 2430 1196 2431 1198 2431 1197 2431 1198 2432 1599 2432 1193 2432 1176 2433 1601 2433 1198 2433 1196 2434 1176 2434 1198 2434 1197 2435 1198 2435 1199 2435 1191 2436 1190 2436 1194 2436 1190 2437 1195 2437 1194 2437 1198 2438 1193 2438 1199 2438 1191 2439 1194 2439 1192 2439 1601 2440 1177 2440 1599 2440 1197 2441 1199 2441 1171 2441 1601 2442 1599 2442 1198 2442 1197 2443 1171 2443 1190 2443 1027 2444 1191 2444 1026 2444 1032 2445 1209 2445 1200 2445 1032 2446 1200 2446 1202 2446 1034 2447 1577 2447 1201 2447 1034 2448 1202 2448 1577 2448 1034 2449 1032 2449 1202 2449 1035 2450 1201 2450 1586 2450 1035 2451 1034 2451 1201 2451 1205 2452 1203 2452 1204 2452 1205 2453 1586 2453 1203 2453 1205 2454 1035 2454 1586 2454 1036 2455 1582 2455 1579 2455 1036 2456 1204 2456 1582 2456 1036 2457 1205 2457 1204 2457 1206 2458 1579 2458 1578 2458 1206 2459 1036 2459 1579 2459 1030 2460 1578 2460 1585 2460 1030 2461 1206 2461 1578 2461 1031 2462 1585 2462 1584 2462 1031 2463 1030 2463 1585 2463 1207 2464 1584 2464 1208 2464 1207 2465 1031 2465 1584 2465 1208 2466 1583 2466 1581 2466 1033 2467 1581 2467 1580 2467 1033 2468 1208 2468 1581 2468 1169 2469 1033 2469 1580 2469 1212 2470 1217 2470 1043 2470 1213 2471 1112 2471 1387 2471 1213 2472 1043 2472 1112 2472 1213 2473 1212 2473 1043 2473 1211 2474 1213 2474 1387 2474 1211 2475 1387 2475 1619 2475 1388 2476 1210 2476 1620 2476 1388 2477 1620 2477 1619 2477 1619 2478 1622 2478 2138 2478 1619 2479 1620 2479 1622 2479 1044 2480 1227 2480 2534 2480 1659 2481 2534 2481 1227 2481 1673 2482 1666 2482 1215 2482 1215 2483 1211 2483 1673 2483 1673 2484 1211 2484 1671 2484 1215 2485 1045 2485 1214 2485 1211 2486 1619 2486 1671 2486 1214 2487 1212 2487 1213 2487 1215 2488 1214 2488 1211 2488 1214 2489 1213 2489 1211 2489 1218 2490 1048 2490 1217 2490 1048 2491 1216 2491 1217 2491 1219 2492 1218 2492 1217 2492 1050 2493 1049 2493 1046 2493 1049 2494 1220 2494 1046 2494 1221 2495 1050 2495 1222 2495 1050 2496 1046 2496 1222 2496 1052 2497 1221 2497 1223 2497 1221 2498 1222 2498 1223 2498 1051 2499 1052 2499 1045 2499 1052 2500 1223 2500 1045 2500 1225 2501 1227 2501 1044 2501 1225 2502 1044 2502 1224 2502 1044 2503 1053 2503 1224 2503 1225 2504 1224 2504 1226 2504 1051 2505 1225 2505 1052 2505 1052 2506 1225 2506 1226 2506 1671 2507 1674 2507 1673 2507 1672 2508 1674 2508 1671 2508 1619 2509 1229 2509 1671 2509 1228 2510 1666 2510 1673 2510 1256 2511 1233 2511 1109 2511 1256 2512 1680 2512 1233 2512 1231 2513 1109 2513 1230 2513 1231 2514 1256 2514 1109 2514 1232 2515 1678 2515 1676 2515 1232 2516 1676 2516 1677 2516 1232 2517 2144 2517 1679 2517 1232 2518 1679 2518 1678 2518 1354 2519 1242 2519 1234 2519 1263 2520 1234 2520 1235 2520 780 2521 1236 2521 1234 2521 1242 2522 780 2522 1234 2522 1236 2523 1237 2523 1234 2523 1234 2524 1235 2524 1237 2524 1240 2525 1241 2525 1242 2525 1239 2526 874 2526 1242 2526 1238 2527 1239 2527 1242 2527 1242 2528 780 2528 874 2528 1682 2529 1243 2529 1241 2529 1682 2530 1260 2530 1245 2530 1244 2531 1245 2531 1059 2531 1246 2532 1244 2532 1058 2532 1244 2533 1059 2533 1058 2533 1248 2534 1246 2534 1247 2534 1246 2535 1058 2535 1247 2535 1251 2536 1248 2536 1249 2536 1248 2537 1247 2537 1249 2537 1250 2538 1251 2538 1253 2538 1252 2539 1250 2539 1253 2539 1251 2540 1249 2540 1253 2540 1254 2541 1252 2541 1253 2541 1254 2542 1253 2542 1255 2542 1690 2543 1685 2543 1256 2543 1257 2544 1231 2544 1247 2544 1247 2545 1231 2545 1253 2545 1256 2546 1685 2546 1680 2546 1259 2547 1258 2547 1256 2547 1258 2548 1690 2548 1256 2548 1260 2549 1259 2549 1257 2549 1259 2550 1256 2550 1257 2550 1257 2551 1256 2551 1231 2551 1068 2552 1354 2552 1262 2552 1261 2553 1262 2553 1354 2553 1240 2554 1242 2554 1354 2554 1234 2555 1263 2555 1354 2555 1263 2556 890 2556 1354 2556 1354 2557 889 2557 890 2557 1683 2558 1240 2558 1354 2558 889 2559 1264 2559 1354 2559 1354 2560 1261 2560 1264 2560 1689 2561 1687 2561 1685 2561 1689 2562 1685 2562 1690 2562 1265 2563 1689 2563 1690 2563 1465 2564 1271 2564 1266 2564 1269 2565 1338 2565 1465 2565 1267 2566 1451 2566 1270 2566 1270 2567 1451 2567 1268 2567 1465 2568 1266 2568 1272 2568 1451 2569 1450 2569 1268 2569 1269 2570 1465 2570 1272 2570 1272 2571 1266 2571 1271 2571 1271 2572 1277 2572 1278 2572 1272 2573 1271 2573 1275 2573 1450 2574 1274 2574 1273 2574 1268 2575 1450 2575 1270 2575 1270 2576 1450 2576 1742 2576 1450 2577 1273 2577 1742 2577 1750 2578 1272 2578 1275 2578 1275 2579 1271 2579 1278 2579 1741 2580 1742 2580 1273 2580 1273 2581 1274 2581 1741 2581 1750 2582 1275 2582 1278 2582 1274 2583 1463 2583 1276 2583 1281 2584 1750 2584 1278 2584 1741 2585 1274 2585 1276 2585 1278 2586 1277 2586 1281 2586 1276 2587 1463 2587 1280 2587 1281 2588 1277 2588 1279 2588 1276 2589 1280 2589 1754 2589 1279 2590 1285 2590 1284 2590 1281 2591 1279 2591 1282 2591 1283 2592 1281 2592 1282 2592 1280 2593 1473 2593 1756 2593 1754 2594 1280 2594 1756 2594 1282 2595 1279 2595 1283 2595 1279 2596 1284 2596 1283 2596 1283 2597 1284 2597 1285 2597 1756 2598 1473 2598 1474 2598 1285 2599 1291 2599 1288 2599 1283 2600 1285 2600 1288 2600 1756 2601 1474 2601 1286 2601 1290 2602 1283 2602 1288 2602 1474 2603 1476 2603 1287 2603 1286 2604 1474 2604 1287 2604 1288 2605 1291 2605 1290 2605 1293 2606 1301 2606 1718 2606 1287 2607 1476 2607 1422 2607 1296 2608 1289 2608 1293 2608 1296 2609 1293 2609 1718 2609 1291 2610 1294 2610 1292 2610 1287 2611 1422 2611 1713 2611 1297 2612 1713 2612 1295 2612 1713 2613 1422 2613 1295 2613 1422 2614 1299 2614 1297 2614 1292 2615 1294 2615 1303 2615 1295 2616 1422 2616 1297 2616 1725 2617 1298 2617 1302 2617 1298 2618 1297 2618 1299 2618 1290 2619 1291 2619 1292 2619 1302 2620 1298 2620 1299 2620 1290 2621 1292 2621 1303 2621 1426 2622 1725 2622 1302 2622 1718 2623 1301 2623 1300 2623 1725 2624 1426 2624 1429 2624 1727 2625 1725 2625 1429 2625 1303 2626 1294 2626 1308 2626 1305 2627 1727 2627 1429 2627 1294 2628 1306 2628 1308 2628 1429 2629 1304 2629 1305 2629 1308 2630 1306 2630 1318 2630 1305 2631 1304 2631 1731 2631 1304 2632 1439 2632 1731 2632 1731 2633 1439 2633 1734 2633 1439 2634 1307 2634 1734 2634 1300 2635 1301 2635 1456 2635 1734 2636 1307 2636 1309 2636 1307 2637 1445 2637 1309 2637 1309 2638 1445 2638 1312 2638 1303 2639 1308 2639 1310 2639 1328 2640 1310 2640 1318 2640 1445 2641 1311 2641 1312 2641 1310 2642 1308 2642 1318 2642 1312 2643 1311 2643 1314 2643 1318 2644 1306 2644 1313 2644 1311 2645 1316 2645 1314 2645 1300 2646 1456 2646 1315 2646 1740 2647 1314 2647 1316 2647 1740 2648 1316 2648 1317 2648 1316 2649 1319 2649 1317 2649 1318 2650 1313 2650 1328 2650 1319 2651 1321 2651 1748 2651 1315 2652 1456 2652 1333 2652 1317 2653 1319 2653 1748 2653 1748 2654 1321 2654 1322 2654 1321 2655 1466 2655 1322 2655 1315 2656 1333 2656 1324 2656 1322 2657 1466 2657 1323 2657 1320 2658 1748 2658 1322 2658 1328 2659 1313 2659 1325 2659 1326 2660 1320 2660 1323 2660 1320 2661 1322 2661 1323 2661 1323 2662 1466 2662 1329 2662 1326 2663 1323 2663 1329 2663 1330 2664 1326 2664 1329 2664 1328 2665 1325 2665 1327 2665 1329 2666 1467 2666 1330 2666 1330 2667 1467 2667 1763 2667 1467 2668 1493 2668 1763 2668 1499 2669 584 2669 1493 2669 1333 2670 1334 2670 1332 2670 1324 2671 1333 2671 1332 2671 1327 2672 1325 2672 1719 2672 1325 2673 1331 2673 1719 2673 1719 2674 1331 2674 1335 2674 1336 2675 1719 2675 1335 2675 1335 2676 1331 2676 1438 2676 1335 2677 1438 2677 1336 2677 1332 2678 1334 2678 1338 2678 1336 2679 1438 2679 1337 2679 1267 2680 1336 2680 1337 2680 1332 2681 1338 2681 1269 2681 1438 2682 1443 2682 1267 2682 1337 2683 1438 2683 1267 2683 1443 2684 1451 2684 1267 2684 1339 2685 1820 2685 1768 2685 2096 2686 1339 2686 1768 2686 2197 2687 788 2687 1340 2687 1547 2688 1159 2688 1341 2688 650 2689 1547 2689 1342 2689 1063 2690 1342 2690 1344 2690 1063 2691 650 2691 1342 2691 1343 2692 1344 2692 1553 2692 1343 2693 1063 2693 1344 2693 1065 2694 1553 2694 1552 2694 1065 2695 1343 2695 1553 2695 1066 2696 1552 2696 1550 2696 1066 2697 1065 2697 1552 2697 1067 2698 1345 2698 1548 2698 1067 2699 1550 2699 1345 2699 1067 2700 1066 2700 1550 2700 1346 2701 1546 2701 1554 2701 1346 2702 1548 2702 1546 2702 1346 2703 1067 2703 1548 2703 886 2704 1554 2704 1348 2704 886 2705 1346 2705 1554 2705 1347 2706 1551 2706 1549 2706 1347 2707 1348 2707 1551 2707 1347 2708 886 2708 1348 2708 1062 2709 1549 2709 1349 2709 1062 2710 1347 2710 1549 2710 1064 2711 1349 2711 1545 2711 1064 2712 1062 2712 1349 2712 1340 2713 1064 2713 1545 2713 1068 2714 790 2714 1350 2714 1350 2715 1351 2715 790 2715 1350 2716 1353 2716 1351 2716 1350 2717 1352 2717 1353 2717 1354 2718 1068 2718 1683 2718 1068 2719 1350 2719 1683 2719 1841 2720 1069 2720 1833 2720 895 2721 1069 2721 1841 2721 718 2722 1355 2722 896 2722 1355 2723 716 2723 896 2723 717 2724 718 2724 1352 2724 718 2725 896 2725 1352 2725 1352 2726 896 2726 1350 2726 1357 2727 1356 2727 1878 2727 1356 2728 1081 2728 1878 2728 1360 2729 1073 2729 1361 2729 1081 2730 1078 2730 1878 2730 1878 2731 1078 2731 1362 2731 1078 2732 1358 2732 1362 2732 1358 2733 1077 2733 1362 2733 1362 2734 1077 2734 1359 2734 1890 2735 1360 2735 1361 2735 1363 2736 1357 2736 1878 2736 1362 2737 1359 2737 1073 2737 1873 2738 1878 2738 1882 2738 980 2739 1890 2739 1361 2739 1882 2740 1878 2740 1362 2740 1882 2741 1362 2741 1360 2741 1362 2742 1073 2742 1360 2742 1368 2743 904 2743 1364 2743 900 2744 1363 2744 1364 2744 1363 2745 1892 2745 1364 2745 1892 2746 1866 2746 1364 2746 1866 2747 1368 2747 1364 2747 902 2748 1365 2748 1369 2748 1365 2749 1086 2749 1369 2749 1086 2750 1366 2750 1370 2750 1366 2751 1367 2751 1370 2751 1367 2752 1089 2752 1370 2752 1089 2753 903 2753 1370 2753 1370 2754 903 2754 1368 2754 1370 2755 1090 2755 1369 2755 1372 2756 1371 2756 1373 2756 1375 2757 1376 2757 1374 2757 1376 2758 1703 2758 1374 2758 1374 2759 1708 2759 1375 2759 1375 2760 1377 2760 2209 2760 1708 2761 1378 2761 1375 2761 1093 2762 1102 2762 1128 2762 1093 2763 1128 2763 1130 2763 1095 2764 1093 2764 1130 2764 1397 2765 1127 2765 1386 2765 1397 2766 1379 2766 1127 2766 1393 2767 1386 2767 1125 2767 1393 2768 1397 2768 1386 2768 1395 2769 1125 2769 1126 2769 1395 2770 1393 2770 1125 2770 1391 2771 1126 2771 1380 2771 1391 2772 1395 2772 1126 2772 1392 2773 1380 2773 1124 2773 1392 2774 1391 2774 1380 2774 1905 2775 1392 2775 1124 2775 1382 2776 1908 2776 1381 2776 1382 2777 1907 2777 1906 2777 1382 2778 1381 2778 1907 2778 1389 2779 641 2779 642 2779 1383 2780 644 2780 642 2780 1110 2781 1385 2781 1384 2781 1210 2782 1388 2782 1597 2782 1384 2783 1385 2783 1232 2783 1232 2784 1677 2784 1382 2784 1677 2782 1908 2782 1382 2782 1232 2785 1382 2785 638 2785 1384 2786 1232 2786 638 2786 1382 2787 1126 2787 638 2787 638 2788 1126 2788 637 2788 637 2789 1126 2789 928 2789 928 2790 1126 2790 1386 2790 928 2791 1386 2791 634 2791 634 2792 1386 2792 633 2792 1122 2793 633 2793 946 2793 934 2794 823 2794 1387 2794 1387 2795 823 2795 1111 2795 633 2796 1122 2796 649 2796 1387 2797 1111 2797 1388 2797 1388 2798 1111 2798 640 2798 1388 2799 640 2799 1170 2799 649 2800 1153 2800 1074 2800 1170 2801 640 2801 700 2801 644 2802 647 2802 649 2802 1170 2803 700 2803 1389 2803 700 2804 641 2804 1389 2804 1390 2805 1910 2805 1911 2805 1915 2806 1914 2806 1916 2806 1914 2807 1911 2807 1916 2807 1910 2808 1394 2808 1911 2808 1401 2809 1397 2809 1398 2809 1396 2810 1398 2810 1392 2810 1398 2811 1397 2811 1393 2811 1396 2812 1392 2812 1905 2812 1131 2813 1399 2813 1379 2813 1398 2814 1393 2814 1395 2814 1911 2815 1394 2815 1396 2815 1396 2816 1905 2816 1911 2816 1401 2817 1131 2817 1379 2817 1401 2818 1379 2818 1397 2818 1398 2819 1395 2819 1391 2819 1911 2820 1905 2820 1916 2820 1398 2821 1391 2821 1392 2821 1900 2822 1130 2822 1399 2822 1901 2823 1900 2823 1129 2823 1900 2824 1399 2824 1129 2824 1902 2825 1901 2825 1400 2825 1901 2826 1129 2826 1400 2826 1897 2827 1902 2827 1131 2827 1902 2828 1400 2828 1131 2828 1899 2829 1897 2829 1132 2829 1897 2830 1131 2830 1132 2830 2236 2831 1899 2831 1401 2831 1899 2832 1132 2832 1401 2832 1955 2833 553 2833 1966 2833 1925 2834 1403 2834 1955 2834 1955 2835 553 2835 1403 2835 1402 2836 1403 2836 1925 2836 680 2837 1404 2837 1402 2837 1929 2838 680 2838 1402 2838 1920 2839 1402 2839 1925 2839 1405 2840 1407 2840 1409 2840 1407 2841 1138 2841 1409 2841 1408 2842 1137 2842 1136 2842 1409 2843 1138 2843 1134 2843 1136 2844 1137 2844 1409 2844 1137 2845 1405 2845 1409 2845 1406 2846 1136 2846 1409 2846 1417 2847 1411 2847 1410 2847 1411 2848 963 2848 1410 2848 963 2849 1135 2849 1410 2849 1135 2850 1938 2850 1410 2850 1938 2851 1417 2851 1410 2851 1150 2852 1412 2852 1155 2852 1151 2853 1142 2853 1414 2853 1142 2854 1145 2854 1414 2854 1145 2855 1413 2855 1414 2855 1413 2856 1415 2856 1414 2856 1415 2857 1416 2857 1414 2857 1416 2858 1418 2858 1414 2858 1418 2859 1417 2859 1414 2859 1155 2860 1419 2860 1928 2860 1151 2861 1414 2861 1412 2861 1412 2862 1414 2862 1420 2862 1420 2863 1414 2863 1953 2863 1420 2864 1953 2864 1949 2864 1412 2865 1420 2865 1419 2865 1155 2866 1412 2866 1419 2866 1420 2867 1949 2867 1419 2867 1419 2868 1949 2868 1946 2868 679 2869 1958 2869 1962 2869 1402 2870 1404 2870 1964 2870 1403 2871 1964 2871 553 2871 1403 2872 1402 2872 1964 2872 1299 2873 1422 2873 1423 2873 1422 2874 1421 2874 1423 2874 1423 2875 1421 2875 1424 2875 1291 2876 1982 2876 1294 2876 1302 2877 1299 2877 1425 2877 1299 2878 1423 2878 1425 2878 1294 2879 1982 2879 1306 2879 1423 2880 1424 2880 1971 2880 1425 2881 1423 2881 1971 2881 1306 2882 1982 2882 1981 2882 1302 2883 1425 2883 1428 2883 1426 2884 1302 2884 1428 2884 1428 2885 1425 2885 1971 2885 1313 2886 1306 2886 1427 2886 1306 2887 1981 2887 1433 2887 1427 2888 1306 2888 1433 2888 1426 2889 1428 2889 1430 2889 1429 2890 1426 2890 1430 2890 1428 2891 1971 2891 1430 2891 1971 2892 1970 2892 1430 2892 1325 2893 1313 2893 1435 2893 1313 2894 1427 2894 1435 2894 1435 2895 1427 2895 1433 2895 1431 2896 1530 2896 1432 2896 1304 2897 1429 2897 1430 2897 1430 2898 1970 2898 2378 2898 1432 2899 1530 2899 1437 2899 1530 2900 1978 2900 1437 2900 1304 2901 1430 2901 1436 2901 1331 2902 1325 2902 1435 2902 1435 2903 1433 2903 1434 2903 1430 2904 2378 2904 1436 2904 1331 2905 1435 2905 1438 2905 1439 2906 1304 2906 1436 2906 1438 2907 1435 2907 1434 2907 1440 2908 1432 2908 1437 2908 1438 2909 1434 2909 1447 2909 1439 2910 1436 2910 1441 2910 1307 2911 1439 2911 1441 2911 1289 2912 1440 2912 1444 2912 1436 2913 2378 2913 1441 2913 1441 2914 2378 2914 2430 2914 1307 2915 1441 2915 1442 2915 1440 2916 1437 2916 1444 2916 1293 2917 1289 2917 1444 2917 1442 2918 1441 2918 2430 2918 1445 2919 1307 2919 1442 2919 1443 2920 1438 2920 1447 2920 1444 2921 2439 2921 1293 2921 1445 2922 1442 2922 1446 2922 1446 2923 1442 2923 1448 2923 1442 2924 2430 2924 1448 2924 1311 2925 1445 2925 1446 2925 1311 2926 1446 2926 1449 2926 1443 2927 1447 2927 1451 2927 1452 2928 1293 2928 1455 2928 1293 2929 2439 2929 1455 2929 1451 2930 1447 2930 1459 2930 1449 2931 1446 2931 1448 2931 1456 2932 1301 2932 1452 2932 1301 2933 1293 2933 1452 2933 1316 2934 1311 2934 1449 2934 1450 2935 1451 2935 1457 2935 1274 2936 1450 2936 1457 2936 1316 2937 1449 2937 1453 2937 1457 2938 1451 2938 1459 2938 1452 2939 1455 2939 1456 2939 1449 2940 1448 2940 1453 2940 1453 2941 1448 2941 1972 2941 1319 2942 1316 2942 1453 2942 2435 2943 1454 2943 1459 2943 1447 2944 2435 2944 1459 2944 1274 2945 1457 2945 1460 2945 1274 2946 1460 2946 1463 2946 1333 2947 1456 2947 1461 2947 1319 2948 1453 2948 1462 2948 1456 2949 1455 2949 1461 2949 1461 2950 1455 2950 1973 2950 1453 2951 1972 2951 1462 2951 1334 2952 1333 2952 1461 2952 1462 2953 1972 2953 1458 2953 1457 2954 1459 2954 1460 2954 1454 2955 1471 2955 1459 2955 1460 2956 1459 2956 1471 2956 1319 2957 1462 2957 1321 2957 1338 2958 1334 2958 1461 2958 1462 2959 1458 2959 1321 2959 1338 2960 1461 2960 1464 2960 1460 2961 1471 2961 1463 2961 1461 2962 1973 2962 1464 2962 1321 2963 1458 2963 1975 2963 1464 2964 1973 2964 1969 2964 1466 2965 1321 2965 1975 2965 1465 2966 1338 2966 1464 2966 1280 2967 1463 2967 1470 2967 1465 2968 1464 2968 1468 2968 1329 2969 1466 2969 1469 2969 1468 2970 1464 2970 1969 2970 1463 2971 1471 2971 1470 2971 1271 2972 1465 2972 1468 2972 1280 2973 1470 2973 1473 2973 1467 2974 1329 2974 1469 2974 1469 2975 1466 2975 1975 2975 1277 2976 1271 2976 1468 2976 1976 2977 1469 2977 1975 2977 1969 2978 1983 2978 1468 2978 1277 2979 1468 2979 1475 2979 1470 2980 1471 2980 1977 2980 1470 2981 1977 2981 1473 2981 1474 2982 1473 2982 1472 2982 1472 2983 1473 2983 1977 2983 1279 2984 1277 2984 1475 2984 1475 2985 1468 2985 1983 2985 1474 2986 1472 2986 1476 2986 1476 2987 1472 2987 1421 2987 1475 2988 1983 2988 2392 2988 1472 2989 1977 2989 1421 2989 1285 2990 1279 2990 1475 2990 1285 2991 1475 2991 1982 2991 1982 2992 1475 2992 2392 2992 1422 2993 1476 2993 1421 2993 1291 2994 1285 2994 1982 2994 580 2995 1540 2995 1485 2995 1484 2996 1477 2996 1482 2996 1477 2997 1543 2997 1482 2997 1543 2998 1478 2998 1482 2998 1478 2999 1480 2999 1483 2999 1482 3000 1478 3000 1483 3000 1484 3001 1482 3001 1487 3001 1479 3002 580 3002 1485 3002 1481 3003 580 3003 1479 3003 1481 3004 1479 3004 1486 3004 605 3005 1484 3005 1487 3005 1487 3006 1482 3006 1483 3006 1486 3007 1479 3007 1485 3007 599 3008 1481 3008 1486 3008 605 3009 1487 3009 1489 3009 1485 3010 2319 3010 1486 3010 610 3011 605 3011 1489 3011 1487 3012 1483 3012 1489 3012 607 3013 599 3013 1488 3013 599 3014 1486 3014 1488 3014 1489 3015 1483 3015 2320 3015 1488 3016 1486 3016 1980 3016 1486 3017 2319 3017 1980 3017 610 3018 1489 3018 1490 3018 1491 3019 610 3019 1490 3019 614 3020 607 3020 1488 3020 1490 3021 1489 3021 2320 3021 614 3022 1488 3022 1494 3022 1467 3023 1469 3023 1492 3023 1490 3024 2320 3024 1968 3024 611 3025 1491 3025 1503 3025 1491 3026 1490 3026 1503 3026 1469 3027 1976 3027 1492 3027 1488 3028 1980 3028 1496 3028 1497 3029 614 3029 1494 3029 1503 3030 1490 3030 1968 3030 1493 3031 1467 3031 1492 3031 1488 3032 1496 3032 1494 3032 1493 3033 1492 3033 1500 3033 1500 3034 1492 3034 1495 3034 616 3035 611 3035 1503 3035 1498 3036 1497 3036 1494 3036 1976 3037 1501 3037 1495 3037 1492 3038 1976 3038 1495 3038 1503 3039 1968 3039 1505 3039 1499 3040 1493 3040 1500 3040 1498 3041 1494 3041 1502 3041 1500 3042 1495 3042 1508 3042 1494 3043 1496 3043 1502 3043 1504 3044 616 3044 1507 3044 1502 3045 1496 3045 2349 3045 616 3046 1503 3046 1507 3046 556 3047 1498 3047 1502 3047 1508 3048 1495 3048 1501 3048 1509 3049 1499 3049 1508 3049 1499 3050 1500 3050 1508 3050 1503 3051 1505 3051 1507 3051 556 3052 1502 3052 1506 3052 1506 3053 1502 3053 2349 3053 1509 3054 1508 3054 1510 3054 1507 3055 1505 3055 1512 3055 624 3056 1504 3056 1511 3056 565 3057 556 3057 1506 3057 1504 3058 1507 3058 1511 3058 1501 3059 1979 3059 1510 3059 1508 3060 1501 3060 1510 3060 1513 3061 1509 3061 1510 3061 1511 3062 1507 3062 1512 3062 1513 3063 1510 3063 1514 3063 1506 3064 2349 3064 1517 3064 570 3065 565 3065 1506 3065 1515 3066 1511 3066 1512 3066 631 3067 624 3067 1511 3067 1514 3068 1510 3068 1979 3068 587 3069 1513 3069 1514 3069 631 3070 1511 3070 1515 3070 587 3071 1514 3071 1519 3071 577 3072 570 3072 1516 3072 1519 3073 1514 3073 2354 3073 570 3074 1506 3074 1516 3074 1514 3075 1979 3075 2354 3075 1516 3076 1506 3076 1517 3076 1518 3077 587 3077 1519 3077 630 3078 631 3078 1515 3078 1515 3079 1512 3079 1974 3079 1516 3080 1517 3080 1524 3080 1519 3081 2354 3081 1520 3081 583 3082 577 3082 1516 3082 1522 3083 1518 3083 1520 3083 1518 3084 1519 3084 1520 3084 557 3085 630 3085 1523 3085 630 3086 1515 3086 1523 3086 583 3087 1516 3087 1521 3087 1523 3088 1515 3088 1974 3088 1516 3089 1524 3089 1521 3089 1520 3090 2354 3090 1522 3090 1522 3091 2354 3091 1525 3091 1528 3092 557 3092 1526 3092 557 3093 1523 3093 1526 3093 592 3094 1522 3094 1527 3094 1523 3095 1974 3095 1526 3095 1526 3096 1974 3096 1531 3096 1530 3097 1521 3097 1524 3097 1431 3098 1521 3098 1530 3098 593 3099 592 3099 1527 3099 562 3100 1528 3100 1529 3100 1528 3101 1526 3101 1529 3101 1530 3102 1524 3102 1978 3102 1527 3103 1522 3103 1525 3103 1529 3104 1526 3104 1531 3104 1527 3105 1525 3105 1534 3105 595 3106 593 3106 1532 3106 593 3107 1527 3107 1532 3107 1527 3108 1534 3108 1532 3108 566 3109 562 3109 1537 3109 562 3110 1529 3110 1537 3110 1529 3111 1531 3111 1537 3111 1537 3112 1531 3112 1535 3112 1533 3113 595 3113 1532 3113 574 3114 566 3114 1537 3114 1533 3115 1532 3115 1536 3115 1536 3116 1532 3116 1534 3116 598 3117 1533 3117 1536 3117 1536 3118 1534 3118 1541 3118 572 3119 574 3119 1542 3119 574 3120 1537 3120 1542 3120 1542 3121 1537 3121 1540 3121 1537 3122 1535 3122 1540 3122 1536 3123 1541 3123 1538 3123 1539 3124 598 3124 1538 3124 598 3125 1536 3125 1538 3125 1543 3126 1538 3126 1539 3126 576 3127 572 3127 1542 3127 576 3128 1542 3128 580 3128 1477 3129 1539 3129 1543 3129 1543 3130 1538 3130 1478 3130 1538 3131 1541 3131 1478 3131 580 3132 1542 3132 1540 3132 1541 3133 1480 3133 1478 3133 1782 3134 1577 3134 2091 3134 1544 3135 1782 3135 1577 3135 2570 3136 1545 3136 1967 3136 1548 3137 1554 3137 1546 3137 1548 3138 2304 3138 1554 3138 1341 3139 1342 3139 1547 3139 982 3140 1342 3140 1341 3140 1550 3141 1548 3141 1345 3141 1550 3142 2304 3142 1548 3142 1549 3143 1545 3143 1349 3143 1551 3144 1545 3144 1549 3144 1553 3145 1550 3145 1552 3145 1553 3146 2304 3146 1550 3146 1554 3147 1551 3147 1348 3147 1553 3148 1342 3148 2304 3148 1342 3149 1553 3149 1344 3149 1572 3150 1557 3150 1573 3150 1993 3151 1556 3151 1555 3151 1557 3152 1572 3152 1556 3152 1559 3153 1560 3153 1164 3153 1559 3154 1575 3154 1563 3154 1564 3155 1168 3155 1991 3155 1558 3156 1557 3156 1556 3156 1556 3157 1993 3157 1743 3157 1561 3158 1168 3158 1564 3158 1564 3159 1991 3159 1990 3159 1562 3160 1559 3160 1563 3160 1562 3161 1560 3161 1559 3161 1568 3162 1556 3162 1743 3162 1571 3163 1556 3163 1568 3163 1755 3164 1743 3164 1568 3164 1571 3165 1558 3165 1556 3165 1566 3166 1562 3166 1563 3166 1565 3167 1561 3167 1564 3167 1567 3168 1564 3168 1990 3168 1569 3169 1564 3169 1567 3169 1569 3170 1565 3170 1564 3170 1572 3171 1563 3171 1994 3171 1576 3172 1569 3172 1567 3172 1576 3173 1567 3173 1574 3173 1572 3174 1566 3174 1563 3174 1572 3175 1573 3175 1570 3175 1572 3176 1570 3176 1566 3176 1555 3177 1572 3177 1994 3177 1559 3178 1576 3178 1574 3178 1556 3179 1572 3179 1555 3179 1559 3180 1574 3180 1575 3180 1164 3181 1576 3181 1559 3181 1991 3182 1580 3182 1583 3182 1991 3183 1168 3183 1580 3183 1201 3184 1577 3184 1203 3184 2450 3185 1585 3185 1578 3185 1582 3186 2450 3186 1578 3186 1582 3187 1578 3187 1579 3187 1582 3188 1203 3188 2450 3188 1202 3189 1200 3189 2091 3189 1577 3190 1202 3190 2091 3190 1583 3191 1580 3191 1581 3191 1203 3192 1582 3192 1204 3192 1584 3193 1583 3193 1208 3193 1201 3194 1203 3194 1586 3194 1585 3195 1583 3195 1584 3195 1585 3196 2450 3196 1583 3196 1589 3197 1371 3197 1997 3197 1587 3198 1589 3198 1997 3198 1588 3199 1589 3199 1587 3199 1587 3200 1590 3200 1588 3200 1014 3201 802 3201 1587 3201 1592 3202 1014 3202 1591 3202 1591 3203 1179 3203 1592 3203 1591 3204 1593 3204 1179 3204 1594 3205 1608 3205 1178 3205 1594 3206 1591 3206 1608 3206 1999 3207 2780 3207 1595 3207 1172 3208 1999 3208 1595 3208 1596 3209 1172 3209 1595 3209 1388 3210 1598 3210 1597 3210 1113 3211 1177 3211 1172 3211 1999 3212 1172 3212 1606 3212 1175 3213 1176 3213 2002 3213 1173 3214 1175 3214 2004 3214 1175 3215 2002 3215 2004 3215 1601 3216 1173 3216 1600 3216 1173 3217 2004 3217 1600 3217 1174 3218 1601 3218 1603 3218 1601 3219 1600 3219 1603 3219 1177 3220 1174 3220 1602 3220 1174 3221 1603 3221 1602 3221 1605 3222 1177 3222 1604 3222 1177 3223 1602 3223 1604 3223 1605 3224 1604 3224 2005 3224 1605 3225 2005 3225 1607 3225 1606 3226 1605 3226 1607 3226 1608 3227 2020 3227 1181 3227 1610 3228 1611 3228 1612 3228 1183 3229 693 3229 2032 3229 1614 3230 1189 3230 1183 3230 2063 3231 1185 3231 2032 3231 1189 3232 1029 3232 1183 3232 1185 3233 1615 3233 2032 3233 1615 3234 1187 3234 2032 3234 1187 3235 1188 3235 1183 3235 2032 3236 1187 3236 1183 3236 1188 3237 1614 3237 1183 3237 2063 3238 2008 3238 1027 3238 1027 3239 2008 3239 1191 3239 1176 3240 1196 3240 2002 3240 1196 3241 2050 3241 2002 3241 2050 3242 2001 3242 2002 3242 1190 3243 1191 3243 1616 3243 1197 3244 1190 3244 1616 3244 1196 3245 1197 3245 1616 3245 1191 3246 2008 3246 1616 3246 2050 3247 1196 3247 1616 3247 2020 3248 1617 3248 2478 3248 2020 3249 1608 3249 1617 3249 1209 3250 2091 3250 1200 3250 2091 3251 1209 3251 1618 3251 2091 3252 2103 3252 1782 3252 1618 3253 2121 3253 2091 3253 2138 3254 2137 3254 1619 3254 2137 3255 1229 3255 1619 3255 1622 3256 1620 3256 1621 3256 2138 3257 1622 3257 2791 3257 1649 3258 1644 3258 1633 3258 1661 3259 1660 3259 1665 3259 1644 3260 1630 3260 1623 3260 1649 3261 1633 3261 1627 3261 1658 3262 1649 3262 1627 3262 1644 3263 1623 3263 1633 3263 1661 3264 1665 3264 1624 3264 1637 3265 1658 3265 1627 3265 1662 3266 1661 3266 1624 3266 1662 3267 1624 3267 1625 3267 1662 3268 1625 3268 1664 3268 1664 3269 1625 3269 1632 3269 1665 3270 2519 3270 1626 3270 1664 3271 1632 3271 1669 3271 1630 3272 1652 3272 1629 3272 1624 3273 1665 3273 1626 3273 1624 3274 1626 3274 1628 3274 1625 3275 1624 3275 1631 3275 1624 3276 1628 3276 1631 3276 1625 3277 1631 3277 1632 3277 1639 3278 1669 3278 1632 3278 1630 3279 1629 3279 1623 3279 1626 3280 2084 3280 1638 3280 1633 3281 1623 3281 1646 3281 1627 3282 1633 3282 1634 3282 1633 3283 1646 3283 1634 3283 1626 3284 1638 3284 1641 3284 1628 3285 1626 3285 1641 3285 1637 3286 1627 3286 1635 3286 1627 3287 1634 3287 1635 3287 2084 3288 2082 3288 1638 3288 1631 3289 1628 3289 1641 3289 1631 3290 1641 3290 1636 3290 2135 3291 2476 3291 1640 3291 1669 3292 1670 3292 1635 3292 1632 3293 1631 3293 1636 3293 1632 3294 1636 3294 1645 3294 1639 3295 1632 3295 1645 3295 1637 3296 1635 3296 1670 3296 1629 3297 1655 3297 1642 3297 1641 3298 1638 3298 2082 3298 1623 3299 1629 3299 1642 3299 1641 3300 2082 3300 1650 3300 1636 3301 1641 3301 1656 3301 1656 3302 1641 3302 1650 3302 1647 3303 2135 3303 1644 3303 2135 3304 1640 3304 1630 3304 1657 3305 1639 3305 1645 3305 1636 3306 1656 3306 1645 3306 1645 3307 1656 3307 1654 3307 1623 3308 1642 3308 1648 3308 1644 3309 2135 3309 1630 3309 1646 3310 1623 3310 1648 3310 1634 3311 1646 3311 1648 3311 1645 3312 1654 3312 1657 3312 1647 3313 1644 3313 1649 3313 1634 3314 1648 3314 1651 3314 1653 3315 1647 3315 1649 3315 1635 3316 1634 3316 1651 3316 1653 3317 1649 3317 1658 3317 1635 3318 1651 3318 1664 3318 1663 3319 1653 3319 1658 3319 1650 3320 2082 3320 2080 3320 1655 3321 2086 3321 1660 3321 1642 3322 1655 3322 1660 3322 1656 3323 1650 3323 1659 3323 1640 3324 2476 3324 1652 3324 1650 3325 2080 3325 1659 3325 1635 3326 1664 3326 1669 3326 1656 3327 1659 3327 1654 3327 1227 3328 1657 3328 1654 3328 2080 3329 2534 3329 1659 3329 1654 3330 1659 3330 1227 3330 1640 3331 1652 3331 1630 3331 1642 3332 1660 3332 1661 3332 1648 3333 1642 3333 1661 3333 1670 3334 1643 3334 1637 3334 1663 3335 1658 3335 1637 3335 1651 3336 1648 3336 1662 3336 1648 3337 1661 3337 1662 3337 1637 3338 1643 3338 1663 3338 1651 3339 1662 3339 1664 3339 1660 3340 2086 3340 2519 3340 1660 3341 2519 3341 1665 3341 1215 3342 1666 3342 2143 3342 1215 3343 2143 3343 1667 3343 1667 3344 1643 3344 1670 3344 1227 3345 1225 3345 1657 3345 1225 3346 1051 3346 1668 3346 1051 3347 1045 3347 1668 3347 1045 3348 1215 3348 1668 3348 1639 3349 1657 3349 1668 3349 1669 3350 1639 3350 1668 3350 1670 3351 1669 3351 1668 3351 1215 3352 1667 3352 1668 3352 1667 3353 1670 3353 1668 3353 1657 3354 1225 3354 1668 3354 2137 3355 2555 3355 2142 3355 1229 3356 2137 3356 2142 3356 1671 3357 1229 3357 2141 3357 1229 3358 2142 3358 2141 3358 1671 3359 2141 3359 1672 3359 1673 3360 1674 3360 1675 3360 1674 3361 1672 3361 1675 3361 1228 3362 1673 3362 2140 3362 1673 3363 1675 3363 2140 3363 1666 3364 1228 3364 2143 3364 2795 3365 1679 3365 2144 3365 2144 3366 1232 3366 1685 3366 1685 3367 1232 3367 1233 3367 1680 3368 1685 3368 1233 3368 1685 3369 1684 3369 2144 3369 1241 3370 1240 3370 1682 3370 1825 3371 1836 3371 1681 3371 1836 3372 1846 3372 1681 3372 1846 3373 2145 3373 1681 3373 1682 3374 1240 3374 1681 3374 1259 3375 1260 3375 1681 3375 1260 3376 1682 3376 1681 3376 2145 3377 2148 3377 1681 3377 2148 3378 1259 3378 1681 3378 1240 3379 1825 3379 1681 3379 2148 3380 2149 3380 1259 3380 2149 3381 1258 3381 1259 3381 1825 3382 1240 3382 1683 3382 2152 3383 2562 3383 1684 3383 2152 3384 1684 3384 1685 3384 2153 3385 2152 3385 1685 3385 1686 3386 2153 3386 1687 3386 2153 3387 1685 3387 1687 3387 1688 3388 1686 3388 1689 3388 1686 3389 1687 3389 1689 3389 2150 3390 1688 3390 1265 3390 1688 3391 1689 3391 1265 3391 2149 3392 2150 3392 1690 3392 2150 3393 1265 3393 1690 3393 2149 3394 1690 3394 1258 3394 1705 3395 1707 3395 1706 3395 2133 3396 2479 3396 1692 3396 1377 3397 1375 3397 1694 3397 1375 3398 1378 3398 1694 3398 1708 3399 1706 3399 1691 3399 1704 3400 2160 3400 1693 3400 1378 3401 1708 3401 1691 3401 1691 3402 1706 3402 1696 3402 1704 3403 1693 3403 1339 3403 1706 3404 1707 3404 1696 3404 1691 3405 1704 3405 1339 3405 2160 3406 2163 3406 1693 3406 2163 3407 1820 3407 1693 3407 1693 3408 1820 3408 1339 3408 2096 3409 2482 3409 1339 3409 1378 3410 1691 3410 1695 3410 1694 3411 1378 3411 1695 3411 1707 3412 2159 3412 1697 3412 1377 3413 1694 3413 1700 3413 1694 3414 1695 3414 1700 3414 1707 3415 1697 3415 1696 3415 2159 3416 1702 3416 1697 3416 1704 3417 1696 3417 1697 3417 1702 3418 1704 3418 1697 3418 1699 3419 1377 3419 1700 3419 2479 3420 1699 3420 1698 3420 1695 3421 2479 3421 1698 3421 1696 3422 1704 3422 1691 3422 2133 3423 1692 3423 1701 3423 1692 3424 1691 3424 1701 3424 1691 3425 1339 3425 1701 3425 1339 3426 2482 3426 1701 3426 2482 3427 2133 3427 1701 3427 1699 3428 1700 3428 1698 3428 1700 3429 1695 3429 1698 3429 1703 3430 1376 3430 1705 3430 1374 3431 1703 3431 1705 3431 1374 3432 1705 3432 1706 3432 1376 3433 2159 3433 1705 3433 1702 3434 2160 3434 1704 3434 1374 3435 1706 3435 1708 3435 2479 3436 1695 3436 1692 3436 1695 3437 1691 3437 1692 3437 1705 3438 2159 3438 1707 3438 1758 3439 1756 3439 1286 3439 1985 3440 1967 3440 1757 3440 1709 3441 1988 3441 1758 3441 1303 3442 1757 3442 1290 3442 1985 3443 1757 3443 1303 3443 1758 3444 1286 3444 1287 3444 1715 3445 1758 3445 1287 3445 1711 3446 1814 3446 613 3446 1715 3447 1709 3447 1758 3447 1717 3448 1806 3448 1814 3448 1715 3449 1287 3449 1713 3449 1717 3450 1814 3450 1711 3450 1303 3451 1710 3451 1985 3451 1712 3452 1303 3452 1310 3452 1717 3453 1711 3453 619 3453 1712 3454 1710 3454 1303 3454 1712 3455 1310 3455 1328 3455 1297 3456 1715 3456 1713 3456 1717 3457 619 3457 1296 3457 1714 3458 2177 3458 1715 3458 1712 3459 1328 3459 1327 3459 1710 3460 1712 3460 1716 3460 1716 3461 1712 3461 1327 3461 1714 3462 1715 3462 1297 3462 1714 3463 1297 3463 1298 3463 1718 3464 1717 3464 1296 3464 1994 3465 1717 3465 1718 3465 1723 3466 1710 3466 1716 3466 1720 3467 1716 3467 1327 3467 1720 3468 1327 3468 1719 3468 1723 3469 1716 3469 1720 3469 2177 3470 1714 3470 1721 3470 1722 3471 1298 3471 1725 3471 1722 3472 1714 3472 1298 3472 1722 3473 1721 3473 1714 3473 1726 3474 1718 3474 1300 3474 1726 3475 1994 3475 1718 3475 2187 3476 1723 3476 1720 3476 1727 3477 1722 3477 1725 3477 1336 3478 1720 3478 1719 3478 1724 3479 1720 3479 1336 3479 2187 3480 1720 3480 1724 3480 1726 3481 1300 3481 1315 3481 1724 3482 1336 3482 1267 3482 2173 3483 1722 3483 1727 3483 1730 3484 1727 3484 1305 3484 1730 3485 2173 3485 1727 3485 1729 3486 1724 3486 1267 3486 1324 3487 1994 3487 1726 3487 1324 3488 1726 3488 1315 3488 1728 3489 2187 3489 1724 3489 1730 3490 1305 3490 1731 3490 1728 3491 1724 3491 1729 3491 1729 3492 1267 3492 1270 3492 1732 3493 1729 3493 1270 3493 1733 3494 1731 3494 1734 3494 1733 3495 1730 3495 1731 3495 2173 3496 1730 3496 1733 3496 1736 3497 1729 3497 1732 3497 1736 3498 1728 3498 1729 3498 1743 3499 1324 3499 1332 3499 1735 3500 1733 3500 1734 3500 1735 3501 1734 3501 1309 3501 1735 3502 2173 3502 1733 3502 1732 3503 1270 3503 1742 3503 2182 3504 2173 3504 1735 3504 1743 3505 1332 3505 1269 3505 1736 3506 1737 3506 1728 3506 1735 3507 1309 3507 1312 3507 1738 3508 1735 3508 1312 3508 1739 3509 2182 3509 1735 3509 1744 3510 1732 3510 1742 3510 1739 3511 1735 3511 1738 3511 1738 3512 1312 3512 1314 3512 1744 3513 1736 3513 1732 3513 1744 3514 1737 3514 1736 3514 1738 3515 1314 3515 1740 3515 1746 3516 1738 3516 1740 3516 1744 3517 1742 3517 1741 3517 1746 3518 1739 3518 1738 3518 1269 3519 1755 3519 1743 3519 1746 3520 1740 3520 1317 3520 2192 3521 1739 3521 1746 3521 1744 3522 1741 3522 1276 3522 1747 3523 2192 3523 1746 3523 1745 3524 1317 3524 1748 3524 1745 3525 1746 3525 1317 3525 1747 3526 1746 3526 1745 3526 1750 3527 1269 3527 1272 3527 1751 3528 1744 3528 1276 3528 1755 3529 1269 3529 1750 3529 1749 3530 1747 3530 1745 3530 1320 3531 1745 3531 1748 3531 1320 3532 1749 3532 1745 3532 1751 3533 1737 3533 1744 3533 1752 3534 1320 3534 1326 3534 1987 3535 1737 3535 1751 3535 1752 3536 1749 3536 1320 3536 1326 3537 1330 3537 1753 3537 1751 3538 1276 3538 1754 3538 1752 3539 1326 3539 1753 3539 1752 3540 1753 3540 2169 3540 1749 3541 1752 3541 2169 3541 1967 3542 1755 3542 1750 3542 1967 3543 1750 3543 1281 3543 1987 3544 1751 3544 1754 3544 1987 3545 1754 3545 1756 3545 1757 3546 1281 3546 1283 3546 1757 3547 1967 3547 1281 3547 1758 3548 1988 3548 1987 3548 1758 3549 1987 3549 1756 3549 1290 3550 1757 3550 1283 3550 1761 3551 1819 3551 1760 3551 1762 3552 1759 3552 618 3552 1766 3553 1818 3553 612 3553 2102 3554 2108 3554 1759 3554 1766 3555 1815 3555 1818 3555 1771 3556 1761 3556 1760 3556 1771 3557 2125 3557 1761 3557 1772 3558 1815 3558 1766 3558 1764 3559 1753 3559 1330 3559 1764 3560 1330 3560 1763 3560 1764 3561 2169 3561 1753 3561 2172 3562 2169 3562 1764 3562 1765 3563 1763 3563 1493 3563 1765 3564 1764 3564 1763 3564 554 3565 1759 3565 1762 3565 2172 3566 1764 3566 1765 3566 1771 3567 1760 3567 1767 3567 1820 3568 2172 3568 1765 3568 1768 3569 1765 3569 1493 3569 1768 3570 1820 3570 1765 3570 584 3571 1768 3571 1493 3571 1775 3572 2125 3572 1771 3572 1780 3573 1759 3573 554 3573 1774 3574 584 3574 1769 3574 1774 3575 1768 3575 584 3575 1780 3576 2102 3576 1759 3576 1774 3577 1770 3577 1768 3577 1774 3578 1769 3578 585 3578 1782 3579 2102 3579 1780 3579 1779 3580 1774 3580 585 3580 1778 3581 1772 3581 1766 3581 1779 3582 1773 3582 1770 3582 1779 3583 1770 3583 1774 3583 1778 3584 1766 3584 617 3584 1776 3585 585 3585 588 3585 1776 3586 1779 3586 585 3586 1781 3587 1771 3587 1767 3587 1786 3588 1771 3588 1781 3588 1786 3589 1775 3589 1771 3589 1778 3590 617 3590 1777 3590 1773 3591 1779 3591 1776 3591 1783 3592 588 3592 590 3592 1783 3593 1776 3593 588 3593 1785 3594 1776 3594 1783 3594 1785 3595 1773 3595 1776 3595 1992 3596 1772 3596 1778 3596 1782 3597 1780 3597 563 3597 1784 3598 1785 3598 1783 3598 1788 3599 590 3599 1787 3599 1788 3600 1783 3600 590 3600 1784 3601 1783 3601 1788 3601 1792 3602 1775 3602 1786 3602 1786 3603 1781 3603 1796 3603 1789 3604 1778 3604 1777 3604 2114 3605 1784 3605 1788 3605 1795 3606 1778 3606 1789 3606 1791 3607 1787 3607 1790 3607 1791 3608 1788 3608 1787 3608 1992 3609 1778 3609 1795 3609 2114 3610 1788 3610 1791 3610 1796 3611 1792 3611 1786 3611 1793 3612 1790 3612 597 3612 1793 3613 1791 3613 1790 3613 1991 3614 1782 3614 563 3614 2114 3615 1791 3615 1793 3615 1794 3616 597 3616 1798 3616 1794 3617 1793 3617 597 3617 571 3618 1991 3618 563 3618 1797 3619 2114 3619 1793 3619 1797 3620 1793 3620 1794 3620 1802 3621 1789 3621 629 3621 1794 3622 1798 3622 1800 3622 1802 3623 1795 3623 1789 3623 1799 3624 1794 3624 1800 3624 1989 3625 1796 3625 594 3625 578 3626 1991 3626 571 3626 1989 3627 1792 3627 1796 3627 1799 3628 1797 3628 1794 3628 1799 3629 1800 3629 1803 3629 2132 3630 1795 3630 1802 3630 1802 3631 629 3631 1801 3631 2123 3632 1797 3632 1799 3632 1804 3633 1799 3633 1803 3633 1809 3634 1802 3634 1801 3634 601 3635 1989 3635 594 3635 1804 3636 2123 3636 1799 3636 1804 3637 1803 3637 600 3637 1809 3638 2132 3638 1802 3638 1990 3639 1991 3639 578 3639 1805 3640 1804 3640 600 3640 1989 3641 601 3641 608 3641 586 3642 1990 3642 578 3642 1809 3643 1801 3643 1813 3643 2123 3644 1804 3644 1805 3644 1805 3645 600 3645 603 3645 1807 3646 2123 3646 1805 3646 1990 3647 586 3647 1806 3647 1809 3648 1810 3648 2132 3648 1811 3649 1805 3649 603 3649 1808 3650 1805 3650 1811 3650 1808 3651 1807 3651 1805 3651 1814 3652 586 3652 596 3652 1817 3653 603 3653 604 3653 1817 3654 1811 3654 603 3654 1806 3655 586 3655 1814 3655 1812 3656 1989 3656 608 3656 2108 3657 1989 3657 1812 3657 1816 3658 1809 3658 1813 3658 1808 3659 1811 3659 1817 3659 1810 3660 1809 3660 1816 3660 596 3661 613 3661 1814 3661 1817 3662 604 3662 609 3662 1815 3663 1808 3663 1817 3663 1759 3664 2108 3664 1812 3664 1818 3665 1817 3665 609 3665 2125 3666 1810 3666 1816 3666 1761 3667 1816 3667 1819 3667 1759 3668 1812 3668 618 3668 2125 3669 1816 3669 1761 3669 1815 3670 1817 3670 1818 3670 612 3671 1818 3671 609 3671 1340 3672 2570 3672 2197 3672 2570 3673 1340 3673 1545 3673 1838 3674 1826 3674 1821 3674 1839 3675 1830 3675 1835 3675 1822 3676 1848 3676 1832 3676 1825 3677 1839 3677 1834 3677 1845 3678 1822 3678 1829 3678 1830 3679 1843 3679 1837 3679 1844 3680 1845 3680 1829 3680 2205 3681 1823 3681 1849 3681 1822 3682 1832 3682 1829 3682 1826 3683 1831 3683 1842 3683 1850 3684 1838 3684 1840 3684 1846 3685 1827 3685 2145 3685 1827 3686 1850 3686 1840 3686 1836 3687 1844 3687 1846 3687 1833 3688 2198 3688 1350 3688 1848 3689 1824 3689 1832 3689 1683 3690 1828 3690 1839 3690 1834 3691 1839 3691 1835 3691 1683 3692 1839 3692 1825 3692 1825 3693 1834 3693 1836 3693 1849 3694 1823 3694 1843 3694 1834 3695 1835 3695 1844 3695 1832 3696 1824 3696 1826 3696 1834 3697 1844 3697 1836 3697 2198 3698 2586 3698 1350 3698 1830 3699 1837 3699 2203 3699 1840 3700 1838 3700 1821 3700 1832 3701 1826 3701 1838 3701 1830 3702 2203 3702 1822 3702 1826 3703 1842 3703 1851 3703 1829 3704 1850 3704 1844 3704 1832 3705 1838 3705 1850 3705 1829 3706 1832 3706 1850 3706 1824 3707 1831 3707 1826 3707 1828 3708 1849 3708 1839 3708 1821 3709 1826 3709 1851 3709 1835 3710 1830 3710 1845 3710 1830 3711 1822 3711 1845 3711 1841 3712 1833 3712 896 3712 1840 3713 1821 3713 1847 3713 896 3714 1833 3714 1350 3714 1835 3715 1845 3715 1844 3715 1846 3716 1844 3716 1827 3716 1844 3717 1850 3717 1827 3717 1849 3718 1843 3718 1830 3718 1821 3719 1851 3719 1847 3719 2586 3720 2205 3720 1849 3720 1350 3721 2586 3721 1849 3721 1842 3722 1852 3722 1851 3722 1839 3723 1849 3723 1830 3723 1350 3724 1849 3724 1828 3724 1827 3725 1840 3725 2157 3725 1822 3726 2203 3726 1848 3726 1840 3727 1847 3727 2157 3727 1827 3728 2157 3728 2145 3728 1350 3729 1828 3729 1683 3729 1878 3730 1873 3730 1874 3730 1868 3731 1893 3731 1853 3731 1857 3732 1870 3732 1872 3732 1870 3733 1891 3733 1872 3733 1886 3734 1895 3734 1860 3734 1868 3735 1853 3735 1862 3735 1878 3736 1874 3736 1856 3736 1880 3737 1868 3737 1862 3737 1893 3738 1875 3738 1896 3738 1888 3739 1889 3739 1859 3739 1371 3740 1372 3740 1853 3740 1856 3741 1864 3741 1863 3741 1857 3742 1872 3742 1894 3742 1887 3743 1886 3743 1860 3743 1862 3744 1853 3744 1372 3744 1894 3745 1880 3745 1862 3745 1889 3746 1894 3746 1859 3746 1896 3747 1858 3747 1997 3747 1894 3748 1862 3748 1867 3748 1895 3749 1884 3749 1879 3749 1888 3750 1859 3750 1864 3750 1890 3751 1887 3751 1860 3751 1862 3752 1372 3752 1373 3752 1883 3753 1893 3753 1868 3753 1869 3754 1360 3754 1870 3754 1360 3755 1890 3755 1870 3755 1859 3756 1894 3756 1867 3756 1858 3757 1854 3757 1997 3757 1947 3758 1885 3758 1861 3758 1883 3759 1868 3759 1880 3759 1891 3760 1883 3760 1880 3760 1862 3761 1373 3761 1865 3761 1859 3762 1867 3762 1877 3762 1867 3763 1862 3763 1865 3763 1893 3764 1896 3764 1853 3764 1360 3765 1869 3765 1882 3765 1867 3766 1865 3766 1881 3766 1877 3767 1867 3767 1881 3767 1857 3768 1894 3768 1889 3768 1890 3769 1860 3769 1871 3769 1860 3770 1895 3770 1879 3770 1870 3771 1890 3771 1871 3771 1878 3772 1856 3772 1863 3772 1866 3773 1892 3773 1863 3773 1864 3774 1859 3774 1877 3774 1860 3775 1879 3775 1876 3775 980 3776 1861 3776 1886 3776 1872 3777 1891 3777 1880 3777 1861 3778 1885 3778 1886 3778 1874 3779 1889 3779 1888 3779 1864 3780 1877 3780 1370 3780 1869 3781 1870 3781 1857 3781 1882 3782 1869 3782 1857 3782 1878 3783 1863 3783 1892 3783 1864 3784 1370 3784 1863 3784 1997 3785 1854 3785 1995 3785 1879 3786 1875 3786 1893 3786 1890 3787 980 3787 1887 3787 1881 3788 1091 3788 1877 3788 1872 3789 1880 3789 1894 3789 1871 3790 1860 3790 1883 3790 1860 3791 1876 3791 1883 3791 1886 3792 1885 3792 1895 3792 1876 3793 1879 3793 1893 3793 1877 3794 1091 3794 1370 3794 1863 3795 1370 3795 1866 3795 1873 3796 1882 3796 1874 3796 1885 3797 1884 3797 1895 3797 1997 3798 1995 3798 1855 3798 1853 3799 1896 3799 1997 3799 980 3800 1886 3800 1887 3800 1882 3801 1857 3801 1889 3801 1882 3802 1889 3802 1874 3802 1997 3803 1371 3803 1853 3803 1370 3804 1368 3804 1866 3804 1871 3805 1883 3805 1891 3805 1870 3806 1871 3806 1891 3806 1892 3807 1363 3807 1878 3807 1875 3808 1858 3808 1896 3808 1874 3809 1888 3809 1856 3809 1856 3810 1888 3810 1864 3810 1883 3811 1876 3811 1893 3811 1130 3812 1902 3812 1898 3812 1902 3813 1897 3813 1898 3813 1897 3814 1899 3814 1898 3814 1899 3815 2236 3815 1898 3815 1900 3816 1901 3816 1130 3816 2236 3817 2234 3817 1898 3817 1901 3818 1902 3818 1130 3818 2216 3819 679 3819 682 3819 682 3820 1903 3820 2216 3820 2235 3821 1903 3821 1904 3821 1382 3822 1906 3822 1916 3822 1124 3823 1382 3823 1916 3823 1124 3824 1916 3824 1905 3824 1906 3825 2284 3825 1916 3825 1907 3826 2804 3826 1906 3826 2808 3827 1907 3827 1381 3827 1909 3828 1918 3828 1394 3828 2287 3829 1909 3829 1910 3829 1909 3830 1394 3830 1910 3830 2288 3831 2287 3831 1390 3831 2287 3832 1910 3832 1390 3832 1913 3833 2288 3833 1911 3833 2288 3834 1390 3834 1911 3834 1912 3835 1913 3835 1914 3835 1913 3836 1911 3836 1914 3836 2289 3837 1912 3837 1915 3837 1912 3838 1914 3838 1915 3838 2290 3839 2289 3839 1916 3839 2289 3840 1915 3840 1916 3840 1917 3841 2290 3841 2284 3841 2290 3842 1916 3842 2284 3842 2230 3843 2236 3843 1401 3843 1918 3844 2293 3844 1396 3844 1918 3845 1396 3845 1394 3845 2293 3846 2267 3846 1919 3846 1398 3847 1396 3847 1919 3847 1401 3848 1398 3848 1919 3848 2230 3849 1401 3849 1919 3849 1396 3850 2293 3850 1919 3850 1925 3851 1948 3851 1957 3851 1929 3852 1922 3852 1923 3852 1933 3853 2621 3853 1930 3853 2628 3854 1947 3854 1861 3854 2208 3855 1924 3855 1926 3855 1948 3856 1933 3856 1930 3856 1938 3857 1136 3857 1940 3857 1136 3858 1950 3858 1940 3858 1943 3859 2628 3859 1861 3859 1934 3860 1937 3860 1935 3860 1937 3861 1939 3861 1935 3861 1932 3862 1952 3862 1939 3862 1929 3863 1402 3863 1922 3863 1966 3864 2597 3864 2604 3864 1931 3865 1929 3865 1923 3865 1943 3866 1861 3866 1928 3866 1948 3867 1930 3867 1936 3867 1921 3868 1931 3868 1945 3868 1956 3869 1932 3869 1941 3869 1920 3870 1925 3870 1956 3870 1925 3871 1957 3871 1956 3871 1136 3872 1406 3872 1950 3872 2604 3873 1927 3873 1933 3873 1931 3874 1934 3874 1944 3874 2604 3875 1933 3875 1966 3875 1926 3876 1924 3876 1943 3876 1922 3877 1956 3877 1941 3877 1950 3878 1921 3878 1945 3878 1936 3879 1926 3879 1943 3879 1943 3880 1924 3880 2628 3880 1928 3881 1861 3881 980 3881 1402 3882 1920 3882 1956 3882 1930 3883 2621 3883 2208 3883 1931 3884 1944 3884 1945 3884 1950 3885 1945 3885 1940 3885 1966 3886 1933 3886 1948 3886 1406 3887 1133 3887 1921 3887 1934 3888 1935 3888 1944 3888 1922 3889 1941 3889 1937 3889 1943 3890 1928 3890 1954 3890 1923 3891 1922 3891 1937 3891 1936 3892 1943 3892 1954 3892 1954 3893 1928 3893 1419 3893 1923 3894 1937 3894 1934 3894 1939 3895 1952 3895 1946 3895 1955 3896 1966 3896 1948 3896 1402 3897 1956 3897 1922 3897 1957 3898 1948 3898 1936 3898 1935 3899 1939 3899 1942 3899 1942 3900 1939 3900 1946 3900 1930 3901 2208 3901 1926 3901 1925 3902 1955 3902 1948 3902 1927 3903 2621 3903 1933 3903 1952 3904 1954 3904 1419 3904 1406 3905 1921 3905 1950 3905 1133 3906 1929 3906 1931 3906 1940 3907 1945 3907 1951 3907 1952 3908 1419 3908 1946 3908 1932 3909 1936 3909 1954 3909 1133 3910 1931 3910 1921 3910 1937 3911 1941 3911 1939 3911 1938 3912 1940 3912 1951 3912 1941 3913 1932 3913 1939 3913 1944 3914 1935 3914 1949 3914 1935 3915 1942 3915 1949 3915 1944 3916 1949 3916 1953 3916 1945 3917 1944 3917 1953 3917 1942 3918 1946 3918 1949 3918 1931 3919 1923 3919 1934 3919 1945 3920 1953 3920 1951 3920 1936 3921 1930 3921 1926 3921 1932 3922 1954 3922 1952 3922 1951 3923 1953 3923 1414 3923 1956 3924 1957 3924 1932 3924 1957 3925 1936 3925 1932 3925 1417 3926 1938 3926 1951 3926 1938 3927 1135 3927 1136 3927 1414 3928 1417 3928 1951 3928 2298 3929 1962 3929 1958 3929 1965 3930 2649 3930 1964 3930 1959 3931 1961 3931 2298 3931 1963 3932 2298 3932 2649 3932 1963 3933 685 3933 2298 3933 685 3934 684 3934 1959 3934 685 3935 1959 3935 2298 3935 1960 3936 1963 3936 2649 3936 1966 3937 1964 3937 2649 3937 1965 3938 1960 3938 2649 3938 1961 3939 1962 3939 2298 3939 2301 3940 678 3940 982 3940 2320 3941 2369 3941 1968 3941 2372 3942 2430 3942 2378 3942 1968 3943 2369 3943 2365 3943 1448 3944 2430 3944 2426 3944 1968 3945 2365 3945 1505 3945 1448 3946 2426 3946 1972 3946 1505 3947 2365 3947 2361 3947 1505 3948 2361 3948 1512 3948 1970 3949 1971 3949 1424 3949 2424 3950 1973 3950 1455 3950 1458 3951 1972 3951 1975 3951 1975 3952 1972 3952 2400 3952 1512 3953 2361 3953 2343 3953 1424 3954 1421 3954 2399 3954 1512 3955 2343 3955 1974 3955 2424 3956 1455 3956 2439 3956 1975 3957 2400 3957 1976 3957 2399 3958 1421 3958 1977 3958 1974 3959 2343 3959 1531 3959 1976 3960 2400 3960 1501 3960 2439 3961 1444 3961 2374 3961 2413 3962 2399 3962 1977 3962 1501 3963 2400 3963 2705 3963 2374 3964 1444 3964 1437 3964 1531 3965 2343 3965 2338 3965 2413 3966 1977 3966 1471 3966 1531 3967 2338 3967 1535 3967 1501 3968 2705 3968 1979 3968 2374 3969 1437 3969 1978 3969 1979 3970 2705 3970 2362 3970 1535 3971 2338 3971 2330 3971 1454 3972 2413 3972 1471 3972 1979 3973 2362 3973 2354 3973 1535 3974 2330 3974 1540 3974 1540 3975 2330 3975 2327 3975 1540 3976 2327 3976 1485 3976 1524 3977 1517 3977 2346 3977 1485 3978 2327 3978 2319 3978 1525 3979 2354 3979 2347 3979 1517 3980 2349 3980 2346 3980 2376 3981 2435 3981 1447 3981 2349 3982 1496 3982 2363 3982 1980 3983 2319 3983 2368 3983 1525 3984 2347 3984 1534 3984 2376 3985 1447 3985 1434 3985 2363 3986 1496 3986 2368 3986 1496 3987 1980 3987 2368 3987 1534 3988 2347 3988 2336 3988 2376 3989 1434 3989 1433 3989 2376 3990 1433 3990 1981 3990 1534 3991 2336 3991 1541 3991 1541 3992 2336 3992 1480 3992 2385 3993 1981 3993 1982 3993 1480 3994 2325 3994 1483 3994 2385 3995 1982 3995 2392 3995 1483 3996 2325 3996 2320 3996 2440 3997 1544 3997 1782 3997 1984 3998 2452 3998 1991 3998 1991 3999 2451 3999 1984 3999 2176 4000 1967 4000 1985 4000 1717 4001 1994 4001 1563 4001 2164 4002 1985 4002 1710 4002 1203 4003 675 4003 674 4003 1577 4004 675 4004 1203 4004 1577 4005 1544 4005 675 4005 1987 4006 1986 4006 1737 4006 1575 4007 1717 4007 1563 4007 1988 4008 1986 4008 1987 4008 1574 4009 1717 4009 1575 4009 2179 4010 2164 4010 1710 4010 1806 4011 1717 4011 1574 4011 1567 4012 1806 4012 1574 4012 1989 4013 2108 4013 2122 4013 1723 4014 2179 4014 1710 4014 1792 4015 1989 4015 2122 4015 2110 4016 1792 4016 2122 4016 1990 4017 1806 4017 1567 4017 2099 4018 1792 4018 2110 4018 1775 4019 1792 4019 2099 4019 2125 4020 1775 4020 2099 4020 2187 4021 2179 4021 1723 4021 2656 4022 1967 4022 1545 4022 1992 4023 1795 4023 2132 4023 1993 4024 1324 4024 1743 4024 1551 4025 2656 4025 1545 4025 1342 4026 982 4026 678 4026 1555 4027 1324 4027 1993 4027 1782 4028 1991 4028 2452 4028 1554 4029 2310 4029 2656 4029 1554 4030 2656 4030 1551 4030 1555 4031 1994 4031 1324 4031 1554 4032 2304 4032 2310 4032 2075 4033 2771 4033 1995 4033 2771 4034 1855 4034 1995 4034 1014 4035 1587 4035 2463 4035 1591 4036 1014 4036 2464 4036 1014 4037 2463 4037 2464 4037 2463 4038 1587 4038 1996 4038 1591 4039 2464 4039 2462 4039 1587 4040 1997 4040 1996 4040 1591 4041 2462 4041 1998 4041 1996 4042 1997 4042 1855 4042 1591 4043 1998 4043 1608 4043 1998 4044 1617 4044 1608 4044 2780 4045 1606 4045 1607 4045 2780 4046 2468 4046 2781 4046 2780 4047 1607 4047 2468 4047 2780 4048 1999 4048 1606 4048 2001 4049 2000 4049 2003 4049 2004 4050 2002 4050 2001 4050 1600 4051 2004 4051 2001 4051 1600 4052 2001 4052 2003 4052 1603 4053 1600 4053 2003 4053 2003 4054 2466 4054 1602 4054 1603 4055 2003 4055 1602 4055 1604 4056 1602 4056 2466 4056 2005 4057 1604 4057 2466 4057 2046 4058 2469 4058 2006 4058 2046 4059 2006 4059 2000 4059 2000 4060 2001 4060 2007 4060 2007 4061 2046 4061 2000 4061 1613 4062 2023 4062 2034 4062 2072 4063 2015 4063 2021 4063 2069 4064 2066 4064 2009 4064 2014 4065 2504 4065 2019 4065 693 4066 2039 4066 2032 4066 1612 4067 1613 4067 2034 4067 2010 4068 2029 4068 2023 4068 2062 4069 2011 4069 2029 4069 2013 4070 2014 4070 2031 4070 2074 4071 2013 4071 2070 4071 2069 4072 2009 4072 2025 4072 2068 4073 2072 4073 2024 4073 2014 4074 2019 4074 2017 4074 2012 4075 2540 4075 2009 4075 2070 4076 2013 4076 2016 4076 2013 4077 2031 4077 2016 4077 2073 4078 2068 4078 2037 4078 2068 4079 2024 4079 2037 4079 2015 4080 2070 4080 2018 4080 2011 4081 2069 4081 2025 4081 2072 4082 2021 4082 2024 4082 2014 4083 2017 4083 2031 4083 2021 4084 2015 4084 2018 4084 2021 4085 2018 4085 2030 4085 1616 4086 2073 4086 2037 4086 2070 4087 2016 4087 2018 4087 1612 4088 2034 4088 2039 4088 2009 4089 2540 4089 2027 4089 2050 4090 1616 4090 2037 4090 2019 4091 2078 4091 2022 4091 2023 4092 2029 4092 2048 4092 2011 4093 2025 4093 2040 4093 2016 4094 2031 4094 2026 4094 2017 4095 2019 4095 2022 4095 2034 4096 2023 4096 2043 4096 2024 4097 2021 4097 2030 4097 2018 4098 2016 4098 2028 4098 2029 4099 2011 4099 2040 4099 2030 4100 2018 4100 2038 4100 2018 4101 2028 4101 2038 4101 2032 4102 2039 4102 2045 4102 2017 4103 2022 4103 2035 4103 2031 4104 2017 4104 2035 4104 2022 4105 2078 4105 2077 4105 2016 4106 2026 4106 2028 4106 2039 4107 2034 4107 2041 4107 2024 4108 2030 4108 2038 4108 2023 4109 2048 4109 2043 4109 2031 4110 2035 4110 2036 4110 2026 4111 2031 4111 2036 4111 2032 4112 2045 4112 2008 4112 2029 4113 2040 4113 2033 4113 2048 4114 2029 4114 2033 4114 2024 4115 2038 4115 2047 4115 2022 4116 2077 4116 2469 4116 2027 4117 2540 4117 2550 4117 2028 4118 2026 4118 2036 4118 2034 4119 2043 4119 2044 4119 2037 4120 2024 4120 2047 4120 2035 4121 2022 4121 2469 4121 2028 4122 2036 4122 2042 4122 2036 4123 2035 4123 2469 4123 2037 4124 2047 4124 2050 4124 2038 4125 2028 4125 2042 4125 2034 4126 2044 4126 2041 4126 2039 4127 2041 4127 2045 4127 2047 4128 2038 4128 2042 4128 2469 4129 2046 4129 2036 4129 2025 4130 2009 4130 2027 4130 2043 4131 2048 4131 2044 4131 2040 4132 2025 4132 2051 4132 2036 4133 2046 4133 2042 4133 2051 4134 2025 4134 2027 4134 2020 4135 2057 4135 1181 4135 2042 4136 2046 4136 2007 4136 2047 4137 2042 4137 2007 4137 2033 4138 2040 4138 2051 4138 2047 4139 2007 4139 2050 4139 2041 4140 2044 4140 2054 4140 2027 4141 2550 4141 2056 4141 2050 4142 2007 4142 2001 4142 2048 4143 2033 4143 2055 4143 2033 4144 2051 4144 2058 4144 1609 4145 1181 4145 2062 4145 2044 4146 2048 4146 2055 4146 2020 4147 2049 4147 2057 4147 2051 4148 2027 4148 2052 4148 2550 4149 2059 4149 2056 4149 2033 4150 2058 4150 2055 4150 2041 4151 2054 4151 2045 4151 2044 4152 2055 4152 2053 4152 2051 4153 2052 4153 2058 4153 2027 4154 2056 4154 2052 4154 1181 4155 2057 4155 2062 4155 2044 4156 2053 4156 2054 4156 2055 4157 2058 4157 2053 4157 2057 4158 2049 4158 2066 4158 2058 4159 2052 4159 2064 4159 2053 4160 2058 4160 2067 4160 2056 4161 2059 4161 2061 4161 2052 4162 2056 4162 2065 4162 2045 4163 2054 4163 2073 4163 2057 4164 2066 4164 2069 4164 1616 4165 2008 4165 2045 4165 2052 4166 2065 4166 2064 4166 2054 4167 2053 4167 2067 4167 2067 4168 2058 4168 2064 4168 2056 4169 2061 4169 2065 4169 2054 4170 2067 4170 2068 4170 1609 4171 2062 4171 2010 4171 2059 4172 2060 4172 2061 4172 1616 4173 2045 4173 2073 4173 2054 4174 2068 4174 2073 4174 2064 4175 2065 4175 2070 4175 2067 4176 2064 4176 2071 4176 2065 4177 2061 4177 2074 4177 2067 4178 2071 4178 2072 4178 2071 4179 2064 4179 2070 4179 2061 4180 2060 4180 2504 4180 693 4181 1610 4181 2039 4181 1610 4182 1612 4182 2039 4182 2061 4183 2504 4183 2014 4183 2070 4184 2065 4184 2074 4184 2057 4185 2069 4185 2011 4185 2067 4186 2072 4186 2068 4186 2074 4187 2061 4187 2013 4187 2062 4188 2057 4188 2011 4188 2008 4189 2063 4189 2032 4189 2010 4190 2062 4190 2029 4190 1613 4191 2010 4191 2023 4191 2072 4192 2071 4192 2015 4192 2071 4193 2070 4193 2015 4193 2066 4194 2012 4194 2009 4194 2013 4195 2061 4195 2014 4195 2460 4196 2075 4196 1995 4196 2510 4197 2460 4197 1995 4197 2461 4198 2510 4198 2076 4198 2470 4199 2077 4199 2078 4199 2470 4200 2078 4200 2517 4200 2517 4201 2078 4201 2019 4201 2517 4202 2019 4202 2489 4202 2489 4203 2019 4203 2504 4203 2060 4204 2059 4204 2473 4204 2473 4205 2059 4205 2550 4205 2066 4206 2049 4206 2507 4206 2507 4207 2049 4207 2020 4207 2507 4208 2020 4208 2544 4208 2544 4209 2020 4209 2478 4209 2079 4210 2534 4210 2080 4210 2081 4211 2079 4211 2080 4211 2081 4212 2080 4212 2082 4212 2081 4213 2082 4213 2083 4213 2083 4214 2082 4214 2084 4214 2084 4215 1626 4215 2085 4215 2085 4216 1626 4216 2519 4216 2509 4217 2086 4217 1655 4217 2501 4218 2509 4218 2087 4218 2509 4219 1655 4219 2088 4219 2087 4220 2509 4220 2088 4220 2088 4221 1655 4221 1629 4221 2087 4222 2088 4222 1652 4222 2088 4223 1629 4223 1652 4223 2476 4224 2087 4224 1652 4224 1044 4225 2534 4225 2089 4225 2089 4226 2534 4226 2090 4226 2092 4227 2539 4227 2130 4227 2092 4228 2095 4228 2539 4228 2093 4229 2125 4229 2099 4229 2118 4230 2107 4230 2102 4230 2104 4231 2128 4231 2122 4231 2117 4232 2097 4232 2474 4232 2112 4233 2132 4233 1810 4233 2112 4234 2117 4234 2132 4234 2092 4235 1808 4235 1815 4235 1770 4236 2096 4236 1768 4236 2105 4237 2106 4237 2131 4237 2098 4238 2095 4238 2092 4238 2499 4239 2482 4239 2096 4239 2104 4240 2129 4240 2128 4240 2527 4241 2129 4241 2104 4241 2499 4242 2096 4242 1770 4242 2097 4243 2117 4243 2112 4243 2101 4244 2092 4244 1815 4244 2477 4245 2093 4245 2099 4245 2477 4246 2551 4246 2093 4246 2103 4247 2118 4247 2102 4247 1770 4248 2100 4248 2499 4248 2487 4249 1770 4249 1773 4249 2112 4250 1810 4250 2125 4250 2103 4251 2094 4251 2118 4251 2535 4252 2094 4252 2103 4252 2487 4253 2100 4253 1770 4253 2111 4254 2105 4254 1992 4254 2112 4255 2523 4255 2097 4255 1785 4256 2487 4256 1773 4256 1785 4257 2490 4257 2487 4257 2108 4258 2104 4258 2122 4258 2518 4259 2092 4259 2101 4259 2518 4260 2098 4260 2092 4260 2546 4261 2105 4261 2111 4261 2546 4262 2106 4262 2105 4262 2107 4263 2527 4263 2104 4263 2109 4264 2490 4264 1785 4264 1784 4265 2109 4265 1785 4265 2110 4266 2477 4266 2099 4266 2520 4267 2527 4267 2107 4267 2488 4268 2477 4268 2110 4268 2515 4269 2109 4269 1784 4269 2107 4270 2104 4270 2108 4270 1772 4271 2101 4271 1815 4271 2115 4272 2112 4272 2125 4272 2111 4273 2116 4273 2546 4273 2111 4274 1992 4274 2132 4274 2114 4275 2515 4275 1784 4275 2107 4276 2119 4276 2520 4276 2114 4277 2113 4277 2515 4277 2522 4278 2112 4278 2115 4278 2522 4279 2523 4279 2112 4279 2532 4280 2113 4280 2114 4280 2532 4281 2114 4281 1797 4281 2500 4282 2518 4282 2101 4282 2484 4283 2532 4283 1797 4283 2128 4284 2488 4284 2110 4284 2105 4285 2101 4285 1772 4285 2128 4286 2120 4286 2488 4286 2116 4287 2111 4287 2117 4287 2093 4288 2522 4288 2115 4288 2093 4289 2126 4289 2522 4289 2121 4290 2535 4290 2103 4290 2117 4291 2111 4291 2132 4291 2103 4292 2102 4292 1782 4292 2484 4293 1797 4293 2123 4293 2101 4294 2131 4294 2500 4294 2119 4295 2107 4295 2118 4295 2093 4296 2115 4296 2125 4296 1992 4297 2105 4297 1772 4297 2121 4298 2103 4298 2091 4298 2128 4299 2110 4299 2122 4299 2124 4300 2484 4300 2123 4300 2127 4301 2124 4301 2123 4301 2130 4302 2123 4302 1807 4302 2130 4303 2127 4303 2123 4303 2129 4304 2120 4304 2128 4304 2118 4305 2094 4305 2119 4305 2102 4306 2107 4306 2108 4306 2093 4307 2551 4307 2126 4307 2130 4308 1807 4308 1808 4308 2474 4309 2116 4309 2117 4309 2131 4310 2101 4310 2105 4310 2092 4311 2130 4311 1808 4311 2134 4312 1377 4312 1699 4312 2133 4313 2482 4313 2479 4313 1699 4314 2479 4314 2134 4314 2135 4315 2139 4315 2547 4315 2135 4316 1647 4316 2139 4316 2547 4317 2476 4317 2135 4317 2547 4318 2139 4318 2136 4318 2791 4319 2555 4319 2137 4319 2791 4320 2137 4320 2138 4320 2791 4321 2792 4321 2555 4321 1643 4322 2559 4322 1653 4322 1663 4323 1643 4323 1653 4323 1647 4324 1653 4324 2139 4324 1228 4325 2140 4325 2556 4325 2140 4326 1675 4326 2556 4326 1675 4327 1672 4327 2556 4327 1672 4328 2141 4328 2556 4328 2141 4329 2142 4329 2556 4329 1667 4330 2143 4330 2556 4330 2143 4331 1228 4331 2556 4331 1667 4332 2556 4332 2558 4332 1667 4333 2558 4333 1643 4333 2559 4334 1643 4334 2558 4334 2795 4335 1684 4335 2562 4335 2795 4336 2144 4336 1684 4336 2147 4337 2561 4337 2146 4337 2148 4338 2145 4338 2147 4338 2150 4339 2149 4339 2147 4339 2149 4340 2148 4340 2147 4340 1688 4341 2150 4341 2147 4341 1686 4342 1688 4342 2147 4342 1686 4343 2147 4343 2151 4343 2153 4344 1686 4344 2151 4344 2152 4345 2153 4345 2151 4345 2147 4346 2156 4346 2158 4346 2561 4347 2147 4347 2158 4347 2154 4348 2155 4348 2156 4348 1847 4349 2154 4349 2157 4349 2157 4350 2154 4350 2156 4350 2147 4351 2145 4351 2157 4351 2157 4352 2156 4352 2147 4352 1851 4353 2154 4353 1847 4353 2154 4354 1851 4354 1852 4354 2577 4355 2154 4355 1852 4355 1376 4356 2161 4356 2159 4356 1376 4357 2628 4357 2161 4357 1702 4358 2159 4358 2161 4358 1702 4359 2161 4359 2162 4359 2160 4360 1702 4360 2162 4360 2163 4361 2160 4361 2162 4361 2602 4362 2590 4362 2186 4362 2618 4363 2184 4363 1709 4363 2618 4364 2167 4364 2184 4364 2169 4365 2196 4365 1749 4365 2568 4366 2196 4366 2169 4366 1709 4367 1715 4367 2177 4367 2170 4368 1721 4368 1722 4368 2170 4369 1722 4369 2173 4369 2170 4370 2168 4370 1721 4370 2593 4371 2168 4371 2170 4371 2169 4372 2574 4372 2568 4372 2172 4373 2574 4373 2169 4373 2176 4374 2570 4374 1967 4374 2594 4375 2188 4375 1986 4375 1728 4376 2166 4376 2187 4376 2179 4377 2174 4377 2164 4377 2172 4378 2584 4378 2574 4378 2180 4379 2170 4379 2173 4379 2166 4380 2638 4380 2165 4380 2172 4381 2163 4381 2584 4381 2172 4382 1820 4382 2163 4382 2174 4383 2601 4383 2602 4383 2175 4384 2594 4384 1986 4384 2166 4385 2576 4385 2638 4385 2177 4386 2618 4386 1709 4386 2180 4387 2593 4387 2170 4387 2180 4388 2171 4388 2593 4388 2183 4389 2601 4389 2174 4389 2183 4390 2178 4390 2601 4390 2183 4391 2174 4391 2179 4391 2177 4392 2181 4392 2618 4392 2181 4393 2629 4393 2618 4393 2580 4394 2570 4394 2176 4394 1986 4395 2600 4395 2175 4395 2580 4396 2573 4396 2570 4396 2166 4397 1728 4397 1737 4397 2184 4398 1986 4398 1988 4398 2180 4399 2617 4399 2171 4399 2182 4400 2180 4400 2173 4400 2565 4401 2629 4401 2181 4401 2187 4402 2183 4402 2179 4402 2184 4403 2600 4403 1986 4403 2188 4404 2166 4404 1737 4404 2191 4405 2181 4405 2177 4405 2627 4406 2180 4406 2182 4406 2188 4407 2578 4407 2166 4407 2627 4408 2617 4408 2180 4408 2578 4409 2576 4409 2166 4409 2619 4410 2178 4410 2183 4410 2194 4411 2183 4411 2187 4411 2185 4412 2627 4412 2182 4412 2186 4413 2176 4413 1985 4413 2619 4414 2183 4414 2194 4414 2189 4415 2580 4415 2176 4415 2191 4416 2177 4416 1721 4416 2167 4417 2600 4417 2184 4417 2185 4418 2182 4418 1739 4418 2186 4419 2189 4419 2176 4419 1986 4420 2188 4420 1737 4420 2191 4421 2565 4421 2181 4421 2190 4422 2185 4422 1739 4422 2186 4423 2590 4423 2189 4423 2630 4424 2190 4424 1739 4424 2630 4425 1739 4425 2192 4425 2184 4426 1988 4426 1709 4426 2174 4427 2186 4427 1985 4427 2165 4428 2619 4428 2194 4428 1747 4429 2630 4429 2192 4429 2193 4430 2630 4430 1747 4430 2594 4431 2578 4431 2188 4431 2195 4432 2193 4432 1747 4432 2174 4433 1985 4433 2164 4433 2168 4434 2191 4434 1721 4434 2166 4435 2194 4435 2187 4435 2166 4436 2165 4436 2194 4436 1749 4437 2195 4437 1747 4437 1749 4438 2196 4438 2195 4438 2602 4439 2186 4439 2174 4439 2198 4440 888 4440 2579 4440 788 4441 2197 4441 2579 4441 888 4442 788 4442 2579 4442 2579 4443 2587 4443 2198 4443 2587 4444 2586 4444 2198 4444 2577 4445 1852 4445 1842 4445 2577 4446 1842 4446 2591 4446 2591 4447 1842 4447 1831 4447 2199 4448 2591 4448 1831 4448 2199 4449 1831 4449 2200 4449 2200 4450 1831 4450 1824 4450 2201 4451 2200 4451 1824 4451 2201 4452 1824 4452 2202 4452 2202 4453 1824 4453 1848 4453 2202 4454 1848 4454 2203 4454 2204 4455 2202 4455 2203 4455 2204 4456 2203 4456 1837 4456 2612 4457 2204 4457 1837 4457 2612 4458 1837 4458 1843 4458 2592 4459 1823 4459 2205 4459 2592 4460 2205 4460 2586 4460 2633 4461 2274 4461 2251 4461 2633 4462 2251 4462 2609 4462 2609 4463 2251 4463 2206 4463 2609 4464 2206 4464 2207 4464 2207 4465 2206 4465 2215 4465 2207 4466 2215 4466 2569 4466 2569 4467 2215 4467 2217 4467 2569 4468 2217 4468 2571 4468 2571 4469 2217 4469 2266 4469 2571 4470 2266 4470 2624 4470 2624 4471 2266 4471 2607 4471 2607 4472 2266 4472 2255 4472 2607 4473 2255 4473 2605 4473 2605 4474 2255 4474 2245 4474 2605 4475 2245 4475 2583 4475 2583 4476 2245 4476 2239 4476 2583 4477 2239 4477 2623 4477 2623 4478 2239 4478 2611 4478 2536 4479 1854 4479 1858 4479 2536 4480 1858 4480 2521 4480 2521 4481 1858 4481 1875 4481 2521 4482 1875 4482 2495 4482 2495 4483 1875 4483 1879 4483 2495 4484 1879 4484 1884 4484 2209 4485 1376 4485 1375 4485 1376 4486 2209 4486 1947 4486 2276 4487 679 4487 2271 4487 2272 4488 2281 4488 2226 4488 2278 4489 2214 4489 2220 4489 2281 4490 2273 4490 2224 4490 2226 4491 2281 4491 2224 4491 2210 4492 2283 4492 2227 4492 2282 4493 2280 4493 2212 4493 2213 4494 2219 4494 2222 4494 2275 4495 2211 4495 2227 4495 2273 4496 2282 4496 2212 4496 2272 4497 2226 4497 2211 4497 2214 4498 679 4498 2216 4498 2220 4499 2214 4499 2216 4499 2213 4500 2222 4500 2234 4500 2213 4501 2234 4501 2218 4501 2219 4502 2220 4502 2221 4502 2219 4503 2221 4503 2222 4503 2641 4504 2611 4504 2884 4504 2220 4505 2216 4505 2221 4505 1919 4506 2210 4506 2231 4506 2210 4507 2227 4507 2231 4507 2221 4508 2216 4508 1903 4508 2230 4509 2218 4509 2234 4509 2217 4510 2215 4510 2223 4510 2884 4511 2611 4511 2225 4511 2280 4512 2217 4512 2223 4512 2223 4513 2215 4513 2241 4513 2227 4514 2211 4514 2240 4514 2222 4515 2221 4515 2235 4515 2273 4516 2212 4516 2224 4516 2221 4517 1903 4517 2235 4517 2225 4518 2611 4518 2237 4518 2222 4519 2235 4519 1898 4519 2222 4520 1898 4520 2234 4520 2228 4521 2225 4521 2238 4521 2226 4522 2224 4522 2232 4522 2227 4523 2240 4523 2254 4523 2212 4524 2280 4524 2223 4524 2228 4525 2238 4525 2229 4525 2226 4526 2232 4526 2211 4526 2233 4527 2228 4527 2229 4527 2227 4528 2254 4528 2231 4528 2238 4529 2225 4529 2237 4529 2212 4530 2223 4530 2250 4530 2211 4531 2232 4531 2247 4531 2237 4532 2611 4532 2239 4532 2215 4533 2206 4533 2241 4533 2236 4534 2230 4534 2234 4534 2233 4535 2229 4535 2249 4535 2224 4536 2212 4536 2250 4536 2238 4537 2237 4537 2248 4537 2241 4538 2206 4538 2251 4538 2211 4539 2247 4539 2240 4539 2237 4540 2239 4540 2256 4540 2233 4541 2258 4541 2244 4541 2224 4542 2250 4542 2242 4542 2240 4543 2247 4543 2213 4543 2229 4544 2238 4544 2248 4544 2232 4545 2224 4545 2242 4545 2240 4546 2213 4546 2254 4546 2229 4547 2248 4547 2261 4547 2248 4548 2237 4548 2256 4548 2223 4549 2241 4549 2243 4549 2232 4550 2242 4550 2246 4550 2233 4551 2249 4551 2253 4551 2229 4552 2261 4552 2249 4552 2230 4553 1919 4553 2218 4553 2250 4554 2223 4554 2243 4554 2233 4555 2253 4555 2258 4555 2232 4556 2246 4556 2247 4556 2239 4557 2245 4557 2257 4557 2248 4558 2256 4558 2260 4558 2256 4559 2239 4559 2257 4559 2231 4560 2218 4560 1919 4560 2244 4561 2258 4561 2263 4561 2242 4562 2250 4562 2243 4562 2293 4563 2244 4563 2263 4563 2248 4564 2260 4564 2261 4564 2249 4565 2261 4565 2253 4565 2241 4566 2251 4566 2271 4566 2242 4567 2243 4567 2278 4567 2245 4568 2255 4568 2259 4568 2257 4569 2245 4569 2259 4569 2253 4570 2261 4570 2252 4570 2246 4571 2242 4571 2278 4571 2256 4572 2257 4572 2264 4572 2258 4573 2253 4573 2262 4573 2243 4574 2241 4574 2271 4574 2255 4575 2266 4575 2259 4575 2260 4576 2256 4576 2264 4576 2261 4577 2260 4577 2268 4577 2257 4578 2259 4578 2265 4578 2263 4579 2258 4579 2262 4579 2260 4580 2264 4580 2270 4580 2246 4581 2278 4581 2269 4581 2262 4582 2253 4582 2252 4582 2267 4583 2293 4583 2263 4583 2268 4584 2260 4584 2270 4584 2252 4585 2261 4585 2268 4585 2264 4586 2257 4586 2265 4586 1958 4587 2251 4587 2274 4587 2271 4588 2251 4588 1958 4588 2271 4589 1958 4589 2276 4589 2231 4590 2254 4590 2213 4590 2231 4591 2213 4591 2218 4591 2262 4592 2252 4592 2283 4592 2264 4593 2265 4593 2281 4593 2252 4594 2268 4594 2275 4594 2268 4595 2270 4595 2277 4595 2259 4596 2266 4596 2282 4596 2262 4597 2283 4597 2210 4597 2264 4598 2281 4598 2270 4598 2283 4599 2252 4599 2275 4599 2269 4600 2278 4600 2220 4600 2267 4601 2263 4601 2210 4601 2263 4602 2262 4602 2210 4602 2268 4603 2277 4603 2275 4603 2247 4604 2246 4604 2269 4604 2247 4605 2269 4605 2219 4605 2274 4606 2279 4606 1958 4606 2265 4607 2259 4607 2273 4607 2259 4608 2282 4608 2273 4608 2277 4609 2270 4609 2272 4609 2270 4610 2281 4610 2272 4610 2243 4611 2271 4611 2214 4611 2281 4612 2265 4612 2273 4612 2269 4613 2220 4613 2219 4613 2282 4614 2266 4614 2217 4614 2278 4615 2243 4615 2214 4615 2282 4616 2217 4616 2280 4616 2213 4617 2247 4617 2219 4617 2277 4618 2272 4618 2211 4618 2275 4619 2277 4619 2211 4619 1958 4620 2279 4620 2299 4620 1919 4621 2267 4621 2210 4621 2283 4622 2275 4622 2227 4622 2214 4623 2271 4623 679 4623 2804 4624 1917 4624 2284 4624 2804 4625 2284 4625 1906 4625 2244 4626 2642 4626 2285 4626 2244 4627 2285 4627 2643 4627 2233 4628 2244 4628 2228 4628 2228 4629 2244 4629 2286 4629 2244 4630 2643 4630 2286 4630 1918 4631 1909 4631 2293 4631 1909 4632 2287 4632 2292 4632 2287 4633 2288 4633 2292 4633 2288 4634 1913 4634 2292 4634 1913 4635 1912 4635 2292 4635 1909 4636 2292 4636 2293 4636 1912 4637 2289 4637 2291 4637 2289 4638 2290 4638 2291 4638 2292 4639 1912 4639 2291 4639 2293 4640 2292 4640 2244 4640 2244 4641 2292 4641 2642 4641 2648 4642 2645 4642 2279 4642 2295 4643 2294 4643 2648 4643 2296 4644 2295 4644 2597 4644 2296 4645 2294 4645 2295 4645 2296 4646 2597 4646 2810 4646 2297 4647 2597 4647 1966 4647 2298 4648 1958 4648 2299 4648 2650 4649 2298 4649 2299 4649 2649 4650 2298 4650 2651 4650 2649 4651 552 4651 1966 4651 1967 4652 2652 4652 2300 4652 2813 4653 2652 4653 2815 4653 2301 4654 982 4654 1755 4654 2653 4655 2654 4655 2301 4655 2653 4656 2301 4656 1755 4656 1755 4657 1967 4657 2652 4657 2652 4658 2653 4658 1755 4658 678 4659 2303 4659 2307 4659 1342 4660 2304 4660 678 4660 2302 4661 2303 4661 678 4661 2304 4662 677 4662 2310 4662 2819 4663 2305 4663 2304 4663 2819 4664 2306 4664 2305 4664 678 4665 2819 4665 2304 4665 678 4666 2307 4666 2819 4666 2309 4667 2308 4667 2656 4667 2310 4668 2309 4668 2656 4668 2656 4669 2300 4669 1967 4669 2308 4670 2655 4670 2656 4670 2366 4671 2368 4671 2370 4671 2311 4672 2661 4672 2370 4672 2370 4673 2368 4673 2319 4673 2311 4674 2370 4674 2315 4674 2316 4675 2704 4675 2369 4675 2313 4676 2704 4676 2316 4676 2312 4677 2313 4677 2316 4677 2316 4678 2369 4678 2320 4678 2370 4679 2319 4679 2315 4679 2766 4680 2311 4680 2315 4680 2312 4681 2316 4681 2317 4681 2321 4682 2312 4682 2317 4682 2315 4683 2319 4683 2318 4683 2317 4684 2316 4684 2320 4684 2318 4685 2315 4685 2766 4685 2322 4686 2766 4686 2318 4686 2317 4687 2320 4687 2325 4687 2321 4688 2317 4688 2676 4688 2318 4689 2319 4689 2327 4689 2322 4690 2318 4690 2323 4690 2317 4691 2325 4691 2676 4691 2759 4692 2322 4692 2323 4692 2323 4693 2318 4693 2327 4693 2673 4694 2676 4694 2328 4694 2699 4695 2373 4695 2326 4695 2759 4696 2323 4696 2324 4696 2373 4697 2374 4697 2326 4697 2754 4698 2759 4698 2324 4698 2701 4699 2699 4699 2326 4699 2324 4700 2323 4700 2327 4700 2328 4701 2676 4701 2325 4701 2673 4702 2328 4702 2664 4702 2327 4703 2330 4703 2324 4703 2697 4704 2701 4704 2326 4704 2754 4705 2324 4705 2329 4705 2753 4706 2754 4706 2329 4706 2329 4707 2324 4707 2330 4707 2328 4708 2325 4708 1480 4708 2664 4709 2328 4709 1480 4709 2697 4710 2326 4710 2332 4710 2326 4711 2374 4711 2332 4711 2334 4712 2697 4712 2332 4712 2753 4713 2329 4713 2333 4713 2742 4714 2753 4714 2333 4714 2332 4715 2374 4715 1978 4715 2658 4716 2664 4716 2331 4716 2333 4717 2329 4717 2330 4717 2334 4718 2332 4718 2695 4718 2331 4719 2664 4719 1480 4719 2742 4720 2333 4720 2335 4720 2332 4721 1978 4721 2695 4721 2333 4722 2330 4722 2338 4722 2335 4723 2333 4723 2338 4723 2760 4724 2658 4724 2331 4724 2695 4725 1978 4725 1524 4725 2331 4726 1480 4726 2336 4726 2760 4727 2331 4727 2345 4727 2337 4728 2695 4728 2342 4728 2339 4729 2335 4729 2343 4729 2335 4730 2338 4730 2343 4730 2342 4731 2695 4731 1524 4731 2345 4732 2331 4732 2336 4732 2344 4733 2339 4733 2343 4733 2340 4734 2345 4734 2738 4734 2342 4735 1524 4735 2346 4735 2341 4736 2342 4736 2346 4736 2345 4737 2336 4737 2340 4737 2340 4738 2336 4738 2347 4738 2738 4739 2340 4739 2348 4739 2341 4740 2346 4740 2350 4740 2688 4741 2341 4741 2350 4741 2351 4742 2344 4742 2343 4742 2340 4743 2347 4743 2348 4743 2350 4744 2346 4744 2349 4744 2357 4745 2350 4745 2688 4745 2680 4746 2688 4746 2357 4746 2351 4747 2343 4747 2361 4747 2350 4748 2349 4748 2357 4748 2720 4749 2351 4749 2352 4749 2723 4750 2720 4750 2352 4750 2352 4751 2351 4751 2361 4751 2348 4752 2347 4752 2353 4752 2353 4753 2347 4753 2355 4753 2355 4754 2347 4754 2354 4754 2727 4755 2353 4755 2355 4755 2357 4756 2349 4756 2363 4756 2356 4757 2680 4757 2358 4757 2680 4758 2357 4758 2358 4758 2358 4759 2357 4759 2363 4759 2715 4760 2723 4760 2359 4760 2723 4761 2352 4761 2359 4761 2359 4762 2352 4762 2361 4762 2727 4763 2355 4763 2719 4763 2364 4764 2358 4764 2356 4764 2360 4765 2356 4765 2364 4765 2358 4766 2363 4766 2364 4766 2715 4767 2359 4767 2367 4767 2709 4768 2715 4768 2367 4768 2359 4769 2361 4769 2365 4769 2367 4770 2359 4770 2365 4770 2719 4771 2355 4771 2362 4771 2355 4772 2354 4772 2362 4772 2360 4773 2364 4773 2366 4773 2364 4774 2363 4774 2368 4774 2704 4775 2709 4775 2367 4775 2366 4776 2364 4776 2368 4776 2367 4777 2365 4777 2369 4777 2661 4778 2366 4778 2370 4778 2704 4779 2367 4779 2369 4779 2375 4780 2438 4780 2376 4780 2699 4781 2437 4781 2373 4781 2436 4782 2372 4782 2371 4782 2372 4783 2378 4783 2371 4783 2380 4784 517 4784 2371 4784 2437 4785 2434 4785 2373 4785 2373 4786 2434 4786 2374 4786 2377 4787 538 4787 2375 4787 2434 4788 2439 4788 2374 4788 2375 4789 2376 4789 1981 4789 2377 4790 2375 4790 2381 4790 2379 4791 2380 4791 2371 4791 2371 4792 2378 4792 1970 4792 2375 4793 1981 4793 2381 4793 2379 4794 2371 4794 2382 4794 2377 4795 2381 4795 2383 4795 2383 4796 2381 4796 1981 4796 2371 4797 1970 4797 2382 4797 493 4798 2379 4798 2382 4798 1981 4799 2385 4799 2383 4799 2384 4800 2383 4800 2385 4800 486 4801 493 4801 2382 4801 486 4802 2382 4802 2389 4802 2314 4803 2705 4803 2386 4803 2387 4804 2384 4804 2385 4804 1970 4805 1424 4805 2389 4805 2382 4806 1970 4806 2389 4806 476 4807 486 4807 2389 4807 2386 4808 2705 4808 2400 4808 2387 4809 2385 4809 2396 4809 2391 4810 2387 4810 2388 4810 478 4811 2386 4811 2400 4811 2388 4812 2387 4812 2396 4812 2390 4813 476 4813 2389 4813 2385 4814 2392 4814 2396 4814 2395 4815 478 4815 2394 4815 2397 4816 2391 4816 2393 4816 2391 4817 2388 4817 2393 4817 2394 4818 478 4818 2400 4818 2393 4819 2388 4819 2396 4819 2389 4820 1424 4820 2390 4820 2401 4821 2390 4821 2407 4821 2390 4822 1424 4822 2399 4822 2407 4823 2390 4823 2399 4823 2393 4824 2396 4824 2402 4824 2396 4825 2392 4825 2402 4825 2397 4826 2393 4826 2403 4826 2398 4827 2397 4827 2403 4827 2404 4828 2395 4828 2412 4828 2395 4829 2394 4829 2412 4829 2403 4830 2393 4830 2402 4830 2406 4831 2401 4831 2407 4831 2392 4832 1983 4832 2402 4832 2398 4833 2403 4833 2410 4833 2394 4834 2400 4834 2412 4834 2412 4835 2400 4835 1972 4835 2411 4836 2404 4836 2412 4836 2403 4837 2402 4837 2410 4837 2410 4838 2402 4838 1983 4838 2399 4839 2413 4839 2407 4839 2406 4840 2407 4840 2408 4840 2405 4841 2406 4841 2408 4841 2408 4842 2407 4842 2413 4842 504 4843 2410 4843 2409 4843 2411 4844 2412 4844 463 4844 2410 4845 1983 4845 1969 4845 2409 4846 2410 4846 1969 4846 463 4847 2412 4847 1972 4847 2414 4848 2405 4848 2408 4848 463 4849 1972 4849 2417 4849 2415 4850 504 4850 2416 4850 504 4851 2409 4851 2416 4851 458 4852 463 4852 2417 4852 2414 4853 2408 4853 2421 4853 2408 4854 2413 4854 2421 4854 2418 4855 2414 4855 2421 4855 2416 4856 2409 4856 1969 4856 2416 4857 1969 4857 1973 4857 2420 4858 2415 4858 2419 4858 2417 4859 1972 4859 2426 4859 2415 4860 2416 4860 2419 4860 457 4861 458 4861 2417 4861 2421 4862 2413 4862 1454 4862 2416 4863 1973 4863 2419 4863 459 4864 2418 4864 2423 4864 2418 4865 2421 4865 2423 4865 2419 4866 1973 4866 2424 4866 457 4867 2417 4867 2422 4867 492 4868 2420 4868 2425 4868 2420 4869 2419 4869 2425 4869 2422 4870 2417 4870 2426 4870 2425 4871 2419 4871 2424 4871 2423 4872 2421 4872 1454 4872 2427 4873 459 4873 2423 4873 2423 4874 1454 4874 2435 4874 2429 4875 492 4875 2432 4875 492 4876 2425 4876 2432 4876 2422 4877 2426 4877 2430 4877 2425 4878 2424 4878 2432 4878 2427 4879 2423 4879 2435 4879 532 4880 2422 4880 536 4880 551 4881 2427 4881 2428 4881 2428 4882 2427 4882 2435 4882 2422 4883 2430 4883 536 4883 2432 4884 2424 4884 2434 4884 2424 4885 2439 4885 2434 4885 2433 4886 2429 4886 2437 4886 2429 4887 2432 4887 2437 4887 2437 4888 2432 4888 2434 4888 551 4889 2428 4889 2431 4889 546 4890 551 4890 2431 4890 2431 4891 2428 4891 2435 4891 536 4892 2430 4892 2372 4892 2436 4893 536 4893 2372 4893 2699 4894 2433 4894 2437 4894 546 4895 2431 4895 2438 4895 542 4896 546 4896 2438 4896 2438 4897 2431 4897 2376 4897 2431 4898 2435 4898 2376 4898 517 4899 2436 4899 2371 4899 538 4900 542 4900 2375 4900 542 4901 2438 4901 2375 4901 1544 4902 2856 4902 675 4902 2441 4903 2856 4903 1544 4903 675 4904 2442 4904 674 4904 2443 4905 2450 4905 2444 4905 2444 4906 2450 4906 1203 4906 2445 4907 2443 4907 2444 4907 1583 4908 1991 4908 2451 4908 674 4909 2444 4909 1203 4909 2447 4910 2446 4910 2451 4910 2450 4911 1583 4911 2448 4911 2448 4912 2451 4912 1583 4912 2448 4913 2447 4913 2451 4913 2447 4914 672 4914 2446 4914 2450 4915 2449 4915 2448 4915 2451 4916 2769 4916 1984 4916 2453 4917 2865 4917 2440 4917 1984 4918 2455 4918 2454 4918 2452 4919 2440 4919 1782 4919 2454 4920 2453 4920 2440 4920 2454 4921 2440 4921 2452 4921 2455 4922 2453 4922 2454 4922 1984 4923 2454 4923 2452 4923 2771 4924 2075 4924 2456 4924 2456 4925 2075 4925 2460 4925 2458 4926 2478 4926 2459 4926 2478 4927 2457 4927 2459 4927 2457 4928 2456 4928 2459 4928 2456 4929 2460 4929 2459 4929 2458 4930 2459 4930 2460 4930 1617 4931 2457 4931 2478 4931 2460 4932 2510 4932 2461 4932 2076 4933 2458 4933 2461 4933 2461 4934 2458 4934 2460 4934 2462 4935 2464 4935 2777 4935 1998 4936 2462 4936 2777 4936 2773 4937 1996 4937 1855 4937 2774 4938 2464 4938 2463 4938 2006 4939 2469 4939 2779 4939 2468 4940 2783 4940 2781 4940 1607 4941 2466 4941 2468 4941 2005 4942 2466 4942 1607 4942 2465 4943 2000 4943 2006 4943 2465 4944 2006 4944 2779 4944 2467 4945 2003 4945 2000 4945 2467 4946 2000 4946 2465 4946 2783 4947 2466 4947 2003 4947 2783 4948 2003 4948 2467 4948 2783 4949 2468 4949 2466 4949 2779 4950 2469 4950 2784 4950 2784 4951 2469 4951 2077 4951 2470 4952 2787 4952 2077 4952 2095 4953 2098 4953 2471 4953 2116 4954 2474 4954 2470 4954 2551 4955 2477 4955 2475 4955 2113 4956 2532 4956 1995 4956 2547 4957 2126 4957 2476 4957 2538 4958 2546 4958 2516 4958 2506 4959 2127 4959 2066 4959 2539 4960 2541 4960 2012 4960 2553 4961 2113 4961 1995 4961 2126 4962 2542 4962 2476 4962 2060 4963 2473 4963 2486 4963 2106 4964 2538 4964 2489 4964 2489 4965 2538 4965 2516 4965 2458 4966 2483 4966 2478 4966 1885 4967 2499 4967 2100 4967 2495 4968 1884 4968 2498 4968 2481 4969 2551 4969 2087 4969 2476 4970 2542 4970 2481 4970 2087 4971 2476 4971 2481 4971 2477 4972 2488 4972 2503 4972 2544 4973 2478 4973 2483 4973 2479 4974 2482 4974 2134 4974 2482 4975 2499 4975 2134 4975 2503 4976 2488 4976 2509 4976 2484 4977 2124 4977 2485 4977 2098 4978 2518 4978 2494 4978 2516 4979 2546 4979 2552 4979 2100 4980 2487 4980 1884 4980 2532 4981 2491 4981 2076 4981 2012 4982 2541 4982 2095 4982 2087 4983 2551 4983 2475 4983 2120 4984 2129 4984 2502 4984 2475 4985 2477 4985 2501 4985 2487 4986 2490 4986 2495 4986 2495 4987 2490 4987 2513 4987 2491 4988 2484 4988 2458 4988 2504 4989 2060 4989 2480 4989 2501 4990 2087 4990 2475 4990 2474 4991 2097 4991 2493 4991 2492 4992 2474 4992 2493 4992 2488 4993 2120 4993 2496 4993 2509 4994 2488 4994 2496 4994 2516 4995 2552 4995 2517 4995 2496 4996 2120 4996 2502 4996 2501 4997 2477 4997 2503 4997 2507 4998 2544 4998 2497 4998 2504 4999 2106 4999 2505 4999 2458 5000 2484 5000 2483 5000 2502 5001 2129 5001 2514 5001 2532 5002 2076 5002 2510 5002 2513 5003 2490 5003 2109 5003 2129 5004 2527 5004 2511 5004 2095 5005 2471 5005 2540 5005 2514 5006 2129 5006 2511 5006 2517 5007 2552 5007 2116 5007 2509 5008 2501 5008 2503 5008 2521 5009 2495 5009 2513 5009 2489 5010 2504 5010 2505 5010 2505 5011 2106 5011 2489 5011 2514 5012 2511 5012 2519 5012 2540 5013 2471 5013 2098 5013 2496 5014 2502 5014 2086 5014 2066 5015 2539 5015 2529 5015 2097 5016 2523 5016 2512 5016 2124 5017 2127 5017 2508 5017 2086 5018 2509 5018 2496 5018 2470 5019 2474 5019 2787 5019 2076 5020 2491 5020 2458 5020 2529 5021 2539 5021 2012 5021 2524 5022 2109 5022 2515 5022 2787 5023 2474 5023 2492 5023 2511 5024 2527 5024 2519 5024 2483 5025 2484 5025 2485 5025 2086 5026 2502 5026 2514 5026 2519 5027 2086 5027 2514 5027 2517 5028 2489 5028 2516 5028 2486 5029 2500 5029 2060 5029 2012 5030 2095 5030 2526 5030 2095 5031 2540 5031 2526 5031 2537 5032 2525 5032 1854 5032 2507 5033 2506 5033 2066 5033 2517 5034 2116 5034 2470 5034 2527 5035 2520 5035 2084 5035 1995 5036 2532 5036 2510 5036 1884 5037 2487 5037 2498 5037 2085 5038 2519 5038 2527 5038 2537 5039 2515 5039 2525 5039 2515 5040 2113 5040 2525 5040 2787 5041 2492 5041 2493 5041 2060 5042 2500 5042 2131 5042 2536 5043 2521 5043 2524 5043 1854 5044 2525 5044 2553 5044 2498 5045 2487 5045 2495 5045 1885 5046 2100 5046 2549 5046 2499 5047 1885 5047 1377 5047 2134 5048 2499 5048 1377 5048 2084 5049 2520 5049 2119 5049 2012 5050 2066 5050 2529 5050 2060 5051 2131 5051 2480 5051 2522 5052 2126 5052 2533 5052 2085 5053 2527 5053 2084 5053 2494 5054 2518 5054 2472 5054 2119 5055 2094 5055 2531 5055 2098 5056 2494 5056 2550 5056 2131 5057 2106 5057 2528 5057 2493 5058 2097 5058 2530 5058 2540 5059 2098 5059 2545 5059 2483 5060 2485 5060 2544 5060 2472 5061 2518 5061 2500 5061 2084 5062 2119 5062 2083 5062 2083 5063 2119 5063 2531 5063 2544 5064 2485 5064 2124 5064 2545 5065 2098 5065 2550 5065 2549 5066 2100 5066 1884 5066 2787 5067 2493 5067 2788 5067 2512 5068 2523 5068 2548 5068 2547 5069 2522 5069 2533 5069 2081 5070 2083 5070 2531 5070 2094 5071 2535 5071 2079 5071 2480 5072 2131 5072 2504 5072 2788 5073 2493 5073 2530 5073 2131 5074 2528 5074 2504 5074 2081 5075 2531 5075 2094 5075 2550 5076 2494 5076 2472 5076 2540 5077 2012 5077 2526 5077 2544 5078 2124 5078 2497 5078 2079 5079 2535 5079 2534 5079 2530 5080 2097 5080 2512 5080 1854 5081 2536 5081 2537 5081 2504 5082 2528 5082 2106 5082 2126 5083 2551 5083 2542 5083 2106 5084 2546 5084 2538 5084 1947 5085 2209 5085 1377 5085 2548 5086 2523 5086 2522 5086 2530 5087 2512 5087 2548 5087 2548 5088 2788 5088 2530 5088 1947 5089 1377 5089 1885 5089 2124 5090 2508 5090 2507 5090 2524 5091 2515 5091 2536 5091 2539 5092 2095 5092 2541 5092 2550 5093 2540 5093 2545 5093 2507 5094 2508 5094 2127 5094 2548 5095 2522 5095 2543 5095 2542 5096 2551 5096 2481 5096 1995 5097 1854 5097 2553 5097 2081 5098 2094 5098 2079 5098 2536 5099 2515 5099 2537 5099 2472 5100 2500 5100 2473 5100 2543 5101 2522 5101 2547 5101 2497 5102 2124 5102 2507 5102 2513 5103 2109 5103 2521 5103 2534 5104 2535 5104 2090 5104 2535 5105 2121 5105 2090 5105 2546 5106 2116 5106 2552 5106 2473 5107 2500 5107 2486 5107 2090 5108 2121 5108 1618 5108 2525 5109 2113 5109 2553 5109 2507 5110 2127 5110 2506 5110 1884 5111 1885 5111 2549 5111 2473 5112 2550 5112 2472 5112 2532 5113 2484 5113 2491 5113 2521 5114 2109 5114 2524 5114 2127 5115 2130 5115 2066 5115 2130 5116 2539 5116 2066 5116 2547 5117 2533 5117 2126 5117 2784 5118 2077 5118 2554 5118 2789 5119 2786 5119 2543 5119 2789 5120 2543 5120 2136 5120 2543 5121 2547 5121 2136 5121 2139 5122 2790 5122 2136 5122 2557 5123 2555 5123 2792 5123 2556 5124 2142 5124 2555 5124 2557 5125 2556 5125 2555 5125 2558 5126 2793 5126 2794 5126 2558 5127 2557 5127 2793 5127 2558 5128 2556 5128 2557 5128 2559 5129 2794 5129 2560 5129 2559 5130 2558 5130 2794 5130 2560 5131 2790 5131 2139 5131 2560 5132 2139 5132 1653 5132 2560 5133 1653 5133 2559 5133 2154 5134 2640 5134 2155 5134 2156 5135 2155 5135 2561 5135 2158 5136 2156 5136 2561 5136 2146 5137 2561 5137 2797 5137 2147 5138 2797 5138 2796 5138 2147 5139 2146 5139 2797 5139 2562 5140 2796 5140 2795 5140 2151 5141 2147 5141 2796 5141 2151 5142 2796 5142 2562 5142 2152 5143 2151 5143 2562 5143 2577 5144 2640 5144 2154 5144 2604 5145 2608 5145 2582 5145 2202 5146 2619 5146 2566 5146 2608 5147 2195 5147 2582 5147 2274 5148 2190 5148 2563 5148 1927 5149 2622 5149 2613 5149 2582 5150 2195 5150 1927 5150 2565 5151 2191 5151 2607 5151 2200 5152 2638 5152 2564 5152 2568 5153 2574 5153 2208 5153 2638 5154 2576 5154 2567 5154 2279 5155 2274 5155 2563 5155 2564 5156 2638 5156 2567 5156 2201 5157 2202 5157 2566 5157 2566 5158 2165 5158 2201 5158 2279 5159 2190 5159 2648 5159 2563 5160 2190 5160 2279 5160 2201 5161 2165 5161 2572 5161 2190 5162 2630 5162 2295 5162 2648 5163 2190 5163 2295 5163 2208 5164 2574 5164 2626 5164 2639 5165 2568 5165 2620 5165 2207 5166 2569 5166 2634 5166 2200 5167 2201 5167 2572 5167 2191 5168 2168 5168 2599 5168 2607 5169 2191 5169 2599 5169 2576 5170 2575 5170 2591 5170 2575 5171 2576 5171 2578 5171 2575 5172 2578 5172 2577 5172 2626 5173 2574 5173 1924 5173 2197 5174 2570 5174 2579 5174 2620 5175 2568 5175 2208 5175 2570 5176 2573 5176 2587 5176 2579 5177 2570 5177 2587 5177 2591 5178 2575 5178 2577 5178 1924 5179 2574 5179 2584 5179 2630 5180 2193 5180 2581 5180 2567 5181 2576 5181 2591 5181 2199 5182 2200 5182 2564 5182 2587 5183 2573 5183 2580 5183 2578 5184 2594 5184 2585 5184 2588 5185 2578 5185 2585 5185 2587 5186 2580 5186 2586 5186 2605 5187 2583 5187 2595 5187 2564 5188 2567 5188 2199 5188 2595 5189 2636 5189 2605 5189 2613 5190 2639 5190 2621 5190 2630 5191 2581 5191 2597 5191 2605 5192 2636 5192 2565 5192 2162 5193 2584 5193 2163 5193 2609 5194 2207 5194 2589 5194 2199 5195 2567 5195 2591 5195 2604 5196 2597 5196 2598 5196 2586 5197 2580 5197 2189 5197 2593 5198 2171 5198 2596 5198 2189 5199 2592 5199 2586 5199 2193 5200 2195 5200 2608 5200 2577 5201 2578 5201 2588 5201 2592 5202 2189 5202 2590 5202 2592 5203 2590 5203 1823 5203 2621 5204 2639 5204 2620 5204 2594 5205 2175 5205 2801 5205 2175 5206 2625 5206 2801 5206 2599 5207 2168 5207 2635 5207 2577 5208 2588 5208 2585 5208 2605 5209 2565 5209 2607 5209 2577 5210 2585 5210 2606 5210 2606 5211 2585 5211 2616 5211 2585 5212 2594 5212 2616 5212 1823 5213 2590 5213 2602 5213 2175 5214 2600 5214 2641 5214 2625 5215 2175 5215 2641 5215 2171 5216 2617 5216 2603 5216 1927 5217 2604 5217 2582 5217 2597 5218 2581 5218 2193 5218 1823 5219 2602 5219 1843 5219 2601 5220 2178 5220 2610 5220 2616 5221 2594 5221 2801 5221 2607 5222 2599 5222 2624 5222 2624 5223 2599 5223 2635 5223 2635 5224 2168 5224 2593 5224 1843 5225 2602 5225 2601 5225 2600 5226 2167 5226 2611 5226 2195 5227 2196 5227 2622 5227 2633 5228 2609 5228 2615 5228 2801 5229 2606 5229 2616 5229 1843 5230 2601 5230 2612 5230 2617 5231 2627 5231 2614 5231 2612 5232 2601 5232 2610 5232 2597 5233 2193 5233 2598 5233 2593 5234 2596 5234 2569 5234 2178 5235 2619 5235 2204 5235 2621 5236 1927 5236 2613 5236 2569 5237 2596 5237 2171 5237 2614 5238 2627 5238 2615 5238 1924 5239 2584 5239 2631 5239 2611 5240 2167 5240 2618 5240 2635 5241 2593 5241 2571 5241 2611 5242 2618 5242 2623 5242 2571 5243 2593 5243 2569 5243 2631 5244 2584 5244 2162 5244 2598 5245 2193 5245 2608 5245 2603 5246 2617 5246 2589 5246 2208 5247 2621 5247 2620 5247 2569 5248 2171 5248 2634 5248 2612 5249 2610 5249 2178 5249 2612 5250 2178 5250 2632 5250 2641 5251 2600 5251 2611 5251 2631 5252 2162 5252 2628 5252 2162 5253 2161 5253 2628 5253 1924 5254 2208 5254 2626 5254 2632 5255 2178 5255 2204 5255 2615 5256 2627 5256 2185 5256 2615 5257 2185 5257 2633 5257 2623 5258 2618 5258 2629 5258 2623 5259 2629 5259 2583 5259 2204 5260 2612 5260 2632 5260 2619 5261 2165 5261 2566 5261 2204 5262 2619 5262 2637 5262 2628 5263 1924 5263 2631 5263 2622 5264 2196 5264 2613 5264 2628 5265 1376 5265 1947 5265 2196 5266 2568 5266 2639 5266 2801 5267 2625 5267 2641 5267 2634 5268 2171 5268 2603 5268 2617 5269 2614 5269 2609 5269 2589 5270 2617 5270 2609 5270 2583 5271 2629 5271 2595 5271 2598 5272 2608 5272 2604 5272 2613 5273 2196 5273 2639 5273 2609 5274 2614 5274 2615 5274 2195 5275 2622 5275 1927 5275 2633 5276 2185 5276 2274 5276 2629 5277 2565 5277 2636 5277 2624 5278 2635 5278 2571 5278 2202 5279 2204 5279 2637 5279 2637 5280 2619 5280 2202 5280 2572 5281 2165 5281 2638 5281 2595 5282 2629 5282 2636 5282 2295 5283 2630 5283 2597 5283 2274 5284 2185 5284 2190 5284 2572 5285 2638 5285 2200 5285 2603 5286 2589 5286 2207 5286 2634 5287 2603 5287 2207 5287 2577 5288 2799 5288 2640 5288 2799 5289 2798 5289 2640 5289 2798 5290 2155 5290 2640 5290 2802 5291 2641 5291 2884 5291 2225 5292 2228 5292 2286 5292 2225 5293 2286 5293 2884 5293 2291 5294 2290 5294 1917 5294 2292 5295 1917 5295 2807 5295 2292 5296 2291 5296 1917 5296 2642 5297 2807 5297 2806 5297 2642 5298 2292 5298 2807 5298 2642 5299 2806 5299 2805 5299 2285 5300 2642 5300 2805 5300 2643 5301 2805 5301 2803 5301 2643 5302 2285 5302 2805 5302 2286 5303 2643 5303 2803 5303 2809 5304 2299 5304 2279 5304 2809 5305 2279 5305 2645 5305 2809 5306 2645 5306 2644 5306 2644 5307 2645 5307 2646 5307 2296 5308 2646 5308 2294 5308 2646 5309 2648 5309 2294 5309 2644 5310 2646 5310 2810 5310 2646 5311 2296 5311 2810 5311 2645 5312 2648 5312 2647 5312 2646 5313 2645 5313 2647 5313 2648 5314 2646 5314 2647 5314 2297 5315 2810 5315 2597 5315 2650 5316 2299 5316 2889 5316 2298 5317 2650 5317 455 5317 2298 5318 455 5318 2651 5318 2652 5319 2812 5319 2815 5319 2652 5320 2300 5320 2812 5320 2816 5321 2813 5321 2815 5321 2301 5322 2654 5322 2816 5322 2654 5323 2653 5323 2652 5323 2654 5324 2652 5324 2813 5324 2301 5325 2816 5325 2817 5325 2301 5326 2817 5326 678 5326 2823 5327 2812 5327 2300 5327 2822 5328 2823 5328 2656 5328 2823 5329 2300 5329 2656 5329 2657 5330 2822 5330 2655 5330 2822 5331 2656 5331 2655 5331 2657 5332 2655 5332 2308 5332 676 5333 2657 5333 2309 5333 2657 5334 2308 5334 2309 5334 676 5335 2309 5335 2310 5335 2660 5336 2311 5336 2766 5336 2659 5337 2767 5337 2854 5337 2659 5338 2766 5338 2767 5338 2659 5339 2660 5339 2766 5339 2854 5340 2660 5340 2659 5340 2311 5341 2660 5341 2854 5341 2665 5342 2661 5342 2311 5342 2850 5343 2664 5343 2658 5343 2662 5344 2764 5344 2850 5344 2662 5345 2658 5345 2764 5345 2663 5346 2665 5346 2311 5346 2663 5347 2854 5347 2667 5347 2663 5348 2311 5348 2854 5348 2667 5349 2665 5349 2663 5349 2668 5350 2366 5350 2661 5350 2670 5351 2664 5351 2850 5351 2666 5352 2661 5352 2665 5352 2850 5353 2658 5353 2662 5353 2666 5354 2665 5354 2667 5354 2666 5355 2667 5355 2669 5355 2679 5356 2673 5356 2664 5356 2672 5357 2366 5357 2668 5357 2679 5358 2664 5358 2670 5358 2669 5359 2661 5359 2666 5359 2669 5360 2672 5360 2668 5360 2669 5361 2668 5361 2661 5361 2671 5362 2360 5362 2366 5362 2671 5363 2366 5363 2672 5363 2670 5364 2850 5364 2687 5364 2672 5365 2669 5365 2853 5365 2679 5366 2670 5366 2687 5366 2853 5367 2671 5367 2672 5367 2674 5368 2671 5368 2853 5368 2674 5369 2360 5369 2671 5369 2674 5370 2853 5370 2852 5370 2675 5371 2356 5371 2360 5371 2675 5372 2360 5372 2674 5372 2681 5373 2676 5373 2673 5373 2852 5374 2675 5374 2674 5374 2852 5375 2356 5375 2675 5375 2677 5376 2852 5376 2851 5376 2677 5377 2356 5377 2852 5377 2677 5378 2680 5378 2356 5378 2678 5379 2679 5379 2687 5379 2678 5380 2673 5380 2679 5380 2681 5381 2673 5381 2678 5381 2681 5382 2678 5382 2687 5382 2686 5383 2676 5383 2681 5383 2851 5384 2680 5384 2677 5384 2684 5385 2851 5385 2683 5385 2684 5386 2680 5386 2851 5386 2683 5387 2680 5387 2684 5387 2682 5388 2321 5388 2676 5388 2682 5389 2676 5389 2686 5389 2688 5390 2680 5390 2683 5390 2685 5391 2683 5391 2848 5391 2685 5392 2688 5392 2683 5392 2686 5393 2681 5393 2687 5393 2688 5394 2341 5394 2689 5394 2686 5395 2687 5395 2849 5395 2682 5396 2686 5396 2849 5396 2690 5397 2341 5397 2689 5397 2689 5398 2685 5398 2848 5398 2689 5399 2688 5399 2685 5399 2690 5400 2689 5400 2848 5400 2692 5401 2342 5401 2341 5401 2692 5402 2341 5402 2690 5402 2848 5403 2342 5403 2692 5403 2692 5404 2690 5404 2848 5404 2691 5405 2312 5405 2321 5405 2337 5406 2342 5406 2848 5406 2693 5407 2337 5407 2848 5407 2694 5408 2682 5408 2849 5408 2694 5409 2321 5409 2682 5409 2691 5410 2694 5410 2849 5410 2691 5411 2321 5411 2694 5411 2696 5412 2695 5412 2337 5412 2696 5413 2337 5413 2693 5413 2334 5414 2695 5414 2696 5414 2334 5415 2696 5415 3121 5415 2700 5416 2313 5416 2312 5416 2697 5417 2334 5417 3121 5417 2698 5418 2691 5418 2849 5418 2698 5419 2849 5419 2846 5419 2698 5420 2312 5420 2691 5420 2700 5421 2698 5421 2846 5421 2700 5422 2312 5422 2698 5422 2701 5423 2697 5423 3121 5423 2702 5424 3121 5424 483 5424 2702 5425 2699 5425 2701 5425 2702 5426 483 5426 2699 5426 2702 5427 2701 5427 3121 5427 2703 5428 2704 5428 2313 5428 2703 5429 2700 5429 2846 5429 2703 5430 2313 5430 2700 5430 2707 5431 2704 5431 2703 5431 2708 5432 2705 5432 2314 5432 2713 5433 2709 5433 2704 5433 2713 5434 2704 5434 2707 5434 2362 5435 2705 5435 2708 5435 2707 5436 2703 5436 2846 5436 2708 5437 2314 5437 2706 5437 2707 5438 2846 5438 2712 5438 2712 5439 2713 5439 2707 5439 2710 5440 2362 5440 2708 5440 2714 5441 2715 5441 2709 5441 2716 5442 2362 5442 2710 5442 2711 5443 2712 5443 2845 5443 2711 5444 2713 5444 2712 5444 2711 5445 2709 5445 2713 5445 2714 5446 2709 5446 2711 5446 2714 5447 2711 5447 2845 5447 2717 5448 2719 5448 2362 5448 2717 5449 2362 5449 2716 5449 2721 5450 2723 5450 2715 5450 2716 5451 2710 5451 2842 5451 2718 5452 2714 5452 2845 5452 2718 5453 2715 5453 2714 5453 2721 5454 2718 5454 2845 5454 2721 5455 2715 5455 2718 5455 2842 5456 2717 5456 2716 5456 2722 5457 2727 5457 2719 5457 2724 5458 2721 5458 2845 5458 2724 5459 2723 5459 2721 5459 2724 5460 2845 5460 2841 5460 2841 5461 2723 5461 2724 5461 2725 5462 2717 5462 2842 5462 2725 5463 2719 5463 2717 5463 2725 5464 2842 5464 2843 5464 2720 5465 2723 5465 2841 5465 2730 5466 2351 5466 2720 5466 2843 5467 2719 5467 2725 5467 2843 5468 2722 5468 2719 5468 2726 5469 2720 5469 2841 5469 2726 5470 2841 5470 2838 5470 2728 5471 2353 5471 2727 5471 2730 5472 2720 5472 2726 5472 2730 5473 2726 5473 2838 5473 2731 5474 2351 5474 2730 5474 2732 5475 2344 5475 2351 5475 2729 5476 2727 5476 2722 5476 2729 5477 2722 5477 2843 5477 2729 5478 2843 5478 2733 5478 2732 5479 2351 5479 2731 5479 2731 5480 2730 5480 2838 5480 2732 5481 2731 5481 2838 5481 2733 5482 2728 5482 2727 5482 2733 5483 2353 5483 2728 5483 2735 5484 2344 5484 2732 5484 2733 5485 2727 5485 2729 5485 2736 5486 2348 5486 2353 5486 2734 5487 2344 5487 2735 5487 2734 5488 2339 5488 2344 5488 2737 5489 2736 5489 2353 5489 2737 5490 2353 5490 2733 5490 2737 5491 2733 5491 2745 5491 2735 5492 2732 5492 2838 5492 2741 5493 2348 5493 2736 5493 2735 5494 2838 5494 2739 5494 2734 5495 2735 5495 2739 5495 2739 5496 2339 5496 2734 5496 2740 5497 2335 5497 2339 5497 2736 5498 2737 5498 2745 5498 2740 5499 2339 5499 2739 5499 2743 5500 2348 5500 2741 5500 2743 5501 2738 5501 2348 5501 2744 5502 2335 5502 2740 5502 2741 5503 2736 5503 2745 5503 2744 5504 2740 5504 2739 5504 2744 5505 2742 5505 2335 5505 2744 5506 2739 5506 2747 5506 2743 5507 2741 5507 2745 5507 2748 5508 2345 5508 2738 5508 2749 5509 2753 5509 2742 5509 2751 5510 2743 5510 2745 5510 2751 5511 2738 5511 2743 5511 2751 5512 2745 5512 2756 5512 2750 5513 2747 5513 2746 5513 2751 5514 2748 5514 2738 5514 2750 5515 2744 5515 2747 5515 2750 5516 2742 5516 2744 5516 2755 5517 2345 5517 2748 5517 2749 5518 2750 5518 2746 5518 2749 5519 2742 5519 2750 5519 2748 5520 2751 5520 2756 5520 2752 5521 2753 5521 2749 5521 2752 5522 2749 5522 2746 5522 2746 5523 2753 5523 2752 5523 2757 5524 2760 5524 2345 5524 2757 5525 2345 5525 2755 5525 2754 5526 2753 5526 2746 5526 2755 5527 2748 5527 2756 5527 2758 5528 2754 5528 2746 5528 2757 5529 2755 5529 2756 5529 2762 5530 2322 5530 2759 5530 2761 5531 2759 5531 2754 5531 2761 5532 2754 5532 2746 5532 2761 5533 2746 5533 2854 5533 2764 5534 2658 5534 2760 5534 2762 5535 2759 5535 2761 5535 2762 5536 2761 5536 2854 5536 2767 5537 2766 5537 2322 5537 2763 5538 2757 5538 2756 5538 2763 5539 2760 5539 2757 5539 2763 5540 2756 5540 2850 5540 2765 5541 2762 5541 2854 5541 2765 5542 2322 5542 2762 5542 2764 5543 2760 5543 2763 5543 2765 5544 2767 5544 2322 5544 2764 5545 2763 5545 2850 5545 2767 5546 2765 5546 2854 5546 2768 5547 2440 5547 2865 5547 2440 5548 2768 5548 1544 5548 2862 5549 2863 5549 2446 5549 2863 5550 2451 5550 2446 5550 672 5551 2862 5551 2446 5551 2451 5552 2863 5552 2769 5552 2865 5553 2453 5553 2770 5553 2453 5554 2455 5554 2770 5554 2770 5555 2455 5555 1984 5555 2773 5556 2771 5556 2772 5556 2456 5557 2457 5557 2867 5557 2867 5558 2772 5558 2456 5558 2772 5559 2771 5559 2456 5559 2773 5560 1855 5560 2771 5560 2867 5561 2457 5561 1617 5561 2777 5562 2867 5562 1617 5562 2773 5563 2866 5563 2774 5563 2773 5564 2774 5564 1996 5564 2463 5565 1996 5565 2774 5565 2866 5566 2868 5566 2774 5566 2868 5567 2775 5567 2774 5567 2774 5568 2775 5568 2777 5568 2464 5569 2774 5569 2777 5569 2775 5570 2776 5570 2777 5570 2776 5571 2974 5571 2777 5571 2974 5572 2867 5572 2777 5572 1617 5573 1998 5573 2777 5573 2778 5574 2773 5574 2772 5574 2783 5575 2873 5575 1595 5575 2783 5576 1595 5576 2780 5576 2781 5577 2783 5577 2780 5577 2783 5578 2988 5578 2873 5578 2782 5579 2988 5579 2783 5579 2467 5580 2782 5580 2783 5580 2465 5581 2782 5581 2467 5581 2779 5582 2782 5582 2465 5582 2782 5583 2779 5583 2784 5583 2782 5584 2784 5584 2785 5584 2543 5585 2786 5585 2548 5585 2785 5586 2784 5586 2554 5586 2554 5587 2077 5587 2788 5587 2077 5588 2787 5588 2788 5588 2554 5589 2788 5589 2548 5589 2870 5590 2785 5590 2786 5590 2785 5591 2554 5591 2786 5591 2786 5592 2554 5592 2548 5592 2789 5593 2870 5593 2786 5593 2136 5594 2790 5594 2789 5594 2557 5595 2791 5595 2873 5595 2557 5596 2792 5596 2791 5596 2790 5597 2877 5597 2789 5597 2790 5598 2873 5598 2877 5598 2873 5599 2790 5599 2560 5599 2794 5600 2873 5600 2560 5600 2557 5601 2873 5601 2794 5601 2873 2782 2988 2782 666 2782 2557 5602 2794 5602 2793 5602 2873 5603 2791 5603 1622 5603 1596 5604 1595 5604 1622 5604 1596 5605 1621 5605 1620 5605 1596 5606 1622 5606 1621 5606 1598 5607 1620 5607 1210 5607 1598 5608 1596 5608 1620 5608 1597 5609 1598 5609 1210 5609 2796 5610 2881 5610 1679 5610 2796 5611 1679 5611 2795 5611 2797 5612 2881 5612 2796 5612 2155 5613 2881 5613 2797 5613 2155 5614 2883 5614 2881 5614 2561 5615 2155 5615 2797 5615 2155 5616 2798 5616 2883 5616 2577 5617 2606 5617 2799 5617 2801 5618 2641 5618 2802 5618 2798 5619 2799 5619 2800 5619 2606 5620 2801 5620 2799 5620 2801 5621 2802 5621 2799 5621 2802 5622 2884 5622 2800 5622 2799 5623 2802 5623 2800 5623 2884 5624 2286 5624 2803 5624 2806 5625 2804 5625 1907 5625 2807 5626 2804 5626 2806 5626 2807 5627 1917 5627 2804 5627 2803 5628 2880 5628 2884 5628 2806 5629 2880 5629 2803 5629 2806 5630 2803 5630 2805 5630 2806 5631 3070 5631 2880 5631 2806 5632 2881 5632 3070 5632 2806 5633 1907 5633 2881 5633 1678 5634 1679 5634 1907 5634 1678 5635 2808 5635 1381 5635 1678 5636 1907 5636 2808 5636 1676 5637 1381 5637 1908 5637 1676 5638 1678 5638 1381 5638 1677 5639 1676 5639 1908 5639 2889 5640 2809 5640 2890 5640 2809 5641 2644 5641 2890 5641 2644 5642 2810 5642 2297 5642 2890 5643 2644 5643 2297 5643 2811 5644 2297 5644 1966 5644 2811 5645 2297 5645 2890 5645 660 5646 2892 5646 456 5646 2892 5647 2811 5647 456 5647 2809 5648 2889 5648 2299 5648 2816 5649 2654 5649 2813 5649 2815 5650 2814 5650 2816 5650 2816 5651 2814 5651 2817 5651 2819 5652 2818 5652 2821 5652 2821 5653 677 5653 2820 5653 677 5654 2821 5654 676 5654 2821 5655 2822 5655 676 5655 676 5656 2822 5656 2657 5656 2822 5657 2815 5657 2823 5657 2815 5658 2812 5658 2823 5658 513 5659 2934 5659 521 5659 2903 5660 2907 5660 2854 5660 521 5661 2934 5661 2937 5661 2746 5662 2903 5662 2854 5662 2827 5663 2824 5663 2954 5663 2825 5664 2934 5664 513 5664 2825 5665 2955 5665 2934 5665 2826 5666 2937 5666 2940 5666 2826 5667 521 5667 2937 5667 510 5668 2955 5668 2825 5668 510 5669 2953 5669 2955 5669 506 5670 2953 5670 510 5670 2933 5671 2827 5671 2954 5671 544 5672 2826 5672 2940 5672 2828 5673 2831 5673 2953 5673 2828 5674 2953 5674 506 5674 2829 5675 2827 5675 2933 5675 2830 5676 2831 5676 2828 5676 2832 5677 544 5677 2940 5677 2837 5678 2832 5678 2940 5678 2747 5679 2903 5679 2746 5679 496 5680 2831 5680 2830 5680 2899 5681 2903 5681 2747 5681 2899 5682 2747 5682 2739 5682 2833 5683 2829 5683 2933 5683 2836 5684 2833 5684 2933 5684 2834 5685 2831 5685 496 5685 2834 5686 2944 5686 2831 5686 461 5687 2832 5687 2837 5687 2835 5688 2833 5688 2836 5688 488 5689 2944 5689 2834 5689 462 5690 461 5690 2837 5690 2899 5691 2739 5691 2838 5691 484 5692 2944 5692 488 5692 2706 5693 2835 5693 2836 5693 483 5694 2944 5694 484 5694 464 5695 462 5695 2837 5695 2708 5696 2706 5696 2836 5696 2840 5697 464 5697 2837 5697 3121 5698 2944 5698 483 5698 2839 5699 464 5699 2840 5699 2919 5700 2708 5700 2836 5700 2919 5701 2710 5701 2708 5701 2914 5702 2899 5702 2838 5702 2914 5703 2838 5703 2841 5703 2919 5704 2842 5704 2710 5704 471 5705 2839 5705 2840 5705 2914 5706 2841 5706 2845 5706 2935 5707 471 5707 2840 5707 2844 5708 2842 5708 2919 5708 2844 5709 2843 5709 2842 5709 2696 5710 2928 5710 3121 5710 2844 5711 2733 5711 2843 5711 2912 5712 2845 5712 2712 5712 2912 5713 2914 5713 2845 5713 477 5714 471 5714 2935 5714 2844 5715 2745 5715 2733 5715 2693 5716 2928 5716 2696 5716 2912 5717 2712 5717 2846 5717 2938 5718 477 5718 2935 5718 2924 5719 2928 5719 2693 5719 2848 5720 2924 5720 2693 5720 2926 5721 2745 5721 2844 5721 2847 5722 477 5722 2938 5722 2926 5723 2756 5723 2745 5723 2912 5724 2846 5724 2849 5724 2930 5725 2756 5725 2926 5725 2930 5726 2850 5726 2756 5726 2930 5727 2912 5727 2849 5727 2930 5728 2849 5728 2687 5728 2683 5729 2924 5729 2848 5729 499 5730 2847 5730 2938 5730 2943 5731 499 5731 2938 5731 2930 5732 2687 5732 2850 5732 2851 5733 2924 5733 2683 5733 512 5734 499 5734 2943 5734 2852 5735 2924 5735 2851 5735 516 5736 512 5736 2943 5736 2913 5737 2924 5737 2852 5737 2853 5738 2913 5738 2852 5738 526 5739 516 5739 2943 5739 2669 5740 2913 5740 2853 5740 2949 5741 526 5741 2943 5741 2667 5742 2913 5742 2669 5742 543 5743 526 5743 2949 5743 2907 5744 2913 5744 2667 5744 2854 5745 2907 5745 2667 5745 2824 5746 543 5746 2949 5746 2954 5747 2824 5747 2949 5747 2865 5748 2855 5748 2768 5748 2856 5749 2855 5749 2442 5749 2442 5750 2855 5750 2857 5750 673 5751 2857 5751 2859 5751 673 5752 2859 5752 2858 5752 2860 5753 2859 5753 2861 5753 672 5754 2861 5754 2862 5754 2862 5755 2861 5755 2863 5755 2864 5756 2769 5756 2863 5756 2861 5757 2864 5757 2863 5757 1984 5758 2769 5758 2770 5758 2770 5759 2769 5759 2864 5759 2770 5760 2864 5760 2865 5760 2773 5761 2869 5761 2866 5761 2773 2782 2778 2782 2869 2782 2874 5762 2871 5762 2870 5762 2782 5763 2785 5763 2878 5763 2782 5764 2878 5764 2872 5764 2874 2782 2870 2782 2789 2782 2875 5765 2870 5765 2871 5765 2877 2782 2874 2782 2789 2782 1595 5766 2873 5766 1622 5766 2785 2782 2875 2782 2876 2782 2785 2782 2870 2782 2875 2782 2782 5767 2872 5767 2986 5767 2988 5768 2782 5768 2986 5768 2878 2782 2785 2782 2876 2782 666 5769 2877 5769 2873 5769 2800 2782 2879 2782 663 2782 2800 2782 663 2782 2798 2782 2881 5770 1907 5770 1679 5770 2884 5771 2879 5771 2800 5771 2884 5772 2880 5772 2882 5772 2884 5773 2882 5773 3071 5773 2884 5774 3071 5774 2879 5774 665 5775 2881 5775 2883 5775 2885 5776 2881 5776 665 5776 2885 5777 3070 5777 2881 5777 2886 2782 2883 2782 2798 2782 663 2782 2886 2782 2798 2782 2890 5778 2892 5778 3073 5778 2890 5779 2811 5779 2892 5779 2889 5780 3075 5780 2891 5780 2890 5781 3073 5781 3075 5781 2889 5782 2890 5782 3075 5782 3078 5783 2898 5783 2822 5783 3078 5784 2822 5784 2821 5784 2894 5785 2815 5785 2896 5785 2814 5786 2895 5786 2818 5786 2815 5787 2894 5787 2893 5787 2895 5788 3078 5788 2821 5788 2818 5789 2895 5789 2821 5789 2896 2782 2815 2782 3079 2782 2814 5790 2815 5790 2897 5790 2815 5791 2893 5791 2897 5791 3079 5792 2815 5792 3080 5792 3080 5793 2815 5793 2822 5793 2898 5794 3080 5794 2822 5794 2897 5795 657 5795 2814 5795 657 5796 658 5796 2814 5796 2930 5797 427 5797 431 5797 2903 5798 2899 5798 423 5798 2900 5799 2903 5799 423 5799 439 5800 2930 5800 431 5800 2928 5801 2929 5801 436 5801 2901 5802 2900 5802 363 5802 2903 5803 2900 5803 2901 5803 2930 5804 439 5804 368 5804 2904 5805 2930 5805 368 5805 2903 5806 2901 5806 2902 5806 375 5807 2903 5807 2902 5807 2930 5808 2904 5808 2905 5808 2905 5809 2912 5809 2930 5809 3156 5810 2933 5810 2932 5810 375 5811 2907 5811 2903 5811 2836 5812 2933 5812 3156 5812 2906 5813 380 5813 2908 5813 2906 5814 2905 5814 380 5814 2912 5815 2905 5815 2906 5815 377 5816 2907 5816 375 5816 2906 5817 2908 5817 2911 5817 2907 5818 377 5818 2909 5818 383 5819 2907 5819 2909 5819 2912 5820 2906 5820 2911 5820 2913 5821 2907 5821 383 5821 401 5822 2911 5822 2910 5822 2916 5823 2919 5823 390 5823 401 5824 2912 5824 2911 5824 2916 5825 390 5825 391 5825 2913 5826 383 5826 2915 5826 2914 5827 2912 5827 401 5827 2913 5828 2915 5828 2917 5828 2916 5829 391 5829 397 5829 389 5830 2913 5830 2917 5830 2918 5831 2914 5831 401 5831 2916 5832 2844 5832 2919 5832 2921 5833 397 5833 407 5833 2921 5834 2916 5834 397 5834 2924 5835 2913 5835 389 5835 2921 5836 2844 5836 2916 5836 2924 5837 389 5837 2920 5837 2921 5838 407 5838 413 5838 2914 5839 2918 5839 408 5839 416 5840 2921 5840 413 5840 416 5841 2844 5841 2921 5841 2899 5842 2914 5842 408 5842 2925 5843 408 5843 2922 5843 2844 5844 416 5844 2923 5844 421 5845 2924 5845 409 5845 2899 5846 408 5846 2925 5846 2923 5847 2926 5847 2844 5847 2924 5848 421 5848 424 5848 2926 5849 2923 5849 2927 5849 427 5850 2926 5850 2927 5850 2928 5851 2924 5851 424 5851 423 5852 2925 5852 425 5852 2899 5853 2925 5853 423 5853 427 5854 2930 5854 2926 5854 2929 5855 2928 5855 424 5855 2933 5856 2957 5856 2932 5856 2931 5857 2956 5857 3150 5857 2934 5858 2958 5858 3086 5858 2935 5859 2956 5859 2931 5859 2934 5860 3086 5860 2936 5860 3168 5861 2931 5861 3164 5861 3168 5862 2935 5862 2931 5862 2938 5863 2935 5863 3168 5863 2937 5864 2936 5864 3088 5864 2937 5865 2934 5865 2936 5865 3093 5866 2937 5866 3088 5866 2938 5867 3168 5867 3084 5867 3121 5868 2928 5868 366 5868 3104 5869 3093 5869 3102 5869 3087 5870 2938 5870 3084 5870 3104 5871 2937 5871 3093 5871 367 5872 3121 5872 366 5872 2940 5873 2937 5873 3104 5873 2943 5874 2938 5874 3087 5874 2943 5875 3087 5875 2939 5875 2941 5876 2939 5876 3097 5876 2942 5877 3104 5877 3107 5877 2940 5878 3104 5878 2942 5878 2943 5879 2939 5879 2941 5879 2941 5880 3097 5880 3108 5880 2941 5881 3108 5881 2945 5881 3118 5882 2942 5882 3115 5882 3118 5883 2940 5883 2942 5883 2837 5884 2940 5884 3118 5884 2945 5885 2943 5885 2941 5885 2949 5886 2943 5886 2945 5886 2831 5887 3129 5887 2946 5887 2831 5888 2944 5888 3129 5888 3139 5889 2831 5889 2946 5889 2949 5890 2945 5890 3120 5890 2837 5891 3118 5891 2947 5891 2948 5892 2831 5892 3139 5892 2837 5893 2947 5893 2950 5893 3124 5894 2949 5894 3120 5894 2953 5895 2831 5895 2948 5895 2954 5896 2949 5896 3124 5896 2954 5897 3124 5897 3125 5897 2840 5898 2837 5898 2950 5898 2840 5899 2950 5899 2951 5899 3143 5900 2840 5900 2951 5900 2954 5901 3125 5901 3134 5901 3163 5902 2953 5902 2952 5902 2954 5903 3134 5903 3133 5903 2956 5904 3143 5904 3148 5904 2955 5905 2953 5905 3163 5905 2933 5906 2954 5906 3133 5906 3147 5907 2933 5907 3133 5907 2956 5908 2840 5908 3143 5908 2955 5909 3163 5909 3172 5909 2935 5910 2840 5910 2956 5910 2958 5911 2955 5911 3172 5911 2956 5912 3148 5912 3150 5912 2958 5913 2934 5913 2955 5913 2957 5914 2933 5914 3147 5914 3179 5915 3181 5915 2857 5915 2959 5916 3178 5916 2865 5916 2864 5917 2959 5917 2865 5917 3179 5918 2857 5918 2855 5918 3178 5919 2961 5919 2865 5919 2864 5920 2861 5920 2962 5920 2960 2782 2864 2782 2962 2782 2961 5921 3179 5921 2855 5921 2865 5922 2961 5922 2855 5922 2864 5923 2960 5923 2963 5923 2962 5924 2861 5924 3183 5924 2861 5925 2859 5925 3183 5925 2864 5926 2963 5926 2964 5926 3183 5927 2859 5927 3182 5927 2964 2782 2959 2782 2864 2782 3181 5928 3182 5928 2859 5928 3181 5929 2859 5929 2857 5929 3943 5930 3194 5930 2972 5930 3372 5931 3193 5931 2972 5931 3194 5932 3372 5932 2972 5932 3195 5933 3375 5933 3196 5933 3375 5934 2965 5934 3196 5934 2967 5935 451 5935 452 5935 3216 5936 2967 5936 452 5936 3221 5937 2967 5937 3216 5937 3214 5938 3221 5938 3216 5938 3222 5939 3221 5939 3214 5939 3222 5940 3214 5940 3212 5940 3201 5941 2969 5941 2968 5941 2970 5942 2969 5942 3201 5942 2970 5943 3186 5943 2969 5943 3223 5944 3222 5944 3212 5944 3223 5945 3212 5945 3210 5945 3184 5946 3201 5946 2968 5946 3382 5947 3223 5947 3210 5947 3382 5948 3210 5948 3211 5948 3199 5949 3186 5949 2970 5949 3382 5950 3211 5950 3209 5950 3198 5951 3186 5951 3199 5951 3198 5952 3573 5952 3186 5952 3382 5953 3209 5953 3593 5953 3202 5954 3201 5954 3184 5954 3593 5955 3225 5955 3382 5955 3190 5956 3573 5956 3198 5956 3207 5957 3225 5957 3593 5957 3191 5958 3190 5958 3198 5958 3191 5959 3198 5959 3197 5959 3207 5960 3226 5960 3225 5960 3202 5961 3184 5961 3567 5961 3204 5962 3202 5962 3567 5962 2971 5963 3226 5963 3207 5963 2971 5964 3204 5964 3567 5964 2971 5965 3567 5965 3226 5965 3197 5966 3196 5966 3191 5966 3191 5967 3196 5967 2972 5967 2974 5968 3232 5968 2973 5968 2867 5969 2974 5969 2973 5969 2772 5970 2867 5970 3230 5970 2867 5971 2973 5971 3230 5971 2778 5972 2772 5972 3229 5972 2772 5973 3230 5973 3229 5973 2778 5974 3229 5974 2975 5974 2869 5975 2778 5975 2975 5975 2866 5976 2869 5976 2975 5976 2866 5977 2975 5977 3231 5977 3395 5978 3232 5978 2980 5978 2978 5979 671 5979 2983 5979 2978 5980 3227 5980 2980 5980 3392 5981 2984 5981 2983 5981 2980 5982 3227 5982 2979 5982 2976 5983 2977 5983 2978 5983 2983 5984 2976 5984 2978 5984 2975 5985 3228 5985 2983 5985 3231 5986 2975 5986 2983 5986 671 5987 3231 5987 2983 5987 2982 5988 2978 5988 2980 5988 3232 5989 2982 5989 2980 5989 2983 5990 2984 5990 2976 5990 2978 5991 2977 5991 3227 5991 2876 5992 2875 5992 2985 5992 2878 5993 2876 5993 3240 5993 2876 5994 2985 5994 3240 5994 2878 5995 3240 5995 3241 5995 2872 5996 2878 5996 3241 5996 2872 5997 3241 5997 2987 5997 2986 5998 2872 5998 2987 5998 2988 5999 2986 5999 3238 5999 2986 6000 2987 6000 3238 6000 2988 6001 3238 6001 2989 6001 2990 6002 3243 6002 3554 6002 3245 6003 3068 6003 410 6003 2995 6004 3065 6004 2992 6004 2994 6005 3065 6005 2995 6005 3246 6006 3247 6006 3069 6006 3246 6007 3069 6007 2991 6007 3246 6008 2991 6008 2993 6008 2997 6009 3245 6009 398 6009 3246 6010 2993 6010 3137 6010 2997 6011 398 6011 388 6011 3250 6012 3246 6012 2999 6012 2999 6013 3246 6013 3137 6013 3001 6014 2995 6014 3109 6014 2996 6015 2997 6015 388 6015 2998 6016 2995 6016 3001 6016 3002 6017 388 6017 387 6017 3002 6018 2996 6018 388 6018 3003 6019 2999 6019 3137 6019 3002 6020 3252 6020 2996 6020 3001 6021 3109 6021 3000 6021 3003 6022 3137 6022 3122 6022 3001 6023 3000 6023 3100 6023 3002 6024 3004 6024 3252 6024 3004 6025 3002 6025 387 6025 3251 6026 3001 6026 3100 6026 3005 6027 3003 6027 3122 6027 3255 6028 3251 6028 3100 6028 3007 6029 3100 6029 3092 6029 3006 6030 3005 6030 3009 6030 3007 6031 3255 6031 3100 6031 3009 6032 3113 6032 3006 6032 3011 6033 3255 6033 3007 6033 3010 6034 3012 6034 372 6034 3011 6035 3263 6035 3255 6035 3010 6036 372 6036 3014 6036 3011 6037 3176 6037 3013 6037 3015 6038 3010 6038 3014 6038 3269 6039 3010 6039 3015 6039 3015 6040 3014 6040 365 6040 3017 6041 3483 6041 3016 6041 3017 6042 3016 6042 3101 6042 3019 6043 3244 6043 395 6043 307 6044 3244 6044 3019 6044 3018 6045 3015 6045 365 6045 3279 6046 3015 6046 3018 6046 3017 6047 3277 6047 3483 6047 3017 6048 3280 6048 3277 6048 3020 6049 3017 6049 3023 6049 3022 6050 307 6050 3019 6050 3022 6051 3019 6051 3021 6051 3022 6052 3021 6052 385 6052 3280 6053 3017 6053 3020 6053 3027 6054 3018 6054 429 6054 3020 6055 3284 6055 3280 6055 3284 6056 3020 6056 3023 6056 3024 6057 3022 6057 385 6057 3024 6058 385 6058 3025 6058 3027 6059 3279 6059 3018 6059 3275 6060 3022 6060 3024 6060 3026 6061 3027 6061 429 6061 3167 6062 3030 6062 3284 6062 3024 6063 3282 6063 3275 6063 3024 6064 3025 6064 3029 6064 3031 6065 3027 6065 3026 6065 3028 6066 3027 6066 3031 6066 3286 6067 3024 6067 3029 6067 3286 6068 3282 6068 3024 6068 3033 6069 3030 6069 3167 6069 3034 6070 3029 6070 3035 6070 3034 6071 3286 6071 3029 6071 3033 6072 3167 6072 3037 6072 3036 6073 3031 6073 415 6073 3032 6074 3031 6074 3036 6074 3034 6075 3038 6075 3286 6075 3039 6076 3033 6076 3037 6076 3034 6077 3035 6077 374 6077 3043 6078 3034 6078 374 6078 3039 6079 3037 6079 3041 6079 3043 6080 3038 6080 3034 6080 3042 6081 3032 6081 3036 6081 3043 6082 374 6082 3040 6082 3044 6083 3032 6083 3042 6083 3292 6084 3039 6084 3041 6084 3045 6085 3043 6085 3040 6085 3297 6086 3041 6086 3046 6086 3297 6087 3292 6087 3041 6087 3045 6088 3293 6088 3043 6088 3045 6089 3040 6089 3048 6089 3042 6090 3047 6090 3295 6090 3042 6091 3295 6091 3044 6091 3299 6092 3046 6092 3149 6092 3299 6093 3297 6093 3046 6093 3296 6094 3045 6094 3048 6094 3047 6095 3042 6095 3051 6095 3052 6096 3299 6096 3149 6096 3296 6097 3293 6097 3045 6097 3049 6098 3048 6098 3050 6098 3049 6099 3296 6099 3048 6099 3301 6100 3047 6100 3051 6100 3301 6101 3051 6101 392 6101 3052 6102 3302 6102 3299 6102 3049 6103 357 6103 3296 6103 3049 6104 3050 6104 3053 6104 3060 6105 3052 6105 3145 6105 3054 6106 3301 6106 392 6106 3060 6107 3302 6107 3052 6107 3303 6108 3049 6108 3053 6108 3303 6109 357 6109 3049 6109 3060 6110 3145 6110 3136 6110 3055 6111 3053 6111 3056 6111 3306 6112 3054 6112 3166 6112 3055 6113 3303 6113 3053 6113 3308 6114 3055 6114 3056 6114 3308 6115 3056 6115 422 6115 3057 6116 3060 6116 3136 6116 3059 6117 3306 6117 3166 6117 3058 6118 3060 6118 3057 6118 3058 6119 3061 6119 3060 6119 3308 6120 422 6120 418 6120 3307 6121 3306 6121 3059 6121 3307 6122 3059 6122 3063 6122 3065 6123 3057 6123 3062 6123 3067 6124 3308 6124 418 6124 3064 6125 3307 6125 3063 6125 3067 6126 418 6126 3066 6126 3476 6127 3307 6127 3064 6127 3058 6128 3057 6128 3065 6128 3473 6129 3476 6129 3064 6129 3067 6130 3066 6130 410 6130 3473 6131 3064 6131 3069 6131 3310 6132 3058 6132 3065 6132 3310 6133 3065 6133 2994 6133 3068 6134 3067 6134 410 6134 3247 6135 3473 6135 3069 6135 3070 6136 2885 6136 3318 6136 2885 6137 3321 6137 3318 6137 2880 6138 3070 6138 3323 6138 3070 6139 3318 6139 3323 6139 2882 6140 2880 6140 3323 6140 2882 6141 3323 6141 3316 6141 3071 6142 2882 6142 3316 6142 3071 6143 3316 6143 3313 6143 2879 6144 3071 6144 3313 6144 2879 6145 3313 6145 662 6145 660 6146 661 6146 3331 6146 2892 6147 660 6147 3331 6147 2892 6148 3331 6148 3072 6148 3073 6149 2892 6149 3072 6149 3073 6150 3072 6150 3074 6150 3075 6151 3073 6151 3074 6151 3075 6152 3074 6152 3332 6152 2891 6153 3075 6153 3332 6153 2891 6154 3332 6154 3076 6154 3523 6155 3331 6155 442 6155 2895 6156 2814 6156 3077 6156 2814 6157 3333 6157 3077 6157 3078 6158 2895 6158 3531 6158 2895 6159 3077 6159 3531 6159 2898 6160 3078 6160 3339 6160 3078 6161 3531 6161 3339 6161 3080 6162 2898 6162 3339 6162 3080 6163 3339 6163 3337 6163 3079 6164 3080 6164 3337 6164 3079 6165 3337 6165 3336 6165 3084 6166 3083 6166 3284 6166 3175 6167 3176 6167 3011 6167 3085 6168 3084 6168 3284 6168 3085 6169 3284 6169 3023 6169 3087 6170 3084 6170 3085 6170 3023 6171 3087 6171 3085 6171 3086 6172 3175 6172 3011 6172 3091 6173 3088 6173 2936 6173 3089 6174 3091 6174 2936 6174 3089 6175 2936 6175 3086 6175 3089 6176 3086 6176 3011 6176 3089 6177 3011 6177 3007 6177 3090 6178 3087 6178 3023 6178 3007 6179 3091 6179 3089 6179 3090 6180 3023 6180 3017 6180 3092 6181 3093 6181 3088 6181 3095 6182 3087 6182 3090 6182 3094 6183 3088 6183 3091 6183 3094 6184 3092 6184 3088 6184 3094 6185 3091 6185 3007 6185 2939 6186 3087 6186 3095 6186 3094 6187 3007 6187 3092 6187 3095 6188 3090 6188 3017 6188 3096 6189 2939 6189 3095 6189 3098 6190 3102 6190 3093 6190 3099 6191 3093 6191 3092 6191 3096 6192 3017 6192 3101 6192 3096 6193 3095 6193 3017 6193 3101 6194 3097 6194 2939 6194 3099 6195 3092 6195 3100 6195 3101 6196 2939 6196 3096 6196 3100 6197 3093 6197 3099 6197 3100 6198 3098 6198 3093 6198 3100 6199 3102 6199 3098 6199 3103 6200 3102 6200 3100 6200 3103 6201 3100 6201 3000 6201 3105 6202 3102 6202 3000 6202 3016 6203 3108 6203 3097 6203 3105 6204 3104 6204 3102 6204 3000 6205 3102 6205 3103 6205 3106 6206 3107 6206 3104 6206 3106 6207 3104 6207 3105 6207 3016 6208 3097 6208 3101 6208 3105 6209 3000 6209 3109 6209 3109 6210 3107 6210 3106 6210 3109 6211 3106 6211 3105 6211 3113 6212 2945 6212 3108 6212 3113 6213 3108 6213 3016 6213 3111 6214 2942 6214 3107 6214 3110 6215 3107 6215 3109 6215 3110 6216 3109 6216 2995 6216 3111 6217 3107 6217 3110 6217 3111 6218 3110 6218 2995 6218 3112 6219 3115 6219 2942 6219 3114 6220 2942 6220 3111 6220 3114 6221 3112 6221 2942 6221 3009 6222 2945 6222 3113 6222 3114 6223 3111 6223 2995 6223 3114 6224 2995 6224 2992 6224 3009 6225 3120 6225 2945 6225 3112 6226 3114 6226 2992 6226 3116 6227 3115 6227 3112 6227 3116 6228 3065 6228 3115 6228 3116 6229 2992 6229 3065 6229 3116 6230 3112 6230 2992 6230 3005 6231 3120 6231 3009 6231 3062 6232 3118 6232 3115 6232 3062 6233 3115 6233 3065 6233 3117 6234 3118 6234 3062 6234 3117 6235 2947 6235 3118 6235 3119 6236 3124 6236 3120 6236 3119 6237 3120 6237 3005 6237 2944 6238 3121 6238 3350 6238 3123 6239 2950 6239 2947 6239 3119 6240 3005 6240 3122 6240 3122 6241 3124 6241 3119 6241 3127 6242 3129 6242 2944 6242 3128 6243 2947 6243 3117 6243 3128 6244 3123 6244 2947 6244 3128 6245 3117 6245 3062 6245 3128 6246 3062 6246 3057 6246 3126 6247 3125 6247 3124 6247 3126 6248 3124 6248 3122 6248 3127 6249 2944 6249 3350 6249 3123 6250 3128 6250 3057 6250 3129 6251 3127 6251 3349 6251 3126 6252 3122 6252 3137 6252 3130 6253 3139 6253 2946 6253 3137 6254 3125 6254 3126 6254 3132 6255 2950 6255 3123 6255 3131 6256 3130 6256 2946 6256 3131 6257 3129 6257 3349 6257 3131 6258 3349 6258 3347 6258 3131 6259 2946 6259 3129 6259 3132 6260 3123 6260 3057 6260 3138 6261 3134 6261 3125 6261 3132 6262 3057 6262 3136 6262 3130 6263 3131 6263 3347 6263 3140 6264 2950 6264 3135 6264 3140 6265 2951 6265 2950 6265 3136 6266 2950 6266 3132 6266 3346 6267 3139 6267 3130 6267 3136 6268 3135 6268 2950 6268 3138 6269 3125 6269 3137 6269 3136 6270 3140 6270 3135 6270 3346 6271 3130 6271 3347 6271 3138 6272 3137 6272 2993 6272 2993 6273 3134 6273 3138 6273 3343 6274 2948 6274 3139 6274 3343 6275 3139 6275 3346 6275 3145 6276 3140 6276 3136 6276 3141 6277 3134 6277 2993 6277 3145 6278 2951 6278 3140 6278 3141 6279 3133 6279 3134 6279 3133 6280 3147 6280 3142 6280 3142 6281 3133 6281 3141 6281 2953 6282 2948 6282 3343 6282 3141 6283 2993 6283 2991 6283 3144 6284 3147 6284 3142 6284 2991 6285 3142 6285 3141 6285 2991 6286 3144 6286 3142 6286 3143 6287 2951 6287 3145 6287 3342 6288 2953 6288 3343 6288 3143 6289 3145 6289 3052 6289 3081 6290 2953 6290 3342 6290 3146 6291 3143 6291 3052 6291 3146 6292 3052 6292 3149 6292 3148 6293 3143 6293 3146 6293 3149 6294 3148 6294 3146 6294 2952 6295 2953 6295 3081 6295 3144 6296 2991 6296 3069 6296 3069 6297 3147 6297 3144 6297 3155 6298 3148 6298 3149 6298 2957 6299 2932 6299 3153 6299 3151 6300 3147 6300 3069 6300 3151 6301 2957 6301 3147 6301 3155 6302 3149 6302 3046 6302 3154 6303 3150 6303 3148 6303 3162 6304 2952 6304 3081 6304 3162 6305 3163 6305 2952 6305 3154 6306 3148 6306 3155 6306 3151 6307 3069 6307 3064 6307 3152 6308 2932 6308 3153 6308 3064 6309 3152 6309 3153 6309 3064 6310 2957 6310 3151 6310 3064 6311 3153 6311 2957 6311 3046 6312 3154 6312 3155 6312 3156 6313 2932 6313 3152 6313 3157 6314 3150 6314 3154 6314 3157 6315 3154 6315 3046 6315 3157 6316 3046 6316 3041 6316 3158 6317 2931 6317 3150 6317 3152 6318 3064 6318 3063 6318 3063 6319 3156 6319 3152 6319 3063 6320 3159 6320 3156 6320 3161 6321 2931 6321 3158 6321 3156 6322 2836 6322 3160 6322 3158 6323 3150 6323 3157 6323 3160 6324 3156 6324 3159 6324 3082 6325 3162 6325 3081 6325 3158 6326 3157 6326 3041 6326 3159 6327 3063 6327 3059 6327 3161 6328 3158 6328 3041 6328 3161 6329 3041 6329 3037 6329 3165 6330 2836 6330 3160 6330 3082 6331 3163 6331 3162 6331 3082 6332 3169 6332 3163 6332 3164 6333 2931 6333 3161 6333 3059 6334 3160 6334 3159 6334 3170 6335 3172 6335 3163 6335 3170 6336 3163 6336 3169 6336 3037 6337 3164 6337 3161 6337 3165 6338 3160 6338 3059 6338 3037 6339 3171 6339 3164 6339 3165 6340 3059 6340 3166 6340 3166 6341 3054 6341 2919 6341 3166 6342 2836 6342 3165 6342 3166 6343 2919 6343 2836 6343 3171 6344 3037 6344 3167 6344 3312 6345 3169 6345 3082 6345 3168 6346 3164 6346 3171 6346 3312 6347 3170 6347 3169 6347 3173 6348 3172 6348 3170 6348 3167 6349 3168 6349 3171 6349 3173 6350 3170 6350 3312 6350 3173 6351 3312 6351 3174 6351 2958 6352 3172 6352 3174 6352 3174 6353 3172 6353 3173 6353 3083 6354 3168 6354 3167 6354 3176 6355 2958 6355 3174 6355 3084 6356 3168 6356 3083 6356 3083 6357 3167 6357 3284 6357 3175 6358 2958 6358 3176 6358 3175 6359 3086 6359 2958 6359 3554 6360 3357 6360 2990 6360 2961 6361 3178 6361 3359 6361 3178 6362 3362 6362 3359 6362 3179 6363 2961 6363 3180 6363 2961 6364 3359 6364 3180 6364 3181 6365 3179 6365 3358 6365 3179 6366 3180 6366 3358 6366 3182 6367 3181 6367 3364 6367 3181 6368 3358 6368 3364 6368 3183 6369 3182 6369 3364 6369 3183 6370 3364 6370 3563 6370 3226 6371 3567 6371 3608 6371 3184 6372 3370 6372 3567 6372 3370 6373 3184 6373 3185 6373 3185 6374 3184 6374 3570 6374 3184 6375 2968 6375 3571 6375 3570 6376 3184 6376 3571 6376 2968 6377 2969 6377 3572 6377 2969 6378 3186 6378 3371 6378 3186 6379 3188 6379 3371 6379 3187 6380 3186 6380 3574 6380 3186 6381 3573 6381 3574 6381 3573 6382 3190 6382 3189 6382 3191 6383 3193 6383 3192 6383 3190 6384 3191 6384 3192 6384 3193 6385 3372 6385 3192 6385 3191 6386 2972 6386 3193 6386 2972 6387 3196 6387 2966 6387 3196 6388 3197 6388 3195 6388 3582 6389 2966 6389 3196 6389 2965 6390 3582 6390 3196 6390 3198 6391 3197 6391 3195 6391 3198 6392 3195 6392 3375 6392 2970 6393 3962 6393 3199 6393 3198 6394 3199 6394 3585 6394 2970 6395 3201 6395 3376 6395 3201 6396 3202 6396 3586 6396 3202 6397 3200 6397 3586 6397 3201 6398 3586 6398 3376 6398 3200 6399 3202 6399 3203 6399 3200 6400 3203 6400 3587 6400 3587 6401 3203 6401 3204 6401 3203 6402 3202 6402 3204 6402 3590 6403 3204 6403 2971 6403 3205 6404 3958 6404 2971 6404 2971 6405 3206 6405 3205 6405 3207 6406 3591 6406 3206 6406 3206 6407 2971 6407 3207 6407 3208 6408 3591 6408 3207 6408 3208 6409 3592 6409 3593 6409 3208 6410 3207 6410 3593 6410 3377 6411 3593 6411 3209 6411 3209 6412 3211 6412 3594 6412 3377 6413 3209 6413 3594 6413 3595 6414 3211 6414 3210 6414 3597 6415 3595 6415 3210 6415 3597 6416 3210 6416 3213 6416 3212 6417 3213 6417 3210 6417 3217 6418 453 6418 3216 6418 3212 6419 3214 6419 3215 6419 452 6420 3217 6420 3216 6420 2967 6421 3219 6421 451 6421 3220 6422 3219 6422 3221 6422 3219 6423 2967 6423 3221 6423 3220 6424 3221 6424 3380 6424 3222 6425 3380 6425 3221 6425 3954 6426 3222 6426 3603 6426 3222 6427 3954 6427 3380 6427 3222 6428 3224 6428 3381 6428 3224 6429 3223 6429 3222 6429 3224 6430 3223 6430 3607 6430 3605 6431 3223 6431 3382 6431 3604 6432 3225 6432 3382 6432 3604 6433 3225 6433 3947 6433 3392 6434 3385 6434 2984 6434 2976 6435 2984 6435 3385 6435 2976 6436 3385 6436 3383 6436 2977 6437 2976 6437 3390 6437 2976 6438 3383 6438 3390 6438 2977 6439 3390 6439 3386 6439 3227 6440 2977 6440 3386 6440 3227 6441 3386 6441 3384 6441 2979 6442 3227 6442 3384 6442 2981 6443 2979 6443 3384 6443 2983 6444 3228 6444 3392 6444 3395 6445 3228 6445 3229 6445 3395 6446 3229 6446 3230 6446 3395 2093 3230 2093 2973 2093 3395 2093 2973 2093 3232 2093 671 6447 2978 6447 2982 6447 3228 6448 2975 6448 3229 6448 3398 6449 3395 6449 2980 6449 3398 6450 2980 6450 2979 6450 3398 6451 2979 6451 2981 6451 3233 6452 3234 6452 3235 6452 362 6453 3233 6453 3235 6453 3235 6454 3234 6454 3236 6454 3234 6455 3237 6455 3236 6455 2989 6456 3238 6456 3233 6456 359 2093 3237 2093 669 2093 359 2093 669 2093 3239 2093 3238 6457 2987 6457 3233 6457 3233 6458 2987 6458 3234 6458 669 2782 3237 2782 670 2782 670 6459 3237 6459 2985 6459 359 6460 3239 6460 668 6460 358 6461 359 6461 668 6461 2985 6462 3237 6462 3240 6462 358 6463 668 6463 667 6463 3240 6464 3237 6464 3241 6464 667 6465 2989 6465 358 6465 358 6466 2989 6466 3233 6466 2987 6467 3241 6467 3234 6467 3241 6468 3237 6468 3234 6468 3242 6469 359 6469 358 6469 406 6470 2990 6470 417 6470 3243 6471 2990 6471 406 6471 3244 6472 3243 6472 406 6472 3311 6473 3245 6473 3249 6473 3248 6474 3311 6474 3249 6474 3249 6475 3245 6475 2997 6475 3427 6476 3434 6476 2994 6476 3427 6477 2994 6477 2998 6477 3457 6478 3247 6478 3246 6478 3248 6479 3249 6479 2996 6479 2994 6480 2995 6480 2998 6480 3426 6481 3427 6481 2998 6481 3457 6482 3246 6482 3250 6482 3249 6483 2997 6483 2996 6483 3426 6484 2998 6484 3001 6484 3451 6485 3457 6485 3250 6485 3248 6486 2996 6486 327 6486 327 6487 2996 6487 3252 6487 3251 6488 3426 6488 3001 6488 326 6489 327 6489 3252 6489 3256 6490 3451 6490 3254 6490 3451 6491 3250 6491 3254 6491 3250 6492 2999 6492 3254 6492 326 6493 3252 6493 3253 6493 3421 6494 3251 6494 3255 6494 2999 6495 3003 6495 3254 6495 3258 6496 3256 6496 3254 6496 3260 6497 3421 6497 3255 6497 3258 6498 3254 6498 3259 6498 3253 6499 3252 6499 3004 6499 3262 6500 3253 6500 3004 6500 326 6501 3253 6501 3262 6501 3003 6502 3005 6502 3254 6502 3257 6503 326 6503 324 6503 3261 6504 3258 6504 3259 6504 326 6505 3262 6505 324 6505 3255 6506 3263 6506 3260 6506 3259 6507 3254 6507 3005 6507 3262 6508 3004 6508 3008 6508 3261 6509 3259 6509 3006 6509 3005 6510 3006 6510 3259 6510 3266 6511 3261 6511 3006 6511 324 6512 3262 6512 3267 6512 3264 6513 3260 6513 3263 6513 3262 6514 3008 6514 3267 6514 3008 6515 3012 6515 3267 6515 3265 6516 324 6516 3267 6516 3263 6517 3011 6517 3264 6517 3266 6518 3006 6518 3268 6518 3006 6519 3113 6519 3268 6519 3011 6520 3013 6520 3264 6520 3267 6521 3012 6521 3010 6521 3270 6522 3267 6522 3010 6522 301 6523 3265 6523 3270 6523 3265 6524 3267 6524 3270 6524 3113 6525 3016 6525 3268 6525 3268 6526 3016 6526 3271 6526 3270 6527 3010 6527 3269 6527 3272 6528 301 6528 3269 6528 3413 6529 3244 6529 307 6529 301 6530 3270 6530 3269 6530 3016 6531 3483 6531 3271 6531 3269 6532 3015 6532 3272 6532 307 6533 3022 6533 3273 6533 3478 6534 3483 6534 3277 6534 3274 6535 3272 6535 3279 6535 3022 6536 3275 6536 3273 6536 3272 6537 3015 6537 3279 6537 3276 6538 3274 6538 3279 6538 3475 6539 3478 6539 3280 6539 3478 6540 3277 6540 3280 6540 3281 6541 3276 6541 3279 6541 3278 6542 3275 6542 3286 6542 3286 6543 3275 6543 3282 6543 3466 6544 3475 6544 3280 6544 3281 6545 3279 6545 3028 6545 3466 6546 3280 6546 3030 6546 3279 6547 3027 6547 3028 6547 3283 6548 3278 6548 3286 6548 3285 6549 3281 6549 3028 6549 3280 6550 3284 6550 3030 6550 3288 6551 3466 6551 3030 6551 3285 6552 3028 6552 3287 6552 3028 6553 3031 6553 3287 6553 322 6554 3285 6554 3287 6554 3283 6555 3286 6555 3038 6555 3030 6556 3033 6556 3288 6556 3290 6557 3288 6557 3039 6557 3287 6558 3031 6558 3032 6558 3291 6559 322 6559 3032 6559 322 6560 3287 6560 3032 6560 3288 6561 3033 6561 3039 6561 3289 6562 3038 6562 3043 6562 3291 6563 3032 6563 3044 6563 3294 6564 3291 6564 3044 6564 300 6565 3289 6565 3293 6565 3289 6566 3043 6566 3293 6566 3456 6567 3039 6567 3292 6567 3294 6568 3044 6568 3295 6568 3298 6569 3294 6569 3295 6569 300 6570 3293 6570 3296 6570 3449 6571 3456 6571 3297 6571 3456 6572 3292 6572 3297 6572 3298 6573 3295 6573 3047 6573 309 6574 3298 6574 3047 6574 309 6575 3047 6575 3301 6575 3449 6576 3297 6576 3299 6576 3300 6577 3449 6577 3299 6577 3305 6578 309 6578 3301 6578 355 6579 357 6579 3303 6579 3300 6580 3299 6580 3302 6580 308 6581 3305 6581 3306 6581 3303 6582 3055 6582 3304 6582 355 6583 3303 6583 3304 6583 3305 6584 3301 6584 3306 6584 3443 6585 3302 6585 3061 6585 3055 6586 3308 6586 3304 6586 3054 6587 3306 6587 3301 6587 308 6588 3306 6588 3486 6588 3302 6589 3060 6589 3061 6589 3309 6590 3443 6590 3061 6590 347 6591 3304 6591 344 6591 344 6592 3304 6592 3308 6592 3482 6593 3486 6593 3307 6593 3486 6594 3306 6594 3307 6594 3061 6595 3058 6595 3309 6595 3308 6596 3067 6596 344 6596 3439 6597 3309 6597 3310 6597 3482 6598 3307 6598 3476 6598 3067 6599 3068 6599 344 6599 3309 6600 3058 6600 3310 6600 3435 6601 3439 6601 3310 6601 3311 6602 3068 6602 3245 6602 3435 6603 3310 6603 2994 6603 3434 6604 3435 6604 2994 6604 3312 6605 3176 6605 3174 6605 3312 6606 3013 6606 3176 6606 3493 6607 3013 6607 3312 6607 3312 6608 3082 6608 3492 6608 3319 6609 3502 6609 3504 6609 3324 6610 3319 6610 3504 6610 3316 2093 3323 2093 3314 2093 662 6611 3313 6611 3314 6611 3315 2782 662 2782 3314 2782 3502 6612 3315 6612 3314 6612 3313 6613 3316 6613 3314 6613 3502 6614 3326 6614 3317 6614 3317 6615 3315 6615 3502 6615 3326 6616 3502 6616 3319 6616 3318 6617 3321 6617 3325 6617 3500 6618 3319 6618 3324 6618 3509 6619 3318 6619 3325 6619 3314 6620 3323 6620 3320 6620 3320 6621 3323 6621 3322 6621 3321 6622 664 6622 3325 6622 664 6623 3326 6623 3325 6623 3617 6624 3509 6624 3414 6624 3323 6625 3318 6625 3322 6625 3509 6626 3325 6626 3414 6626 3325 6627 3326 6627 3319 6627 3414 6628 3325 6628 3500 6628 3322 6629 3318 6629 3509 6629 3500 6630 3325 6630 3319 6630 3507 6631 3322 6631 3509 6631 444 6632 445 6632 3516 6632 443 6633 444 6633 3511 6633 444 6634 3516 6634 3511 6634 446 6635 443 6635 3511 6635 446 6636 3511 6636 3510 6636 449 6637 446 6637 3515 6637 446 6638 3510 6638 3515 6638 449 6639 3515 6639 3513 6639 3328 6640 447 6640 3327 6640 447 6641 3329 6641 3521 6641 3523 6642 3521 6642 3074 6642 3523 6643 3074 6643 3072 6643 442 6644 3331 6644 661 6644 3076 6645 3521 6645 3329 6645 3076 6646 3329 6646 3330 6646 3523 6647 3072 6647 3331 6647 3332 6648 3521 6648 3076 6648 3074 6649 3521 6649 3332 6649 661 6650 448 6650 442 6650 3524 6651 3523 6651 442 6651 3530 6652 3529 6652 3498 6652 3498 6653 3528 6653 3526 6653 3527 6654 3531 6654 3077 6654 656 6655 3530 6655 3495 6655 656 6656 3335 6656 3334 6656 3336 6657 3530 6657 656 6657 656 2093 3495 2093 3335 2093 3337 6658 3530 6658 3336 6658 295 2093 3333 2093 3338 2093 295 2093 3338 2093 3335 2093 295 2093 3335 2093 3495 2093 3534 6659 3492 6659 3082 6659 296 6660 295 6660 3495 6660 296 6661 3495 6661 3340 6661 296 6662 3340 6662 3341 6662 3533 6663 3341 6663 3537 6663 3345 6664 3081 6664 3342 6664 3345 6665 3082 6665 3081 6665 3345 6666 3342 6666 3343 6666 3344 6667 3345 6667 3343 6667 3344 6668 3540 6668 3345 6668 3344 6669 3343 6669 3346 6669 3347 6670 3344 6670 3346 6670 3347 6671 9 6671 3344 6671 3348 6672 3543 6672 9 6672 3348 6673 9 6673 3347 6673 3348 6674 3347 6674 3349 6674 3348 6675 3542 6675 3543 6675 3350 6676 3349 6676 3127 6676 3351 6677 3349 6677 3350 6677 3351 6678 3348 6678 3349 6678 3351 6679 3542 6679 3348 6679 3351 6680 3350 6680 3352 6680 3353 6681 3351 6681 3352 6681 3353 6682 3546 6682 3351 6682 3353 6683 3547 6683 3546 6683 440 6684 3547 6684 3353 6684 440 6685 3548 6685 3547 6685 3354 6686 3550 6686 3548 6686 3354 6687 3548 6687 440 6687 3355 6688 3552 6688 3550 6688 3355 6689 3550 6689 3354 6689 3356 6690 3177 6690 2990 6690 3356 6691 3552 6691 3355 6691 3356 6692 2990 6692 3357 6692 3356 6693 3357 6693 3552 6693 3360 6694 3359 6694 3362 6694 3363 6695 3365 6695 3555 6695 3363 6696 3361 6696 3365 6696 3362 2093 3555 2093 3360 2093 3362 6697 655 6697 654 6697 3563 6698 3365 6698 3361 6698 3362 2093 654 2093 3555 2093 654 6699 3363 6699 3555 6699 3563 6700 3364 6700 3560 6700 3556 6701 3555 6701 3365 6701 3556 6702 3365 6702 3366 6702 3556 6703 3366 6703 3367 6703 3933 6704 3367 6704 3938 6704 3369 6705 3368 6705 3410 6705 3369 6706 3410 6706 3565 6706 3192 6707 3576 6707 3190 6707 3372 6708 3580 6708 3192 6708 3580 6709 3372 6709 3194 6709 3194 6710 3943 6710 3373 6710 2966 6711 3582 6711 3943 6711 2972 6712 2966 6712 3943 6712 3374 6713 3373 6713 3943 6713 2965 6714 3583 6714 3582 6714 3375 6715 3583 6715 2965 6715 3198 6716 3583 6716 3375 6716 2971 6717 3958 6717 3590 6717 3601 6718 3378 6718 3218 6718 3378 6719 3598 6719 3218 6719 3601 6720 3218 6720 3217 6720 3220 6721 3600 6721 3601 6721 3235 6722 3610 6722 362 6722 362 6723 3614 6723 361 6723 361 6724 3614 6724 3407 6724 360 6725 361 6725 3407 6725 360 6726 3407 6726 3406 6726 3389 6727 3390 6727 3383 6727 3388 6728 2981 6728 3384 6728 260 6729 3389 6729 3383 6729 3388 6730 3397 6730 2981 6730 3388 6731 3384 6731 3386 6731 260 6732 41 6732 3389 6732 260 6733 3383 6733 3385 6733 3389 6734 3388 6734 3386 6734 3387 6735 3388 6735 3389 6735 3392 6736 260 6736 3385 6736 3390 6737 3389 6737 3386 6737 260 6738 3392 6738 3391 6738 3389 6739 41 6739 3387 6739 242 6740 3391 6740 255 6740 3228 6741 3393 6741 3396 6741 3396 6742 255 6742 3391 6742 3228 6743 3391 6743 3392 6743 3228 6744 3396 6744 3391 6744 3393 6745 3228 6745 3395 6745 3399 6746 3393 6746 3395 6746 3396 6747 3393 6747 3399 6747 3394 6748 3396 6748 3399 6748 3394 6749 3397 6749 63 6749 3398 6750 2981 6750 3397 6750 3398 6751 3397 6751 3394 6751 3398 6752 3394 6752 3399 6752 3398 6753 3399 6753 3395 6753 245 6754 3611 6754 3610 6754 245 6755 3609 6755 3611 6755 3400 6756 3235 6756 3236 6756 245 6757 236 6757 235 6757 236 6758 3401 6758 214 6758 236 6759 3236 6759 3401 6759 3400 6760 3236 6760 236 6760 236 6761 245 6761 3610 6761 3400 6762 236 6762 3610 6762 3400 6763 3610 6763 3235 6763 3402 6764 3236 6764 3237 6764 3401 6765 3402 6765 202 6765 3401 6766 202 6766 214 6766 3236 6767 3402 6767 3401 6767 3402 6768 3404 6768 202 6768 3403 6769 3402 6769 3237 6769 3403 6770 3237 6770 359 6770 3404 6771 3402 6771 3403 6771 3406 6772 3407 6772 3405 6772 273 6773 3406 6773 3405 6773 3406 6774 3242 6774 360 6774 273 6775 3242 6775 3406 6775 3403 6776 273 6776 3404 6776 3403 6777 3242 6777 273 6777 3403 6778 359 6778 3242 6778 101 6779 56 6779 3407 6779 3407 6780 3614 6780 3613 6780 56 6781 3405 6781 3407 6781 101 6782 3407 6782 3613 6782 101 6783 3613 6783 3612 6783 56 6784 27 6784 3405 6784 3565 6785 3410 6785 3408 6785 3565 6786 3408 6786 142 6786 142 6787 3409 6787 3565 6787 3565 6788 3566 6788 3369 6788 3360 6789 3410 6789 3368 6789 3410 6790 3360 6790 131 6790 3410 6791 131 6791 3408 6791 3412 6792 3411 6792 3554 6792 3554 6793 3243 6793 3412 6793 3411 6794 3412 6794 131 6794 3411 6795 131 6795 3360 6795 3411 6796 3360 6796 3555 6796 3413 6797 3243 6797 3244 6797 3324 6798 3503 6798 3615 6798 3324 6799 3615 6799 3500 6799 3500 6800 3615 6800 3414 6800 3264 6801 3013 6801 3415 6801 3420 6802 3261 6802 3266 6802 3624 6803 3481 6803 3419 6803 3416 6804 3810 6804 3817 6804 3482 6805 3481 6805 3624 6805 3417 6806 3416 6806 3817 6806 3424 6807 3430 6807 3486 6807 3424 6808 3486 6808 3482 6808 3429 6809 3482 6809 3624 6809 3419 6810 3418 6810 3624 6810 3264 6811 3415 6811 3687 6811 3260 6812 3264 6812 3687 6812 3260 6813 3687 6813 3422 6813 3424 6814 3482 6814 3429 6814 3433 6815 3258 6815 3261 6815 3422 6816 3421 6816 3260 6816 3770 6817 3251 6817 3421 6817 3770 6818 3421 6818 3422 6818 3423 6819 3417 6819 3817 6819 3428 6820 3426 6820 3251 6820 3428 6821 3251 6821 3770 6821 3429 6822 3624 6822 3424 6822 3428 6823 3770 6823 3804 6823 3428 6824 3804 6824 3425 6824 3427 6825 3426 6825 3428 6825 3425 6826 3427 6826 3428 6826 3432 6827 3427 6827 3425 6827 3636 6828 3420 6828 3266 6828 3430 6829 3424 6829 315 6829 3432 6830 3425 6830 3881 6830 3431 6831 3434 6831 3427 6831 3431 6832 3427 6832 3432 6832 315 6833 308 6833 3430 6833 3436 6834 3434 6834 3431 6834 3431 6835 3432 6835 3881 6835 3438 6836 3434 6836 3436 6836 3438 6837 3435 6837 3434 6837 3436 6838 3431 6838 3881 6838 3423 6839 3447 6839 3268 6839 3436 6840 3881 6840 3621 6840 3423 6841 3268 6841 3271 6841 3621 6842 3438 6842 3436 6842 3437 6843 3256 6843 3258 6843 3440 6844 3435 6844 3438 6844 3440 6845 3438 6845 3621 6845 3266 6846 3268 6846 3447 6846 3439 6847 3435 6847 3440 6847 3440 6848 3621 6848 3646 6848 3261 6849 3420 6849 3636 6849 3309 6850 3439 6850 3441 6850 3439 6851 3440 6851 3646 6851 3441 6852 3439 6852 3646 6852 3441 6853 3646 6853 3716 6853 3636 6854 3266 6854 3447 6854 3716 6855 3309 6855 3441 6855 3716 6856 3445 6856 3309 6856 3446 6857 3443 6857 3309 6857 3446 6858 3309 6858 3445 6858 3468 6859 3256 6859 3437 6859 3445 6860 3716 6860 3760 6860 3442 6861 3451 6861 3256 6861 3760 6862 3443 6862 3446 6862 3443 6863 3302 6863 3444 6863 3442 6864 3256 6864 3468 6864 3452 6865 3302 6865 3444 6865 3446 6866 3445 6866 3760 6866 3444 6867 3443 6867 3760 6867 3302 6868 3300 6868 3448 6868 3448 6869 3302 6869 3452 6869 3465 6870 3258 6870 3433 6870 3437 6871 3258 6871 3465 6871 3838 6872 3448 6872 3452 6872 3300 6873 3448 6873 3838 6873 3454 6874 3261 6874 3636 6874 3444 6875 3760 6875 3771 6875 3452 6876 3771 6876 3838 6876 3452 6877 3444 6877 3771 6877 3433 6878 3261 6878 3454 6878 3848 6879 3449 6879 3300 6879 3450 6880 3457 6880 3451 6880 3455 6881 3300 6881 3838 6881 3455 6882 3838 6882 3848 6882 3848 6883 3300 6883 3455 6883 3449 6884 3456 6884 3453 6884 3458 6885 3449 6885 3848 6885 3453 6886 3449 6886 3458 6886 3459 6887 3039 6887 3456 6887 3458 6888 3848 6888 3462 6888 3461 6889 3247 6889 3457 6889 3464 6890 3456 6890 3453 6890 3469 6891 3039 6891 3459 6891 3459 6892 3456 6892 3464 6892 3480 6893 3451 6893 3442 6893 3460 6894 3039 6894 3469 6894 3460 6895 3290 6895 3039 6895 3822 6896 3461 6896 3457 6896 3453 6897 3458 6897 3462 6897 3463 6898 3459 6898 3464 6898 3469 6899 3459 6899 3463 6899 3465 6900 3433 6900 3454 6900 3465 6901 3454 6901 3711 6901 3464 6902 3462 6902 3463 6902 3464 6903 3453 6903 3462 6903 3450 6904 3451 6904 3480 6904 3488 6905 3247 6905 3461 6905 3437 6906 3465 6906 3711 6906 3634 6907 3460 6907 3469 6907 3290 6908 3460 6908 3634 6908 3488 6909 3461 6909 3822 6909 3467 6910 3473 6910 3247 6910 3467 6911 3247 6911 3488 6911 3469 6912 3463 6912 3634 6912 3470 6913 3288 6913 3290 6913 3474 6914 3290 6914 3634 6914 3727 6915 3466 6915 3288 6915 3491 6916 3473 6916 3467 6916 3468 6917 3437 6917 3711 6917 3468 6918 3711 6918 3744 6918 3479 6919 3288 6919 3470 6919 3474 6920 3634 6920 3470 6920 3840 6921 3467 6921 3488 6921 3470 6922 3290 6922 3474 6922 3471 6923 3475 6923 3466 6923 3442 6924 3468 6924 3744 6924 3472 6925 3476 6925 3473 6925 3727 6926 3288 6926 3479 6926 3491 6927 3467 6927 3840 6927 3472 6928 3473 6928 3491 6928 3419 6929 3476 6929 3472 6929 3479 6930 3470 6930 3727 6930 3484 6931 3457 6931 3450 6931 3480 6932 3442 6932 3744 6932 3480 6933 3744 6933 3477 6933 3485 6934 3466 6934 3727 6934 3822 6935 3457 6935 3484 6935 3810 6936 3483 6936 3478 6936 3471 6937 3466 6937 3485 6937 3481 6938 3482 6938 3476 6938 3450 6939 3480 6939 3477 6939 3481 6940 3476 6940 3419 6940 3485 6941 3727 6941 3717 6941 3416 6942 3483 6942 3810 6942 3417 6943 3271 6943 3483 6943 3417 6944 3483 6944 3416 6944 3484 6945 3477 6945 3822 6945 3484 6946 3450 6946 3477 6946 3487 6947 3475 6947 3471 6947 3471 6948 3485 6948 3717 6948 3490 6949 3478 6949 3475 6949 3489 6950 3475 6950 3487 6950 3810 6951 3478 6951 3490 6951 3487 6952 3471 6952 3717 6952 3487 6953 3717 6953 3489 6953 3490 6954 3475 6954 3489 6954 3430 6955 308 6955 3486 6955 3423 6956 3271 6956 3417 6956 3488 6957 3822 6957 3840 6957 3490 6958 3489 6958 3810 6958 3418 6959 3472 6959 3491 6959 3491 6960 3840 6960 3418 6960 3419 6961 3472 6961 3418 6961 3312 6962 3492 6962 3493 6962 3499 6963 3494 6963 3530 6963 3496 6964 3494 6964 3499 6964 3496 6965 3534 6965 3494 6965 3494 6966 3495 6966 3530 6966 3496 6967 3492 6967 3534 6967 3499 6968 3633 6968 3496 6968 3497 6969 3525 6969 3718 6969 3701 6970 3718 6970 3497 6970 3497 6971 3526 6971 3525 6971 3530 6972 3498 6972 3499 6972 3497 6973 3701 6973 3639 6973 3499 6974 3497 6974 3639 6974 3498 6975 3526 6975 3497 6975 3498 6976 3497 6976 3499 6976 3499 6977 3639 6977 3633 6977 3503 6978 3846 6978 3615 6978 3501 6979 3615 6979 3846 6979 3846 6980 3503 6980 3841 6980 3502 6981 3505 6981 3855 6981 3504 6982 3502 6982 3855 6982 3504 6983 3855 6983 3841 6983 3504 6984 3841 6984 3503 6984 3504 6985 3503 6985 3324 6985 3506 6986 3502 6986 3314 6986 3506 6987 3505 6987 3502 6987 3864 6988 3505 6988 3506 6988 3506 6989 3314 6989 3320 6989 3506 6990 3666 6990 3622 6990 3506 6991 3507 6991 3666 6991 3320 6992 3507 6992 3506 6992 3320 6993 3322 6993 3507 6993 3507 6994 3508 6994 3666 6994 3507 6995 3617 6995 3508 6995 3507 6996 3509 6996 3617 6996 3666 6997 3508 6997 3656 6997 3656 6998 3508 6998 3858 6998 3519 6999 3510 6999 3511 6999 3794 7000 3808 7000 3518 7000 3794 7001 3328 7001 3785 7001 3794 7002 3518 7002 3328 7002 3851 7003 3520 7003 3519 7003 3512 7004 3519 7004 3511 7004 3513 7005 3524 7005 442 7005 3514 7006 3524 7006 3513 7006 3517 7007 3514 7007 3513 7007 3512 7008 3851 7008 3519 7008 3517 7009 3513 7009 3515 7009 3520 7010 3514 7010 3517 7010 3512 7011 3511 7011 3516 7011 3518 7012 3512 7012 3516 7012 3518 7013 3808 7013 3512 7013 3510 7014 3517 7014 3515 7014 3519 7015 3517 7015 3510 7015 3328 7016 3516 7016 445 7016 3328 7017 445 7017 447 7017 3520 7018 3517 7018 3519 7018 3328 7019 3518 7019 3516 7019 3327 7020 3521 7020 3790 7020 3327 7021 3790 7021 3785 7021 3327 7022 447 7022 3521 7022 3328 7023 3327 7023 3785 7023 3522 7024 3790 7024 3521 7024 3522 7025 3521 7025 3523 7025 3523 7026 3524 7026 3842 7026 3522 7027 3523 7027 3842 7027 3524 7028 3514 7028 3842 7028 3526 7029 3898 7029 3525 7029 3526 7030 3528 7030 3895 7030 296 7031 3896 7031 3527 7031 3527 7032 3528 7032 3529 7032 3339 7033 3527 7033 3529 7033 3077 7034 3333 7034 296 7034 3528 7035 3498 7035 3529 7035 3337 7036 3339 7036 3529 7036 3530 7037 3337 7037 3529 7037 3077 7038 296 7038 3527 7038 3341 7039 3897 7039 3896 7039 3339 7040 3531 7040 3527 7040 296 7041 3341 7041 3896 7041 3527 7042 3895 7042 3528 7042 3333 7043 295 7043 296 7043 3535 7044 3902 7044 3536 7044 3538 7045 3340 7045 3494 7045 3538 7046 3341 7046 3340 7046 3536 7047 3538 7047 1 7047 3538 7048 3537 7048 3341 7048 3537 7049 3538 7049 3536 7049 3536 7050 1 7050 3535 7050 3537 7051 3536 7051 3533 7051 3538 7052 3494 7052 3534 7052 3494 7053 3340 7053 3495 7053 3534 7054 3082 7054 3345 7054 3539 7055 3534 7055 3345 7055 3540 7056 3539 7056 3345 7056 3541 7057 3540 7057 3344 7057 3541 7058 3344 7058 9 7058 3545 7059 3542 7059 3544 7059 3544 7060 3542 7060 3351 7060 3544 7061 3351 7061 3546 7061 3916 7062 3544 7062 3546 7062 3918 7063 3916 7063 3547 7063 3916 7064 3546 7064 3547 7064 3547 7065 3548 7065 3918 7065 3549 7066 3918 7066 3548 7066 3548 7067 3550 7067 3549 7067 3551 7068 3549 7068 3550 7068 3550 7069 3552 7069 3551 7069 3553 7070 3551 7070 3552 7070 3553 7071 3552 7071 3357 7071 3556 7072 3367 7072 3557 7072 3411 7073 3557 7073 3554 7073 3557 7074 3933 7074 3928 7074 3556 7075 3557 7075 3411 7075 3557 7076 3928 7076 3912 7076 3553 7077 3912 7077 3554 7077 3411 7078 3555 7078 3556 7078 3367 7079 3933 7079 3557 7079 3357 7080 3553 7080 3554 7080 3557 7081 3912 7081 3554 7081 3928 7082 3933 7082 3905 7082 3369 7083 3566 7083 3561 7083 3368 7084 3369 7084 3561 7084 3561 7085 3566 7085 3935 7085 3364 7086 3358 7086 3560 7086 3359 7087 3360 7087 3368 7087 3562 7088 3938 7088 3366 7088 3180 7089 3359 7089 3368 7089 3561 7090 3935 7090 3560 7090 3368 7091 3561 7091 3560 7091 3938 7092 3367 7092 3366 7092 3560 7093 3562 7093 3366 7093 3358 7094 3180 7094 3560 7094 3180 7095 3368 7095 3560 7095 3560 7096 3366 7096 3365 7096 3563 7097 3560 7097 3365 7097 3939 7098 3938 7098 3562 7098 3564 7099 3565 7099 3409 7099 3566 7100 3565 7100 3564 7100 3568 7101 3608 7101 3370 7101 3568 7102 3569 7102 3960 7102 3568 7103 3370 7103 3569 7103 3608 7104 3567 7104 3370 7104 3569 7105 3370 7105 3185 7105 3569 7106 3185 7106 3570 7106 3960 7107 3569 7107 3570 7107 3960 7108 3570 7108 3955 7108 3571 7109 3955 7109 3570 7109 3955 7110 2968 7110 3572 7110 3955 7111 3571 7111 2968 7111 3371 7112 3572 7112 2969 7112 3188 7113 3956 7113 3371 7113 3574 7114 3956 7114 3187 7114 3956 7115 3188 7115 3186 7115 3187 7116 3956 7116 3186 7116 3577 7117 3190 7117 3576 7117 3577 7118 3575 7118 3190 7118 3189 7119 3190 7119 3577 7119 3189 7120 3577 7120 3578 7120 3189 7121 3574 7121 3573 7121 3579 7122 3580 7122 3581 7122 3579 7123 3192 7123 3580 7123 3192 7124 3577 7124 3576 7124 3192 7125 3578 7125 3577 7125 3192 7126 3189 7126 3578 7126 3579 7127 3189 7127 3192 7127 3580 7128 3194 7128 3373 7128 3581 7129 3373 7129 3374 7129 3581 7130 3580 7130 3373 7130 3579 7131 3374 7131 3943 7131 3579 7132 3581 7132 3374 7132 3943 7133 3582 7133 3961 7133 3582 7134 3583 7134 3584 7134 3961 7135 3582 7135 3584 7135 3198 7136 3584 7136 3583 7136 3585 7137 3584 7137 3198 7137 3585 7138 3199 7138 3962 7138 3962 7139 2970 7139 3376 7139 3586 7140 3200 7140 3588 7140 3586 7141 3588 7141 3946 7141 3588 7142 3200 7142 3587 7142 3946 7143 3588 7143 3589 7143 3588 7144 3587 7144 3589 7144 3946 7145 3589 7145 3945 7145 3945 7146 3590 7146 3958 7146 3204 7147 3589 7147 3587 7147 3590 7148 3589 7148 3204 7148 3945 7149 3589 7149 3590 7149 3205 7150 3206 7150 3952 7150 3591 7151 3952 7151 3206 7151 3208 7152 3592 7152 3377 7152 3208 7153 3952 7153 3591 7153 3377 7154 3592 7154 3593 7154 3595 7155 3594 7155 3211 7155 3597 7156 3213 7156 3596 7156 3596 7157 3213 7157 3212 7157 3215 7158 3596 7158 3212 7158 3214 7159 3949 7159 3215 7159 3216 7160 3949 7160 3214 7160 3216 7161 453 7161 3218 7161 3216 7162 3218 7162 3598 7162 3598 7163 3949 7163 3216 7163 3378 7164 3949 7164 3598 7164 3601 7165 3379 7165 3378 7165 3602 7166 3599 7166 3379 7166 3602 7167 3379 7167 3601 7167 3600 7168 3602 7168 3601 7168 3380 7169 3599 7169 3602 7169 3220 7170 3602 7170 3600 7170 3380 7171 3602 7171 3220 7171 3381 7172 3603 7172 3222 7172 3606 7173 3224 7173 3607 7173 3224 7174 3606 7174 3381 7174 3604 7175 3606 7175 3605 7175 3605 7176 3382 7176 3604 7176 3606 7177 3223 7177 3605 7177 3606 7178 3607 7178 3223 7178 3226 7179 3947 7179 3225 7179 3608 7180 3947 7180 3226 7180 3614 7181 3610 7181 3611 7181 3611 7182 3609 7182 3612 7182 3614 7183 362 7183 3610 7183 3613 7184 3611 7184 3612 7184 3613 7185 3614 7185 3611 7185 3616 7186 3615 7186 3501 7186 3414 7187 3615 7187 3616 7187 3616 7188 3501 7188 3858 7188 3617 7189 3616 7189 3858 7189 3617 7190 3414 7190 3616 7190 3617 7191 3858 7191 3508 7191 3644 7192 3884 7192 3867 7192 3418 7193 3673 7193 3624 7193 3673 7194 3418 7194 3874 7194 3619 7195 3885 7195 3860 7195 3447 7196 3423 7196 3863 7196 3629 7197 3891 7197 3888 7197 3876 7198 3856 7198 3889 7198 3619 7199 3860 7199 3868 7199 3861 7200 3625 7200 3618 7200 3661 7201 3878 7201 3872 7201 3678 7202 3668 7202 3506 7202 3623 7203 3463 7203 3462 7203 3640 7204 3879 7204 3882 7204 3631 7205 3447 7205 3863 7205 3635 7206 3629 7206 3620 7206 3671 7207 3859 7207 3833 7207 3648 7208 3833 7208 3880 7208 3622 7209 3678 7209 3506 7209 3631 7210 3863 7210 3884 7210 3625 7211 3627 7211 3862 7211 3653 7212 3867 7212 3866 7212 3653 7213 3644 7213 3867 7213 3619 7214 3868 7214 3876 7214 3626 7215 3861 7215 3514 7215 3670 7216 3886 7216 3871 7216 3627 7217 3651 7217 3887 7217 3661 7218 3652 7218 3870 7218 3696 7219 3622 7219 3666 7219 3642 7220 3648 7220 3880 7220 3665 7221 3623 7221 3891 7221 3625 7222 3862 7222 3618 7222 3493 7223 3492 7223 3496 7223 3675 7224 3892 7224 3869 7224 3630 7225 3653 7225 3866 7225 3633 7226 3493 7226 3496 7226 3637 7227 3673 7227 3874 7227 3637 7228 3874 7228 3883 7228 3658 7229 3631 7229 3884 7229 3663 7230 3634 7230 3463 7230 3643 7231 3626 7231 3514 7231 3628 7232 3013 7232 3493 7232 3628 7233 3415 7233 3013 7233 3643 7234 3514 7234 3520 7234 3628 7235 3493 7235 3633 7235 3630 7236 3866 7236 3885 7236 3665 7237 3891 7237 3629 7237 3680 7238 3675 7238 3869 7238 3680 7239 3869 7239 3849 7239 3668 7240 3672 7240 3635 7240 3671 7241 3833 7241 3648 7241 3647 7242 3621 7242 3632 7242 3647 7243 3646 7243 3621 7243 3641 7244 3619 7244 3876 7244 3663 7245 3470 7245 3634 7245 3689 7246 3627 7246 3625 7246 3664 7247 3415 7247 3628 7247 3665 7248 3629 7248 3635 7248 3663 7249 3463 7249 3623 7249 3692 7250 3625 7250 3861 7250 3641 7251 3876 7251 3889 7251 3670 7252 3871 7252 3948 7252 3652 7253 3640 7253 3882 7253 3650 7254 3663 7254 3623 7254 3667 7255 3651 7255 3627 7255 3638 7256 3628 7256 3633 7256 3672 7257 3665 7257 3635 7257 3702 7258 3643 7258 3520 7258 3660 7259 3664 7259 3628 7259 3699 7260 3630 7260 3885 7260 3654 7261 3658 7261 3884 7261 3654 7262 3884 7262 3644 7262 3676 7263 3666 7263 3656 7263 3696 7264 3678 7264 3622 7264 3669 7265 3661 7265 3872 7265 3680 7266 3849 7266 3655 7266 3692 7267 3861 7267 3626 7267 3690 7268 3641 7268 3889 7268 3649 7269 3636 7269 3447 7269 3645 7270 3889 7270 3892 7270 3638 7271 3633 7271 3639 7271 3677 7272 3872 7272 3671 7272 3723 7273 3652 7273 3661 7273 3699 7274 3885 7274 3619 7274 3649 7275 3447 7275 3631 7275 3690 7276 3889 7276 3645 7276 3689 7277 3667 7277 3627 7277 3640 7278 3647 7278 3632 7278 3636 7279 3649 7279 3454 7279 3650 7280 3623 7280 3665 7280 3657 7281 3883 7281 3651 7281 3657 7282 3637 7282 3883 7282 3674 7283 3649 7283 3631 7283 3643 7284 3692 7284 3626 7284 3659 7285 3644 7285 3653 7285 3659 7286 3654 7286 3644 7286 3662 7287 3424 7287 3624 7287 3662 7288 3624 7288 3673 7288 3679 7289 3470 7289 3663 7289 3682 7290 3640 7290 3652 7290 3660 7291 3628 7291 3638 7291 3735 7292 3680 7292 3655 7292 3735 7293 3655 7293 3816 7293 3698 7294 3648 7294 3642 7294 3674 7295 3631 7295 3658 7295 3749 7296 3645 7296 3892 7296 3659 7297 3653 7297 3630 7297 3681 7298 3415 7298 3664 7298 3706 7299 3672 7299 3668 7299 3749 7300 3892 7300 3675 7300 3687 7301 3415 7301 3681 7301 3669 7302 3723 7302 3661 7302 3662 7303 315 7303 3424 7303 3683 7304 3659 7304 3630 7304 3684 7305 3674 7305 3658 7305 3715 7306 3651 7306 3667 7306 3715 7307 3657 7307 3651 7307 3699 7308 3619 7308 3641 7308 3669 7309 3872 7309 3677 7309 3662 7310 37 7310 315 7310 3709 7311 3646 7311 3647 7311 3706 7312 3668 7312 3678 7312 3690 7313 3699 7313 3641 7313 3677 7314 3671 7314 3648 7314 3695 7315 3650 7315 3665 7315 3708 7316 3660 7316 3638 7316 3685 7317 3664 7317 3660 7317 3662 7318 3673 7318 3637 7318 3693 7319 3662 7319 3637 7319 3700 7320 3692 7320 3643 7320 3676 7321 3696 7321 3666 7321 3754 7322 3886 7322 3670 7322 3709 7323 3647 7323 3640 7323 3951 7324 3656 7324 3858 7324 3721 7325 3677 7325 3648 7325 3684 7326 3658 7326 3654 7326 3772 7327 3749 7327 3675 7327 3772 7328 3675 7328 3680 7328 3692 7329 3689 7329 3625 7329 3715 7330 3667 7330 3689 7330 3708 7331 3638 7331 3639 7331 3679 7332 3663 7332 3650 7332 3723 7333 3682 7333 3652 7333 3703 7334 3454 7334 3649 7334 3693 7335 37 7335 3662 7335 3736 7336 3715 7336 3689 7336 3696 7337 3706 7337 3678 7337 3683 7338 3630 7338 3699 7338 3691 7339 3470 7339 3679 7339 3691 7340 3727 7340 3470 7340 3688 7341 3679 7341 3650 7341 3694 7342 3640 7342 3682 7342 3695 7343 3665 7343 3672 7343 3730 7344 3676 7344 3656 7344 3710 7345 3670 7345 3948 7345 3686 7346 3684 7346 3654 7346 3686 7347 3654 7347 3659 7347 3685 7348 3681 7348 3664 7348 3707 7349 3709 7349 3640 7349 3732 7350 3702 7350 3886 7350 3693 7351 39 7351 37 7351 3735 7352 3772 7352 3680 7352 3713 7353 3669 7353 3677 7353 3422 7354 3687 7354 3681 7354 3739 7355 3706 7355 3696 7355 3720 7356 3683 7356 3699 7356 3714 7357 3723 7357 3669 7357 3697 7358 3700 7358 3643 7358 3757 7359 3690 7359 3645 7359 3729 7360 3695 7360 3672 7360 3708 7361 3685 7361 3660 7361 3701 7362 3708 7362 3639 7362 3755 7363 3646 7363 3709 7363 3703 7364 3649 7364 3674 7364 3704 7365 3703 7365 3674 7365 3697 7366 3643 7366 3702 7366 3802 7367 3648 7367 3698 7367 3733 7368 3716 7368 3646 7368 3735 7369 3816 7369 3791 7369 3726 7370 3688 7370 3650 7370 3694 7371 3682 7371 3723 7371 3734 7372 3681 7372 3685 7372 3729 7373 3672 7373 3706 7373 3705 7374 3637 7374 3657 7374 3705 7375 3693 7375 3637 7375 3707 7376 3640 7376 3694 7376 3729 7377 3725 7377 3695 7377 3754 7378 3732 7378 3886 7378 3734 7379 3422 7379 3681 7379 3728 7380 3684 7380 3686 7380 3722 7381 3694 7381 3723 7381 3740 7382 3691 7382 3679 7382 3743 7383 3686 7383 3659 7383 3757 7384 3645 7384 3749 7384 3736 7385 3689 7385 3692 7385 3726 7386 3650 7386 3695 7386 3740 7387 3679 7387 3688 7387 3703 7388 3711 7388 3454 7388 3732 7389 3697 7389 3702 7389 3781 7390 3703 7390 3704 7390 3733 7391 3646 7391 3755 7391 3743 7392 3659 7392 3683 7392 3802 7393 3721 7393 3648 7393 3712 7394 3696 7394 3676 7394 3705 7395 40 7395 39 7395 3705 7396 39 7396 3693 7396 3704 7397 3674 7397 3684 7397 3712 7398 3739 7398 3696 7398 3724 7399 3736 7399 3692 7399 3713 7400 3714 7400 3669 7400 3754 7401 3670 7401 3710 7401 3720 7402 3699 7402 3690 7402 3755 7403 3709 7403 3707 7403 3719 7404 3657 7404 3715 7404 3725 7405 3726 7405 3695 7405 3719 7406 3705 7406 3657 7406 3753 7407 3740 7407 3688 7407 3724 7408 3692 7408 3700 7408 3753 7409 3688 7409 3726 7409 3719 7410 44 7410 40 7410 3719 7411 40 7411 3705 7411 3799 7412 3656 7412 3951 7412 3731 7413 3719 7413 3715 7413 3733 7414 3760 7414 3716 7414 3739 7415 3729 7415 3706 7415 3788 7416 3722 7416 3723 7416 3712 7417 3676 7417 3730 7417 3731 7418 3715 7418 3736 7418 3741 7419 3731 7419 3736 7419 3731 7420 44 7420 3719 7420 3780 7421 3685 7421 3708 7421 3764 7422 3713 7422 3677 7422 3745 7423 3711 7423 3703 7423 3782 7424 3708 7424 3701 7424 3728 7425 3704 7425 3684 7425 3694 7426 3755 7426 3707 7426 3697 7427 3724 7427 3700 7427 3747 7428 3770 7428 3422 7428 3751 7429 3726 7429 3725 7429 3751 7430 3753 7430 3726 7430 3769 7431 3712 7431 3730 7431 3759 7432 3714 7432 3713 7432 3743 7433 3683 7433 3720 7433 3717 7434 3727 7434 3691 7434 3752 7435 3743 7435 3720 7435 3788 7436 3723 7436 3714 7436 3784 7437 3729 7437 3739 7437 3731 7438 58 7438 44 7438 3745 7439 3703 7439 3781 7439 3765 7440 3734 7440 3685 7440 3777 7441 3720 7441 3690 7441 3827 7442 3730 7442 3656 7442 3741 7443 58 7443 3731 7443 3796 7444 3690 7444 3757 7444 3780 7445 3765 7445 3685 7445 3827 7446 3769 7446 3730 7446 3759 7447 3713 7447 3764 7447 3764 7448 3677 7448 3721 7448 3717 7449 3691 7449 3740 7449 3746 7450 3739 7450 3712 7450 3738 7451 3741 7451 3736 7451 3778 7452 3755 7452 3694 7452 3738 7453 58 7453 3741 7453 3747 7454 3422 7454 3734 7454 3762 7455 3728 7455 3686 7455 3759 7456 3788 7456 3714 7456 3738 7457 3742 7457 58 7457 3807 7458 3802 7458 3698 7458 3782 7459 3701 7459 3718 7459 3777 7460 3690 7460 3796 7460 3737 7461 3717 7461 3740 7461 3791 7462 3772 7462 3735 7462 3775 7463 3489 7463 3717 7463 3799 7464 3827 7464 3656 7464 3781 7465 3704 7465 3728 7465 3764 7466 3721 7466 3802 7466 3786 7467 3780 7467 3708 7467 3711 7468 3745 7468 3744 7468 3779 7469 3778 7469 3694 7469 3748 7470 3736 7470 3724 7470 3748 7471 3738 7471 3736 7471 3746 7472 3784 7472 3739 7472 3774 7473 3694 7473 3722 7473 3762 7474 3686 7474 3743 7474 3710 7475 3948 7475 3767 7475 3750 7476 3751 7476 3725 7476 3782 7477 3786 7477 3708 7477 3750 7478 3725 7478 3729 7478 3787 7479 3733 7479 3755 7479 3763 7480 3732 7480 3754 7480 3748 7481 3742 7481 3738 7481 3763 7482 3697 7482 3732 7482 3748 7483 3756 7483 3742 7483 3798 7484 3757 7484 3749 7484 3737 7485 3740 7485 3753 7485 3803 7486 3782 7486 3718 7486 3790 7487 3762 7487 3743 7487 3787 7488 3760 7488 3733 7488 3758 7489 3724 7489 3697 7489 3792 7490 3737 7490 3753 7490 3756 7491 3748 7491 3724 7491 3762 7492 3781 7492 3728 7492 3756 7493 3724 7493 3758 7493 3758 7494 66 7494 3756 7494 3801 7495 3796 7495 3757 7495 3773 7496 3745 7496 3781 7496 3809 7497 3747 7497 3734 7497 3775 7498 3717 7498 3737 7498 3760 7499 3761 7499 3771 7499 3800 7500 3804 7500 3770 7500 3801 7501 3757 7501 3798 7501 3763 7502 3758 7502 3697 7502 3758 7503 76 7503 66 7503 3794 7504 3752 7504 3720 7504 3779 7505 3694 7505 3774 7505 3768 7506 3746 7506 3712 7506 3763 7507 76 7507 3758 7507 3811 7508 3780 7508 3786 7508 3789 7509 3788 7509 3759 7509 3766 7510 76 7510 3763 7510 3787 7511 3755 7511 3778 7511 3766 7512 3763 7512 3754 7512 3766 7513 123 7513 76 7513 3803 7514 3718 7514 3525 7514 3767 7515 3766 7515 3754 7515 3767 7516 3754 7516 3710 7516 3767 7517 123 7517 3766 7517 3774 7518 3722 7518 3788 7518 3819 7519 3749 7519 3772 7519 3819 7520 3798 7520 3749 7520 3790 7521 3743 7521 3752 7521 3814 7522 3759 7522 3764 7522 3800 7523 3770 7523 3747 7523 3785 7524 3790 7524 3752 7524 3776 7525 3810 7525 3489 7525 3776 7526 3489 7526 3775 7526 3792 7527 3753 7527 3751 7527 3809 7528 3734 7528 3765 7528 3783 7529 3784 7529 3746 7529 3768 7530 3712 7530 3769 7530 3794 7531 3720 7531 3777 7531 3793 7532 3744 7532 3745 7532 3815 7533 3792 7533 3751 7533 3847 7534 3789 7534 3759 7534 3823 7535 3765 7535 3780 7535 3813 7536 3750 7536 3729 7536 3813 7537 3729 7537 3784 7537 3801 7538 3777 7538 3796 7538 3642 7539 3803 7539 3525 7539 3812 7540 3781 7540 3762 7540 3761 7541 3760 7541 3787 7541 3791 7542 3819 7542 3772 7542 3789 7543 3774 7543 3788 7543 3790 7544 3812 7544 3762 7544 3783 7545 3746 7545 3768 7545 3847 7546 3759 7546 3814 7546 3795 7547 3776 7547 3775 7547 3795 7548 3775 7548 3737 7548 3813 7549 3784 7549 3783 7549 3786 7550 3782 7550 3803 7550 3808 7551 3794 7551 3777 7551 3799 7552 3951 7552 3816 7552 3829 7553 3815 7553 3751 7553 3787 7554 3778 7554 3779 7554 3785 7555 3752 7555 3794 7555 3744 7556 3793 7556 3477 7556 3800 7557 3747 7557 3809 7557 3829 7558 3751 7558 3750 7558 3837 7559 3814 7559 3764 7559 3808 7560 3777 7560 3801 7560 3837 7561 3764 7561 3802 7561 3823 7562 3809 7562 3765 7562 3793 7563 3745 7563 3773 7563 3836 7564 3774 7564 3789 7564 3806 7565 3768 7565 3769 7565 3853 7566 3811 7566 3786 7566 3828 7567 3800 7567 3809 7567 3838 7568 3771 7568 3761 7568 3812 7569 3773 7569 3781 7569 3805 7570 3813 7570 3783 7570 3823 7571 3780 7571 3811 7571 3797 7572 3425 7572 3804 7572 3835 7573 3823 7573 3811 7573 3825 7574 3477 7574 3793 7574 3821 7575 3795 7575 3737 7575 3821 7576 3737 7576 3792 7576 3831 7577 3793 7577 3773 7577 3837 7578 3802 7578 3807 7578 3853 7579 3786 7579 3803 7579 3806 7580 3769 7580 3827 7580 3797 7581 3804 7581 3800 7581 3820 7582 3801 7582 3798 7582 3890 7583 3805 7583 3783 7583 3844 7584 3783 7584 3768 7584 3877 7585 3819 7585 3791 7585 3820 7586 3798 7586 3819 7586 3817 7587 3810 7587 3776 7587 3774 7588 3855 7588 3779 7588 3868 7589 3829 7589 3750 7589 3831 7590 3825 7590 3793 7590 3855 7591 3787 7591 3779 7591 3834 7592 3773 7592 3812 7592 3890 7593 3783 7593 3844 7593 3812 7594 3790 7594 3522 7594 3821 7595 3792 7595 3815 7595 3855 7596 3818 7596 3787 7596 3838 7597 3761 7597 3787 7597 3832 7598 3838 7598 3787 7598 3806 7599 3844 7599 3768 7599 3843 7600 3831 7600 3773 7600 3847 7601 3836 7601 3789 7601 3868 7602 3750 7602 3813 7602 3522 7603 3834 7603 3812 7603 3833 7604 3835 7604 3811 7604 3834 7605 3843 7605 3773 7605 3805 7606 3868 7606 3813 7606 3826 7607 3821 7607 3815 7607 3655 7608 3799 7608 3816 7608 3655 7609 3827 7609 3799 7609 3822 7610 3477 7610 3825 7610 3512 7611 3808 7611 3801 7611 3824 7612 3817 7612 3776 7612 3824 7613 3776 7613 3795 7613 3841 7614 3855 7614 3774 7614 3820 7615 3512 7615 3801 7615 3870 7616 3797 7616 3800 7616 3849 7617 3806 7617 3827 7617 3842 7618 3834 7618 3522 7618 3833 7619 3811 7619 3853 7619 3828 7620 3809 7620 3823 7620 3870 7621 3800 7621 3828 7621 3860 7622 3826 7622 3815 7622 3845 7623 3832 7623 3787 7623 3857 7624 3825 7624 3831 7624 3877 7625 3791 7625 3948 7625 3860 7626 3815 7626 3829 7626 3880 7627 3853 7627 3803 7627 3859 7628 3878 7628 3823 7628 3845 7629 3787 7629 3818 7629 3830 7630 3881 7630 3425 7630 3830 7631 3425 7631 3797 7631 3859 7632 3823 7632 3835 7632 3839 7633 3824 7633 3795 7633 3871 7634 3820 7634 3819 7634 3878 7635 3828 7635 3823 7635 3888 7636 3838 7636 3832 7636 3841 7637 3774 7637 3836 7637 3851 7638 3512 7638 3820 7638 3877 7639 3871 7639 3819 7639 3655 7640 3849 7640 3827 7640 3501 7641 3847 7641 3814 7641 3865 7642 3814 7642 3837 7642 3857 7643 3831 7643 3843 7643 3848 7644 3838 7644 3888 7644 3856 7645 3868 7645 3805 7645 3839 7646 3795 7646 3821 7646 3846 7647 3836 7647 3847 7647 3871 7648 3851 7648 3820 7648 3840 7649 3822 7649 3825 7649 3888 7650 3832 7650 3845 7650 3642 7651 3880 7651 3803 7651 3850 7652 3840 7652 3825 7652 3852 7653 3857 7653 3843 7653 3807 7654 3865 7654 3837 7654 3860 7655 3829 7655 3868 7655 3841 7656 3836 7656 3846 7656 3854 7657 3845 7657 3818 7657 3854 7658 3818 7658 3855 7658 3850 7659 3825 7659 3857 7659 3867 7660 3839 7660 3821 7660 3840 7661 3874 7661 3418 7661 3852 7662 3843 7662 3834 7662 3873 7663 3848 7663 3888 7663 3875 7664 3850 7664 3857 7664 3850 7665 3874 7665 3840 7665 3859 7666 3835 7666 3833 7666 3514 7667 3861 7667 3842 7667 3867 7668 3821 7668 3826 7668 3873 7669 3462 7669 3848 7669 3864 7670 3854 7670 3855 7670 3862 7671 3852 7671 3834 7671 3879 7672 3830 7672 3797 7672 3863 7673 3423 7673 3817 7673 3880 7674 3833 7674 3853 7674 3846 7675 3847 7675 3501 7675 3864 7676 3855 7676 3505 7676 3888 7677 3845 7677 3854 7677 3890 7678 3856 7678 3805 7678 3869 7679 3844 7679 3806 7679 3879 7680 3881 7680 3830 7680 3824 7681 3863 7681 3817 7681 3862 7682 3834 7682 3842 7682 3875 7683 3857 7683 3852 7683 3866 7684 3867 7684 3826 7684 3620 7685 3888 7685 3854 7685 3501 7686 3814 7686 3865 7686 3864 7687 3620 7687 3854 7687 3884 7688 3863 7688 3824 7688 3661 7689 3870 7689 3828 7689 3869 7690 3806 7690 3849 7690 3883 7691 3850 7691 3875 7691 3866 7692 3826 7692 3860 7692 3882 7693 3879 7693 3797 7693 3887 7694 3875 7694 3852 7694 3618 7695 3862 7695 3842 7695 3892 7696 3890 7696 3844 7696 3882 7697 3797 7697 3870 7697 3858 7698 3865 7698 3807 7698 3948 7699 3871 7699 3877 7699 3872 7700 3878 7700 3859 7700 3661 7701 3828 7701 3878 7701 3876 7702 3868 7702 3856 7702 3462 7703 3873 7703 3888 7703 3632 7704 3621 7704 3881 7704 3629 7705 3888 7705 3620 7705 3885 7706 3866 7706 3860 7706 3884 7707 3824 7707 3839 7707 3886 7708 3520 7708 3851 7708 3869 7709 3892 7709 3844 7709 3891 7710 3462 7710 3888 7710 3889 7711 3856 7711 3890 7711 3891 7712 3623 7712 3462 7712 3632 7713 3881 7713 3879 7713 3651 7714 3883 7714 3875 7714 3627 7715 3852 7715 3862 7715 3627 7716 3887 7716 3852 7716 3858 7717 3501 7717 3865 7717 3874 7718 3850 7718 3883 7718 3618 7719 3842 7719 3861 7719 3635 7720 3620 7720 3864 7720 3702 7721 3520 7721 3886 7721 3884 7722 3839 7722 3867 7722 3668 7723 3864 7723 3506 7723 3886 7724 3851 7724 3871 7724 3651 7725 3875 7725 3887 7725 3668 7726 3635 7726 3864 7726 3892 7727 3889 7727 3890 7727 3652 7728 3882 7728 3870 7728 3671 7729 3872 7729 3859 7729 3640 7730 3632 7730 3879 7730 3894 7731 3896 7731 3897 7731 3900 7732 3950 7732 3894 7732 3950 7733 3894 7733 3897 7733 3899 7734 3895 7734 3527 7734 3898 7735 3526 7735 3895 7735 3899 7736 3527 7736 3532 7736 3901 7737 3897 7737 3341 7737 3899 7738 3898 7738 3895 7738 3901 7739 3950 7739 3897 7739 3894 7740 3899 7740 3532 7740 3898 7741 3900 7741 3899 7741 3894 7742 3900 7742 3899 7742 3894 7743 3532 7743 3896 7743 3533 7744 3901 7744 3341 7744 3896 7745 3532 7745 3527 7745 3893 7746 3950 7746 3901 7746 3893 7747 3533 7747 3536 7747 3893 7748 3901 7748 3533 7748 3893 7749 3536 7749 3902 7749 3931 7750 7 7750 3930 7750 3907 7751 3927 7751 3928 7751 7 7752 3931 7752 3921 7752 3904 7753 3903 7753 3919 7753 7 7754 3921 7754 3959 7754 3959 7755 14 7755 7 7755 3906 7756 3904 7756 3919 7756 3906 7757 3919 7757 3909 7757 3917 7758 3907 7758 3905 7758 3549 7759 3551 7759 3932 7759 3908 7760 3549 7760 3932 7760 3908 7761 3932 7761 3904 7761 3913 7762 3908 7762 3904 7762 3913 7763 3904 7763 3906 7763 3913 7764 3906 7764 3909 7764 3915 7765 3913 7765 3909 7765 3915 7766 3909 7766 3927 7766 3910 7767 3927 7767 3907 7767 3910 7768 3915 7768 3927 7768 3918 7769 3549 7769 3908 7769 3917 7770 3910 7770 3907 7770 3911 7771 3918 7771 3908 7771 3911 7772 3908 7772 3913 7772 3914 7773 3911 7773 3913 7773 3920 7774 3914 7774 3913 7774 3920 7775 3913 7775 3915 7775 3920 7776 3915 7776 3910 7776 3921 7777 3920 7777 3910 7777 3921 7778 3910 7778 3917 7778 3959 7779 3921 7779 3917 7779 3923 7780 3918 7780 3911 7780 3903 7781 3553 7781 3912 7781 3923 7782 3916 7782 3918 7782 3923 7783 3911 7783 3914 7783 3925 7784 3923 7784 3914 7784 3925 7785 3914 7785 3920 7785 3919 7786 3903 7786 3912 7786 3926 7787 3925 7787 3920 7787 3909 7788 3919 7788 3912 7788 3909 7789 3912 7789 3928 7789 3905 7790 3907 7790 3928 7790 3921 7791 3926 7791 3920 7791 3544 7792 3924 7792 3545 7792 3922 7793 3916 7793 3923 7793 3922 7794 3544 7794 3916 7794 3932 7795 3551 7795 3553 7795 3922 7796 3924 7796 3544 7796 3929 7797 3923 7797 3925 7797 3929 7798 3922 7798 3923 7798 3929 7799 2 7799 3924 7799 3929 7800 3924 7800 3922 7800 3929 7801 3925 7801 3926 7801 3927 7802 3909 7802 3928 7802 3932 7803 3553 7803 3903 7803 3931 7804 3929 7804 3926 7804 3931 7805 3930 7805 2 7805 3931 7806 2 7806 3929 7806 3904 7807 3932 7807 3903 7807 3931 7808 3926 7808 3921 7808 3933 7809 3938 7809 3559 7809 3905 7810 3933 7810 3559 7810 3905 7811 3559 7811 3558 7811 3935 7812 3941 7812 3560 7812 3935 7813 3934 7813 3941 7813 3936 7814 3941 7814 3934 7814 3559 7815 3938 7815 3937 7815 3936 7816 3940 7816 3934 7816 3558 7817 3559 7817 3937 7817 3937 7818 3938 7818 3939 7818 3558 7819 3942 7819 3937 7819 3564 7820 3934 7820 3935 7820 3940 7821 3934 7821 3564 7821 3564 7822 3935 7822 3566 7822 3941 7823 3939 7823 3562 7823 3942 7824 3937 7824 3939 7824 3941 7825 3562 7825 3560 7825 3941 7826 3942 7826 3939 7826 3940 7827 3409 7827 3564 7827 3941 7828 3936 7828 3942 7828 3905 7829 3558 7829 3917 7829 3574 7830 283 7830 190 7830 272 7831 3943 7831 3940 7831 3409 7832 272 7832 3940 7832 3579 7833 3943 7833 234 7833 3379 7834 3948 7834 3791 7834 3579 7835 234 7835 3944 7835 3961 7836 3584 7836 3945 7836 3378 7837 3791 7837 3816 7837 3378 7838 3379 7838 3791 7838 283 7839 3574 7839 3189 7839 3584 7840 3962 7840 3946 7840 3584 7841 3585 7841 3962 7841 3947 7842 3606 7842 3604 7842 3599 7843 3948 7843 3379 7843 3949 7844 3378 7844 3816 7844 3949 7845 3816 7845 3951 7845 3568 7846 3960 7846 3381 7846 3568 7847 3381 7847 3606 7847 3568 7848 3606 7848 3947 7848 3608 7849 3568 7849 3947 7849 3955 7850 3948 7850 3599 7850 3572 7851 3948 7851 3955 7851 3525 7852 3898 7852 3642 7852 3572 7853 3767 7853 3948 7853 3900 7854 3642 7854 3898 7854 3900 7855 3377 7855 3642 7855 3371 7856 3767 7856 3572 7856 3371 7857 3953 7857 3767 7857 3596 7858 3215 7858 3949 7858 3950 7859 3377 7859 3900 7859 3950 7860 3208 7860 3377 7860 3945 7861 3584 7861 3946 7861 3955 7862 3599 7862 3380 7862 112 7863 283 7863 3189 7863 3596 7864 3949 7864 3951 7864 3597 7865 3596 7865 3951 7865 3950 7866 3952 7866 3208 7866 3597 7867 3951 7867 3858 7867 3954 7868 3955 7868 3380 7868 3595 7869 3597 7869 3858 7869 3595 7870 3858 7870 3807 7870 3956 7871 51 7871 3953 7871 3956 7872 3953 7872 3371 7872 112 7873 3189 7873 3579 7873 250 7874 112 7874 3579 7874 3957 7875 3893 7875 3902 7875 3594 7876 3807 7876 3698 7876 3594 7877 3595 7877 3807 7877 3957 7878 3950 7878 3893 7878 14 7879 3950 7879 3957 7879 14 7880 3952 7880 3950 7880 3586 7881 3962 7881 3376 7881 3609 7882 250 7882 3579 7882 3612 7883 3609 7883 3579 7883 3594 7884 3698 7884 3642 7884 3961 7885 3945 7885 3958 7885 3961 7886 3958 7886 3205 7886 3961 7887 3205 7887 3952 7887 3944 7888 3612 7888 3579 7888 3944 7889 101 7889 3612 7889 3946 7890 3962 7890 3586 7890 3959 7891 3961 7891 3952 7891 3959 7892 3952 7892 14 7892 3960 7893 3954 7893 3603 7893 3960 7894 3603 7894 3381 7894 3960 7895 3955 7895 3954 7895 3936 7896 3943 7896 3961 7896 3942 7897 3936 7897 3961 7897 3574 7898 190 7898 51 7898 3574 7899 51 7899 3956 7899 3940 7900 3943 7900 3936 7900 3917 7901 3961 7901 3959 7901 3917 7902 3942 7902 3961 7902 3917 7903 3558 7903 3942 7903 272 7904 234 7904 3943 7904 3377 7905 3594 7905 3642 7905

 0.03250765 0.05446481 0.08426898 0.03047508 0.05612355 0.08424603 0.03548967 0.05634886 0.086021 0.03428429 0.05610334 0.08662801 0.0339902 0.05474203 0.08602797 0.03300803 0.05454272 0.08592301 0.03397667 0.05538797 0.08656299 0.032727 0.05552309 0.08658301 0.03097808 0.05551064 0.086012 0.0325793 0.05497992 0.08648902 0.03445237 0.05483162 0.08419996 0.03351306 0.05446887 0.08426898 0.03156542 0.05482023 0.08419996 0.03195255 0.05845153 0.08419996 0.03552752 0.0581932 0.08419996 0.03065013 0.05855041 0.08419996 0.03149163 0.05949163 0.08420097 0.0329473 0.05990344 0.08419996 0.03460115 0.05936515 0.08426398 0.03200978 0.05957305 0.08602797 0.03397876 0.0597139 0.08426797 0.03427881 0.05610394 0.08498001 0.03343278 0.05600082 0.08498001 0.03421807 0.05695414 0.08498001 0.0325672 0.05831432 0.08498001 0.03455173 0.05773836 0.08498001 0.03378528 0.05811071 0.08657997 0.03327298 0.05879193 0.08498001 0.03378528 0.05811071 0.08498001 0.0325672 0.05831432 0.08657997 0.03178185 0.057361 0.08498001 0.03172105 0.05821114 0.08498001 0.03144794 0.05657684 0.08498001 0.0322147 0.05620431 0.08498001 0.032727 0.05552309 0.08498001 0.03343278 0.05600082 0.08657997 0.03178185 0.057361 0.08657997 0.03172034 0.0582112 0.08659499 0.03421807 0.05695414 0.08657997 0.03144472 0.05657362 0.086627 0.0322147 0.05620431 0.08657997 0.03052628 0.05795472 0.08624899 0.03119826 0.05900633 0.086295 0.03294634 0.05976772 0.08592802 0.03327268 0.05879265 0.08659303 0.03407657 0.05955046 0.08603298 0.03455173 0.05773842 0.08658403 0.03556567 0.05749499 0.08626997 0.03347557 0.05929881 0.08651202 0.03519046 0.05551689 0.08427602 0.03483384 0.05530953 0.086057 0.03186661 0.05480062 0.08605802 0.03081762 0.05549675 0.08419996 0.0304231 0.05662626 0.08603399 0.03028178 0.05685651 0.084257 0.03519028 0.05868232 0.08539599 0.03571987 0.05745822 0.08423399 0.03457999 0.05727785 0.08419996 0.03377068 0.05847764 0.08419996 0.03367328 0.0557233 0.08419996 0.03173887 0.05613732 0.08419996 -0.002799153 0.08198893 0.07571703 -0.001655638 0.08014476 0.07570898 -0.0023579 0.08387076 0.07570403 -0.001481831 0.08206921 0.07435899 -0.002664566 0.08138722 0.07609999 -0.001050949 0.08499366 0.07569497 -9.34474e-4 0.08186042 0.07526999 -9.34474e-4 0.08293944 0.07526999 0 0.08347892 0.07526999 0 0.08132094 0.07526999 9.34474e-4 0.08293944 0.07526999 -7.5e-4 0.08110094 0.07526999 7.5e-4 0.08110094 0.07526999 9.34474e-4 0.08186042 0.07526999 0.001499891 0.08239996 0.07526999 7.5e-4 0.08369892 0.07609999 9.34474e-4 0.08293944 0.07609999 7.5e-4 0.08369892 0.07526999 0 0.08347892 0.07609999 -7.5e-4 0.08369892 0.07526999 -9.34474e-4 0.08186042 0.07609999 -0.001499891 0.08239996 0.07526999 -9.34474e-4 0.08293944 0.07609999 0.002151489 0.08057183 0.07609999 7.5e-4 0.08110094 0.07609999 0 0.08132094 0.07609999 -0.001499891 0.08239996 0.07609999 -0.002571821 0.08345824 0.07609999 -0.001113057 0.07983773 0.07609999 -7.5e-4 0.08110094 0.07609999 0.001499891 0.08239996 0.07609999 -7.5e-4 0.08369892 0.07609999 7.09302e-4 0.08516097 0.07609999 9.34474e-4 0.08186042 0.07609999 -0.001503825 0.08476197 0.07609999 0.002785921 0.08318054 0.07609999 5.12534e-4 0.07966643 0.07609999 0.001475811 0.08006042 0.07570701 0.00250113 0.08114272 0.07570201 9.34971e-4 0.08120906 0.0744 0.001556575 0.08236372 0.07447701 0.002685487 0.08317112 0.07565599 7.02233e-4 0.08377617 0.074467 -3.17971e-5 0.07964372 0.07565301 -5.82314e-4 0.08382123 0.07441502 0.001251459 0.08497965 0.07567399 -0.030267 0.05692702 0.08419996 -0.03062075 0.05606901 0.08609998 -0.03241103 0.05560582 0.08662199 -0.03105205 0.05635994 0.08654701 -0.03283679 0.05455863 0.08607 -0.03487312 0.0553267 0.08610802 -0.03488814 0.05517524 0.08427196 -0.03042209 0.0581355 0.084279 -0.03137063 0.05496895 0.08424103 -0.0308119 0.0555092 0.08425402 -0.03354305 0.05447363 0.08427196 -0.03505146 0.05884754 0.085509 -0.03465747 0.05936092 0.08428901 -0.0356034 0.05701524 0.08602499 -0.03204315 0.05637651 0.08498001 -0.03136438 0.05689191 0.08498001 -0.0319522 0.05844122 0.08498001 -0.03395658 0.05793839 0.08498001 -0.03319787 0.05593854 0.08498001 -0.03415465 0.05671954 0.08498001 -0.03204315 0.05637651 0.08657997 -0.03184527 0.05759567 0.08498001 -0.03280186 0.05837661 0.08498001 -0.03358769 0.05870682 0.08498001 -0.03395658 0.05793839 0.08657997 -0.0346356 0.05742323 0.08498001 -0.03319787 0.05593854 0.08657997 -0.03404778 0.05587393 0.08498001 -0.03241229 0.05560833 0.08498001 -0.03136426 0.05689191 0.08658403 -0.03184527 0.05759567 0.08657997 -0.03404814 0.05586767 0.08662402 -0.03280186 0.05837661 0.08657997 -0.03415465 0.05671954 0.08657997 -0.03359097 0.05871123 0.086631 -0.035456 0.05802482 0.086061 -0.03464019 0.05742305 0.08663201 -0.03417396 0.05947613 0.08612102 -0.03291183 0.05977654 0.086111 -0.03195178 0.05844634 0.08661901 -0.03149873 0.05931675 0.086003 -0.0311895 0.05835694 0.08650296 -0.03039598 0.05730319 0.08601999 -0.03252226 0.05445611 0.08419996 -0.03162252 0.05493324 0.08611601 -0.03381466 0.05467945 0.08601498 -0.03533035 0.05574065 0.08425897 -0.03540384 0.05624014 0.086286 -0.03572839 0.05687785 0.08419996 -0.03563737 0.05789214 0.08427298 -0.0334779 0.05985903 0.08419996 -0.03245687 0.05984151 0.08427101 -0.03176206 0.05959224 0.08419996 -0.03111189 0.05914032 0.08427 -0.03064978 0.05829441 0.08602303 -0.03464072 0.05720669 0.08419996 -0.0342381 0.05472314 0.08419996 -0.03295063 0.05879843 0.08419996 -0.03147947 0.05751591 0.08419996 -0.03192883 0.05602043 0.08419996 -0.03344947 0.05566161 0.08419996 -0.06488209 0.001222968 0.08028197 -0.06340861 3.16347e-4 0.07963401 -0.06390869 -5.77937e-4 0.08024996 -0.06384795 -0.001483142 0.07963901 -0.0647431 -0.002303719 0.07972598 -0.0643869 -0.00221163 0.07796698 -0.06551998 -0.002719104 0.077901 -0.06470626 0.002410352 0.07794499 -0.06394308 0.001813471 0.07789999 -0.06640321 0.001474261 0.07789999 -0.06690287 0.002574384 0.07794797 -0.06828075 -0.001270473 0.07967698 -0.06702286 0.002386093 0.07981801 -0.06611287 0.002603054 0.079732 -0.06626576 -0.00120604 0.07867997 -0.06508839 -8.33246e-4 0.07867997 -0.06691157 8.33179e-4 0.07867997 -0.06482261 3.72812e-4 0.08028 -0.06482261 3.72813e-4 0.07867997 -0.06488209 0.001222968 0.07867997 -0.06650096 0.001580417 0.08029901 -0.0657342 0.001205921 0.07867997 -0.06650018 0.001579642 0.07867997 -0.06691157 8.33179e-4 0.08028 -0.06761819 3.5658e-4 0.07867997 -0.06717735 -3.7288e-4 0.08028 -0.06717735 -3.7288e-4 0.07867997 -0.06711786 -0.001223087 0.07867997 -0.06549978 -0.001579642 0.07867997 -0.06626576 -0.00120604 0.08028 -0.06438177 -3.56647e-4 0.07867997 -0.06711786 -0.001223087 0.08028399 -0.0657342 0.001205921 0.08028 -0.06438165 -3.5658e-4 0.08028596 -0.06549978 -0.001579642 0.08028 -0.06508839 -8.33247e-4 0.08028 -0.06487745 0.001788437 0.08026099 -0.06578719 -0.002242565 0.08023297 -0.0685839 -3.55331e-4 0.079701 -0.06815415 -6.70158e-4 0.08019697 -0.06761819 3.56571e-4 0.08028298 -0.06847131 8.83519e-4 0.07972896 -0.06781738 0.001839578 0.07992702 -0.06431949 0.001988351 0.07975798 -0.06371361 0.001241624 0.07977896 -0.06342667 9.47516e-4 0.07789999 -0.06343865 -5.61829e-4 0.07958 -0.06369268 -0.001482129 0.07789999 -0.06588846 -0.002603948 0.079723 -0.06672435 -0.002502322 0.07972401 -0.06765896 -0.002020597 0.07968097 -0.06730109 -0.002418637 0.07789999 -0.06835067 -0.00140208 0.07797199 -0.06870371 -4.58751e-4 0.07789999 -0.06867718 5.90302e-4 0.07796698 -0.06812298 0.001712501 0.07795602 -0.06761628 0.002215266 0.07789999 -0.06620711 0.002728879 0.07797598 -0.06522846 0.00249058 0.07972496 -0.06723588 -8.99537e-4 0.07789999 -0.06749498 5.25215e-4 0.07789999 -0.06514918 0.001299202 0.07789999 -0.0644226 -1.51584e-4 0.07789999 -0.06576508 -0.001605093 0.07789999 -0.02952915 -0.05605751 0.07075196 -0.03191179 -0.05856931 0.07069998 -0.02995717 -0.05455666 0.07251799 -0.03077393 -0.05419296 0.07299399 -0.03071469 -0.05523508 0.07308501 -0.03094238 -0.05687654 0.07308596 -0.02982676 -0.05675745 0.07280498 -0.03269398 -0.05842322 0.07253301 -0.03364557 -0.0580672 0.07252597 -0.03247755 -0.05750018 0.07308995 -0.03271538 -0.0585606 0.07078796 -0.03287225 -0.0573309 0.07069998 -0.03377157 -0.05604839 0.07069998 -0.03204005 -0.0543397 0.07069998 -0.03131008 -0.05328691 0.07077699 -0.03371053 -0.05817008 0.070773 -0.0343163 -0.05764538 0.07069998 -0.03332322 -0.05333364 0.07069998 -0.03437584 -0.05736207 0.07251501 -0.03494107 -0.05534929 0.07076501 -0.03469997 -0.05496919 0.07253599 -0.03335708 -0.053496 0.07250499 -0.03149116 -0.05488419 0.07147997 -0.03102666 -0.0560283 0.07147997 -0.0327146 -0.05471438 0.07147997 -0.03071469 -0.0552352 0.07147997 -0.03202205 -0.05421727 0.07147997 -0.03355747 -0.05484068 0.07147997 -0.03347325 -0.05568867 0.07147997 -0.03378528 -0.05648189 0.07147997 -0.03300869 -0.05683296 0.07147997 -0.03247767 -0.05749988 0.07147997 -0.03178524 -0.0570029 0.07147997 -0.03094244 -0.05687654 0.07147997 -0.03102666 -0.0560283 0.07308 -0.03347325 -0.05568867 0.07308 -0.03149116 -0.05488419 0.07308 -0.03142136 -0.05773568 0.07305699 -0.03202205 -0.05421727 0.07308095 -0.03178524 -0.0570029 0.07308 -0.0327146 -0.05471438 0.07308 -0.03300869 -0.05683296 0.07308 -0.03468239 -0.05669039 0.07276701 -0.03376019 -0.05734115 0.07304197 -0.03412735 -0.0540409 0.07250797 -0.03378528 -0.05648189 0.07308501 -0.03356528 -0.054838 0.07313698 -0.0323053 -0.05327171 0.07276201 -0.03098803 -0.05355161 0.07246899 -0.03001403 -0.05427509 0.07069998 -0.02964228 -0.05584073 0.07252496 -0.02958887 -0.05521178 0.07077097 -0.0298084 -0.0570836 0.07069998 -0.03022575 -0.0574786 0.07254797 -0.03109627 -0.05834293 0.070764 -0.03078657 -0.0580185 0.07250899 -0.03023338 -0.05771195 0.07076501 -0.03174448 -0.05840706 0.07261598 -0.0348773 -0.05600816 0.07220399 -0.03476828 -0.05692929 0.07079797 -0.0345453 -0.05435568 0.07069998 -0.0323202 -0.0531246 0.07076698 -0.03334647 -0.05474603 0.07069998 -0.03098857 -0.05684059 0.07069998 -0.03089153 -0.05508768 0.07069998 0.06835079 0.00140208 0.07796996 0.06830275 -0.001479268 0.07797402 0.0676161 -0.002215445 0.07789999 0.06784039 0.001892447 0.07962995 0.06711798 0.001223266 0.08029198 0.06862318 9.53732e-5 0.079714 0.06761819 -3.56647e-4 0.08028197 0.0681172 -2.18626e-4 0.08025199 0.06724208 -0.002301633 0.079813 0.0667048 -0.0026443 0.07797896 0.06752258 0.002295911 0.07789999 0.06590998 -0.002726376 0.07795298 0.06873637 -4.49616e-5 0.077977 0.06860208 -8.31453e-4 0.07789999 0.0644831 3.33682e-4 0.07789999 0.06576508 0.001604974 0.07789999 0.06427997 -0.002131104 0.07789999 0.06340175 8.30554e-4 0.077955 0.06387346 0.001714468 0.07789999 0.0633974 1.30897e-4 0.07972496 0.06494939 0.002386868 0.07970297 0.06588709 0.002603054 0.079732 0.06672745 0.002488315 0.079858 0.06691157 -8.33246e-4 0.07867997 0.06711786 0.001222968 0.07867997 0.06549978 0.001579642 0.07867997 0.06626576 0.001205921 0.07867997 0.06508839 8.33179e-4 0.07867997 0.06717735 3.72813e-4 0.07867997 0.06438177 3.5658e-4 0.07867997 0.06482261 -3.7288e-4 0.07867997 0.0657342 -0.00120604 0.07867997 0.06488209 -0.001223087 0.07867997 0.06650018 -0.001579642 0.07867997 0.0657342 -0.00120604 0.08028 0.06761819 -3.56647e-4 0.07867997 0.06482261 -3.7288e-4 0.08028 0.06717735 3.72812e-4 0.08028 0.06626576 0.001205921 0.08028 0.06549978 0.001579642 0.08028 0.06691157 -8.33247e-4 0.08028 0.06508839 8.33179e-4 0.08028 0.066504 -0.001585543 0.08033001 0.06438165 3.56512e-4 0.08028596 0.06551647 0.002054691 0.08025598 0.06506508 -0.002445042 0.07972997 0.06402498 -0.001702666 0.07976502 0.06488025 -0.001227319 0.08035701 0.063932 5.51532e-4 0.08025896 0.06832808 0.001124978 0.07985901 0.06864577 6.61852e-4 0.07795995 0.06826978 -0.001310527 0.079768 0.06611287 -0.002603054 0.079732 0.06364899 -0.001402199 0.07796698 0.06351375 -7.9046e-4 0.07967698 0.06325811 4.49419e-5 0.07789999 0.06363427 0.001088321 0.07971698 0.06503117 0.002578616 0.07798999 0.06418907 0.001877069 0.07967096 0.06723588 8.99471e-4 0.07789999 0.06511169 -0.001357138 0.07789999 0.06739276 -9.1153e-4 0.07789999 0.06608617 0.002730071 0.07789999 0.03148639 -0.05334621 0.072281 0.03103417 -0.05401319 0.073044 0.03257948 -0.05376988 0.07305401 0.03286188 -0.0543183 0.07309299 0.03437447 -0.05433243 0.07265299 0.03418982 -0.0551303 0.073058 0.03483664 -0.0555365 0.07254397 0.03004837 -0.05422282 0.07069998 0.03428673 -0.054039 0.07069998 0.03172975 -0.05855232 0.07069998 0.03426969 -0.0575087 0.07251995 0.03253376 -0.05844968 0.07254099 0.03152632 -0.05836355 0.072528 0.02959996 -0.05655139 0.07075601 0.03388947 -0.05561882 0.07147997 0.03286188 -0.05431866 0.07147997 0.03163784 -0.05739837 0.07147997 0.03110224 -0.05540239 0.07147997 0.03321886 -0.05509281 0.07147997 0.03207117 -0.05463659 0.07147997 0.0324288 -0.0570805 0.07147997 0.03207117 -0.05463659 0.07308 0.03110224 -0.05540239 0.07308 0.03122246 -0.05455869 0.07147997 0.0306105 -0.05609852 0.07147997 0.03128105 -0.05662453 0.07147997 0.03327745 -0.05715847 0.07147997 0.03339767 -0.0563147 0.07147997 0.0324288 -0.0570805 0.07308 0.03122246 -0.05455869 0.07308197 0.03321886 -0.05509281 0.07308 0.03128105 -0.05662453 0.07308 0.03388947 -0.05561882 0.07308298 0.03339767 -0.0563147 0.07308 0.03243684 -0.05787539 0.07306295 0.03475093 -0.05660516 0.07252997 0.03327757 -0.05715847 0.07308399 0.03398925 -0.05707705 0.07302498 0.03350478 -0.05814009 0.07257699 0.03163737 -0.05740112 0.073116 0.03063225 -0.05790376 0.07252597 0.02999603 -0.05714571 0.072645 0.0306105 -0.05609852 0.07308197 0.02968537 -0.0561825 0.07274198 0.02974015 -0.05511146 0.07242995 0.0300619 -0.05448168 0.07264995 0.03066909 -0.05370748 0.07183098 0.0322594 -0.05311506 0.07069998 0.03273135 -0.05326962 0.072461 0.03301805 -0.05324071 0.07074499 0.03370457 -0.05358529 0.07131099 0.03472387 -0.05468529 0.07077097 0.03496927 -0.0561816 0.07075995 0.03493613 -0.05538427 0.07075196 0.03445214 -0.0574932 0.070701 0.0337041 -0.0581845 0.070701 0.0327509 -0.05855101 0.07076799 0.0307818 -0.05817008 0.070773 0.03003317 -0.05747461 0.07069998 0.02953076 -0.05553549 0.07076197 0.03309124 -0.05726832 0.07069998 0.03375267 -0.05543267 0.07069998 0.03133338 -0.05708765 0.07069998 0.03068792 -0.0558809 0.07069998 0.03161656 -0.05439078 0.07069998 0.03298717 -0.05453819 0.07069998 -0.5033184 -0.09980785 0.8583176 0.4739162 -0.2167856 0.853468 0.4763981 -0.2716492 0.8362126 0.4692966 -0.2779644 0.8381506 0.3706454 -0.5878634 0.7190541 0.08430194 -0.7566365 0.6483783 0.1433843 -0.6295443 0.7636197 -0.1224512 -0.8281373 0.5469866 -0.002575576 -0.1698207 0.9854716 -0.0865907 -0.1469531 0.9853461 -0.3319864 -0.3589229 0.87233 -0.4124759 -0.3658636 0.8342706 -0.4944726 -0.2623228 0.8286639 -0.291064 0.06763005 0.9543102 0.07937568 -0.02538084 -0.9965217 -0.1479104 -0.008110105 -0.9889675 -0.1005434 -0.01163512 -0.9948647 0.9637933 -0.2636973 -0.03957682 0.1624073 -0.06560069 -0.9845407 7.38835e-4 -0.1887733 -0.9820204 7.51907e-4 -0.1887224 -0.9820302 -0.9771354 0.212601 0.002718031 0.09000796 0.1357783 -0.9866422 0.04328352 0.08857256 -0.9951289 0.7831646 0.6209363 -0.0330345 -0.8401312 0.5323283 0.1039533 -0.7445719 0.665672 0.04993695 -0.5425169 0.8318898 0.1167689 -0.1950427 0.9777913 0.0766983 -0.2720434 0.961674 0.03428667 0.1813074 0.9804002 0.0770927 0.1749013 0.9812325 0.08119475 0.6505995 0.7507732 0.1142809 0.4884653 0.8709349 0.05361342 0.9221935 0.3712672 0.1082581 0.9643918 0.2554746 0.06842023 0.9936309 -0.0825085 0.07674682 1.00278e-5 0 1 0 0 1 2.00548e-5 0 1 -1.25352e-5 0 1 -5.50855e-6 0 1 5.50809e-6 0 1 -8.26146e-6 0 1 -0.1209234 0.9926617 7.25232e-4 -0.1195344 0.9928301 0 -0.9974572 -0.07126826 0 -0.9969936 -0.07741534 0.003289759 -0.9201483 0.3915702 -1.5967e-5 -0.9201608 0.3915405 0 -0.4369489 -0.8994864 0 -0.4368889 -0.8995155 3.72622e-5 -0.7992214 -0.6010368 1.21719e-4 -0.7993568 -0.6008569 0 0.5604476 -0.8281899 0 0.561184 -0.827691 4.71001e-4 0.1209741 -0.9926558 9.51366e-5 0.1208055 -0.9926763 0 0.9974527 0.0713312 0 0.9973937 0.07214993 4.367e-4 0.9200866 -0.391714 0.00103408 0.9193004 -0.3935566 0 0.4370292 0.8994475 0 0.432621 0.9015722 0.002607583 0.799223 0.6010347 0 0.799217 0.6010427 0 -0.5605453 0.8281239 0 -0.5605366 0.8281296 0 0.02561587 -0.0650016 0.9975563 -0.04777878 -0.07000839 0.9964016 0.01439309 -0.01491689 0.9997852 -0.1214922 0.04653692 0.9915009 0.09810853 0.07821351 0.9920974 -0.02295511 -0.03640431 0.9990736 0.1528608 -0.00640273 0.988227 -0.3053823 0.1540312 0.9396894 -0.1069507 0.2887549 0.9514106 -0.08236271 0.7393507 0.6682642 -0.1513693 0.5903242 0.792846 0.1053434 0.8198359 0.562825 -0.1187093 0.2031539 0.9719242 0.1031447 0.116733 0.9877929 0.6945379 0.5558472 0.4567832 0.4524376 0.4185766 0.7874605 0.3446345 0.2790427 0.8963047 0.2910333 -0.02188462 0.9564626 0.8425037 -0.5279183 0.1071916 0.5529642 -0.827266 0.0993061 0.6768409 -0.7344313 0.04997038 0.362826 -0.9307131 0.04615867 0.1875617 -0.9770577 0.1008901 0.003975033 -0.998942 0.04581725 -0.2069191 -0.9723384 0.1083633 -0.3556004 -0.9334163 0.04777771 -0.6244601 -0.7754981 0.09301811 -0.6694334 -0.7400293 0.06492948 -0.8916423 -0.4451902 0.08233845 -0.8687011 -0.4828574 0.1104866 -0.965838 -0.2554336 0.04371362 -0.994187 0.06309968 0.08723884 0 0 -1 -8.58365e-4 5.91561e-4 -0.9999995 9.96527e-6 0 -1 -6.61052e-6 0 -1 -7.19155e-6 0 -1 -2.77765e-6 0 -1 -1.28166e-5 0 -1 5.6332e-6 0 -1 6.66987e-5 0.001008093 -0.9999996 -0.479488 0.5525813 -0.6817223 -0.7130321 0.1624801 -0.6820451 -0.5531211 0.3087294 -0.773785 -0.197915 -0.5508786 -0.810779 -0.6465911 -0.3980839 -0.6507297 -0.001673638 0.7370959 -0.6757861 -0.166693 0.9246184 0.3424824 0.00205481 0.9210624 -0.3894097 -0.1031731 -0.9790806 0.1753752 -0.2901417 -0.9058332 -0.3086814 -0.6503495 -0.6512004 0.3911313 -0.7529543 -0.4648666 -0.4657887 -0.9206089 0.04122614 0.3883038 -0.9354007 0.2174088 -0.2788528 -0.7518202 0.6158938 0.2354604 -0.6259067 0.7261843 -0.2844244 1.19261e-5 0 1 0.971745 0.2360333 0 0.9717457 0.2360306 0 -0.2814744 0.9595688 0 0.2814744 0.9595688 0 -0.9717457 0.2360306 0 -0.971745 0.2360333 0 -0.6903589 0.7234671 0 -0.6903471 0.7234784 0 -0.6903118 -0.723512 0 -0.6903237 -0.7235007 0 -0.971745 -0.2360333 0 -0.9717457 -0.2360306 0 0.2814744 -0.9595688 0 -0.2814744 -0.9595688 0 0.9717457 -0.2360306 0 0.971745 -0.2360333 0 0.6903237 -0.7235007 0 0.6903118 -0.723512 0 0.6903588 0.7234672 0 0.6903471 0.7234783 0 1.23672e-5 0 1 5.59306e-6 0 1 1.21165e-5 0 1 -1.01774e-5 0 1 1.44227e-6 0 1 5.25257e-6 0 1 2.71651e-6 0 1 0.2652295 -0.9238172 -0.276071 0.5995004 0.6286218 0.495413 0.7688531 0.6114976 -0.1869106 0.9715322 -0.09324365 -0.2177865 0.8639662 -0.2101131 0.457619 0.684287 -0.6497935 -0.3309379 0.4621554 -0.8366186 0.2940782 0.5364786 -0.5113323 -0.6713644 0.7475427 -0.0828948 -0.6590208 0.6072443 -0.2772476 -0.7445725 0.5574619 0.331799 -0.7610163 0.5562703 0.4480436 -0.6998717 0.05339235 0.6948642 -0.7171562 0.2149368 -0.6877748 -0.6933743 -0.2680953 -0.1069704 0.9574353 0.665852 0.1682553 0.726864 0.5736636 0.05750799 0.8170697 0.691102 -0.07930755 0.7183929 0.1740902 0.03369176 0.9841533 0.4959195 -0.426788 0.7562513 0.2555536 -0.3717989 0.892445 0.1222066 -0.5012022 0.8566574 -0.06970477 -0.4428292 0.8938924 -0.1073889 -0.4701204 0.8760448 -0.2180162 -0.4787433 0.8504552 -0.3168902 -0.3554717 0.8793297 -0.2607737 -0.2254937 -0.9386958 -0.1493728 -0.1033188 -0.9833683 0.9335177 -0.3583304 0.01200067 0.07182663 -0.06555521 -0.9952605 0.04624122 -0.02379512 -0.9986469 0.4054354 -0.9133706 0.0370993 -0.07047748 -0.4529129 -0.8887648 0.1730944 0.09708648 -0.9801085 0.1315851 0.08150428 -0.9879486 0.917796 -0.05398821 -0.3933649 -0.7156159 0.5402048 -0.4428011 0.3587319 -0.8961424 -0.2612286 0.06974107 0.4484773 -0.8910693 -0.9714822 -0.2220189 0.08324688 -0.9859824 0.1407493 0.08960193 -0.9944888 0.08474326 0.0617299 -0.8810858 0.4698012 0.05453985 -0.6315291 0.7662459 0.118484 -0.2292973 0.9671561 0.1096901 -0.3858524 0.921517 0.0438677 0.01449882 0.9991394 0.03886264 0.3151673 0.9424969 0.1112172 0.3445276 0.9342115 0.0924642 0.567155 0.8224943 0.04287719 0.7639764 0.6365184 0.1057564 0.8228912 0.5654398 0.05592942 2.50707e-6 0 1 1.00298e-5 0 1 1.00283e-5 0 1 -2.00608e-5 0 1 -1.10175e-5 0 1 5.50861e-6 0 1 -5.50828e-6 0 1 -0.6047114 0.7964448 4.25767e-5 -0.6046368 0.7965013 0 -0.8256298 -0.5642122 0 -0.8255659 -0.5643057 6.38596e-5 -0.992099 -0.1254565 6.48482e-4 -0.9922538 -0.124228 0 0.07581925 -0.9971216 0 0.0816738 -0.9966542 0.003127813 -0.3874147 -0.921904 0.001690745 -0.3904268 -0.9206341 0 0.9014621 -0.432858 0 0.9038344 -0.4278727 0.002938508 0.6044925 -0.7966092 0.001596748 0.6020464 -0.7984612 0 0.825595 0.5642633 0 0.8230966 0.5678966 0.002347111 0.9921084 0.1253817 6.95786e-4 0.9922742 0.1240647 0 -0.07580393 0.9971228 0 -0.08287161 0.9965531 0.003781676 0.3874765 0.921879 0.001091897 0.389407 0.9210659 0 -0.9013758 0.4330377 0 -0.902429 0.4308368 0.001299858 0.189832 -0.153857 0.9696865 0.03235703 -0.05048185 0.9982007 0.03384917 0.03829443 0.9986931 -0.09201103 -0.05713832 0.9941173 0.03372663 0.2034045 0.9785138 -0.02733087 -0.2133774 0.9765876 0.216457 0.07738381 0.9732205 0.1545671 0.03619968 0.9873189 -0.5670089 0.1280604 0.8136962 -0.5476017 0.05015903 0.8352345 -0.6996283 0.6020916 0.3847154 -0.3852866 0.3043137 0.8711759 -0.3708216 0.3027214 0.8779814 -0.1101976 0.4918285 0.8636904 0.07138055 0.4004218 0.9135465 0.2283778 0.5066475 0.8313555 0.1863489 0.4650309 0.8654597 0.602932 0.5126876 0.6112484 0.9636358 0.2464205 0.1033595 0.9912451 0.1242895 0.04455775 0.9795656 -0.1716094 0.1048891 0.7452234 -0.6556232 0.121657 0.6919735 -0.7175514 0.07932662 0.2898508 -0.9517351 0.1009302 -0.1273965 -0.9871292 0.09667593 -0.01349467 -0.9985314 0.05247086 -0.5127969 -0.8527718 0.09909385 -0.3425197 -0.9377601 0.05732542 -0.5640158 -0.8225762 0.0724889 -0.8524517 -0.5139694 0.09571623 -0.7870951 -0.6141809 0.05712252 -0.9435805 -0.3278552 0.04655015 -2.1664e-6 0 -1 -1.2456e-5 0 -1 3.85169e-6 0 -1 -5.03907e-6 0 -1 7.05756e-6 0 -1 -0.1663467 -0.9860399 0.007357835 0.3479083 0.6725922 0.6531307 0.1056883 0.03599977 0.9937475 0.5115072 0.435849 0.7405378 0.09105205 0.03262525 0.9953116 0.44706 0.1789277 0.8764259 0.6847812 0.1129342 0.7199449 0.8176938 -0.06314963 0.5721791 0.7811604 -0.3124024 0.5405491 0.4956147 -0.4627799 0.7349836 0.3737899 -0.4302178 0.8217018 0.2126822 -0.8166651 0.5364927 -0.09690588 -0.8028872 0.5882019 0.1653924 -0.2451373 -0.9552764 0.9899974 -0.1083836 -0.09032261 0.9992119 -0.03281003 -0.02234542 0.01236915 0.1130584 -0.9935114 0.01887607 0.09545338 -0.9952549 -0.006394863 0.06706476 -0.9977281 -0.2155759 -0.1542561 -0.9642262 -0.6948164 -0.7188792 0.02104866 -0.1647453 0.06700557 -0.9840576 -0.1933313 0.08593112 -0.9773632 -0.896071 0.4428268 0.03100311 -0.4594339 -0.8820074 0.1048026 -0.7675957 -0.6358513 0.08056044 -0.9455148 -0.3153402 0.08100861 -0.9361967 -0.3454034 0.0650562 -0.9909893 0.08776944 0.101178 -0.9976521 0.02108532 0.06515926 -0.8307 0.5488275 0.09341341 -0.699468 0.7119213 0.06255143 -0.5548294 0.8261632 0.09807676 -0.4520456 0.8899341 0.06059861 -0.2238426 0.971203 0.08160406 -0.2192196 0.9720695 0.08380705 0.1264011 0.9899031 0.06414586 -5.01461e-6 0 1 -5.0143e-6 0 1 -5.5088e-6 0 1 1.1017e-5 0 1 1.37717e-6 0 1 -0.9975609 -0.06980246 0 -0.9975608 -0.06980371 0 0.02000153 -0.9998 0 0.02000182 -0.9998 0 -0.4384901 -0.898736 2.18329e-4 -0.4388639 -0.8985536 0 0.8758001 -0.4826739 0 0.8763985 -0.4815862 6.53996e-4 0.5591778 -0.8290478 -4.70133e-6 0.5591774 -0.829048 0 0.8558438 0.5172346 0 0.8558458 0.5172311 2.91107e-6 0.9975613 0.06979608 0 0.9975611 0.0697993 0 -0.02000153 0.9998 0 -0.02000224 0.9998 0 0.4383792 0.8987901 0 -0.8757829 0.4827055 2.50109e-7 -0.8757861 0.4826996 0 -0.5591707 0.8290527 6.93502e-6 -0.5591657 0.829056 5.00103e-7 -0.8558518 -0.5172213 -3.20109e-7 -0.8557721 -0.5173531 8.5104e-5 0.005539715 -0.04277145 0.9990696 0.01641917 0.005846261 0.9998482 -0.01966243 -0.003695309 0.9997999 -0.00306338 0.03709876 0.9993069 0.02369678 -0.002236485 0.9997168 0.04132902 -0.07374358 0.9964205 0.08658087 -0.1077862 0.9903969 0.02890074 -0.01592725 0.9994555 -0.02850943 -0.05843371 0.9978842 0.03397184 -0.04404425 0.9984519 0.08643829 0.6315176 0.7705284 -0.04264372 0.4916362 0.8697559 -0.2985777 -0.50705 0.8085492 -0.5679513 -0.4671776 0.6776255 -0.3017786 -0.4348401 0.8484362 -0.3070887 -0.4429962 0.8422891 -0.8063823 -0.2807843 0.5204882 -0.1000863 -0.03106355 0.9944938 -0.7382009 0.05189788 0.6725818 -0.4905365 0.1580026 0.8569768 -0.2303112 0.1977641 0.9528096 -0.4371656 0.1539679 0.886104 -0.1684609 0.4225493 0.8905466 -0.03825867 0.5027884 0.8635624 0.4783268 0.8723534 0.1010112 0.6171209 0.7852064 0.05111682 0.7715063 0.6287655 0.09711903 0.857758 0.5115272 0.05090534 0.9496106 0.2975772 0.09842532 0.9127787 -0.4002915 0.08125215 0.6775948 -0.7287328 0.09906446 0.7258506 -0.6848323 0.06438636 0.4068245 -0.9128512 0.03459298 0.2517032 -0.9613412 0.1116634 -0.1205556 -0.991967 0.03831464 0.2079117 0.9780014 0.0169155 -6.48637e-6 0 -1 -1.10404e-5 0 -1 4.97753e-5 -8.91252e-4 -0.9999997 5.111e-6 0 -1 4.34152e-4 -8.08649e-4 -0.9999997 -4.24974e-6 0 -1 3.25626e-6 0 -1 9.66466e-6 0 -1 0.5820697 0.6225262 0.5231215 0.5437851 0.1378505 0.8278255 0.5478532 0.1415771 0.8245077 0.2474951 -0.0337727 0.9683004 0.455722 0.103148 0.8841255 0.4277804 -0.5040124 0.7503169 0.4280604 -0.4990976 0.7534362 0.3322389 -0.6242741 0.7070356 -0.08862131 -0.5166797 0.8515799 -0.06835466 -0.4790036 0.8751476 -0.09630668 -0.5210559 0.8480718 -0.2256554 -0.5877999 0.7768983 -0.1068038 -0.3083701 -0.9452518 0.195254 0.01429855 -0.9806485 0.08196038 0.02818453 -0.996237 0.9630926 -0.2645935 0.04942589 0.1472586 -0.1998369 -0.9687003 0.126192 -0.1786776 -0.9757817 0.03507566 0.1232953 -0.99175 0.03425383 0.1522489 -0.9877485 -0.09281069 -0.2415335 -0.965944 0.7903524 0.5387648 -0.2916774 -0.1384848 -0.009640336 -0.9903177 -0.9626454 -0.2598071 -0.07625108 -0.6414827 0.7669953 0.01477628 -0.6924897 -0.7159183 0.08898949 -0.6579538 -0.7499421 0.06843787 -0.8923092 -0.44184 0.09252983 -0.8407994 -0.5383994 0.05641472 -0.9728556 -0.2208161 0.06922638 -0.9853088 0.134476 0.1052752 -0.845034 0.5244921 0.1040466 -0.9269604 0.3722937 0.04628348 -0.5762718 0.8148645 0.06250298 -0.2272029 0.9703021 0.08302372 -0.2081078 0.9753445 0.07344597 0.2294986 0.9673399 0.1076287 0.1575457 0.9849258 0.07141786 -7.52121e-6 0 1 -1.00305e-5 0 1 5.508e-6 0 1 -0.9305938 0.3660535 -2.65954e-5 -0.9306122 0.3660071 0 -0.4119598 -0.9112021 0 -0.4118431 -0.9112548 6.38372e-5 -0.7823497 -0.6228396 0 -0.7823458 -0.6228443 0 0.5831124 -0.8123915 0 0.5831118 -0.8123919 0 0.1481879 -0.9889566 0.002297818 0.1438094 -0.9896054 0 0.9951053 0.09882056 0 0.9942216 0.1072528 0.004511833 0.9305895 -0.3660645 0 0.9305866 -0.3660718 0 0.41193 0.9112156 0 0.4119324 0.9112145 0 0.7823156 0.6228824 5.83188e-5 0.782379 0.6228027 0 -0.5830914 0.8124065 0 -0.5833902 0.812192 1.96169e-4 -0.1482598 0.9889485 0 -0.1482486 0.9889501 0 -0.9951068 -0.09880483 0 -0.9950988 -0.09888583 3.72071e-5 0.1444615 -0.0505622 0.9882178 -0.02709954 0.00432825 0.9996235 0.2580375 -0.1758949 0.9499883 -0.01106005 -0.0102331 0.9998865 -0.002177774 -0.03247809 0.9994701 -0.08437627 0.2956717 0.951556 0.08343392 -0.1181322 0.9894866 0.02719545 -0.01788032 0.9994703 -0.03942579 -0.01648414 0.9990865 -0.01637977 -0.05049949 0.9985898 -0.534859 -0.5439764 0.6465413 -0.5525277 -0.5017775 0.6655318 -0.32183 -0.05673581 0.9450961 -0.9281767 0.04014742 0.3699678 -0.4751707 0.1122649 0.8727024 -0.3372465 0.01537328 0.9412909 -0.4722998 0.3162317 0.8227579 -0.310862 0.4439883 0.8403805 -0.3009435 0.4441111 0.8439185 0.29405 0.6622816 0.6891427 0.05898606 0.3348459 0.9404249 0.06745237 0.0906195 0.9935987 0.6931364 0.7152783 0.08910107 0.9089774 0.4152989 0.03587418 0.9647012 0.2372013 0.1143992 0.9960823 0.06916642 0.05510056 0.9815343 -0.1720213 0.0836609 0.863904 -0.4998596 0.06172794 0.8291919 -0.5512278 0.09267508 0.6872973 -0.7206069 0.09136897 0.589565 -0.8064509 0.04528069 0.3837611 -0.917726 0.1025008 0.2627614 -0.9630806 0.05858457 0.01005476 -0.9968722 0.07838851 -0.001477241 -0.996425 0.08446913 -0.349856 -0.933565 0.07782894 -0.3653463 -0.9283804 0.06805902 0.6066299 0.7949346 0.008893847 -1.34883e-5 0 -1 -2.76478e-6 0 -1 -2.59515e-6 0 -1 3.48102e-6 0 -1 -3.43293e-6 0 -1 0.08023053 -0.5277936 0.845575 -0.07465684 -0.4837892 0.8719946 0.4205541 0.4943021 0.760789 0.1695575 0.3662204 0.9149497 0.3376066 0.3970097 0.8534666 0.3444445 0.2133392 0.9142454 0.6953664 0.0990535 0.7117964 0.7652086 -0.1684826 0.6213448 0.06559658 -0.02048707 0.997636 0.3329771 -0.3428267 0.8784055 0.3315689 -0.3032509 0.893365 -0.002407848 -0.09520173 -0.9954552 0.06286716 -0.3083755 -0.9491851 0.1333006 0.0460093 -0.9900071 0.06206399 0.03822594 -0.9973399 0.3779112 0.02610141 -0.9254739 0.3981419 -0.2836726 -0.8723606 -0.1189919 0.2592045 -0.9584645 -0.5994946 0.7996811 0.03341573 -0.3425962 -0.939376 -0.01417076 -0.2247898 -0.1055511 -0.9686737 -0.9654649 -0.2600003 0.01665687 -0.3498467 0.1289235 -0.9278934 -0.8800397 0.4725688 0.04699945 -0.5751697 -0.8109205 0.1076462 -0.7530214 -0.6562577 0.04779839 -0.8719465 -0.4787897 0.1023216 -0.9882835 -0.1290439 0.08150732 -0.9663972 0.2398576 0.09243911 -0.9823187 0.1749187 0.06673544 -0.8119479 0.5764713 0.09176945 -0.5576152 0.8274711 0.06600677 -0.2264488 0.969114 0.09766912 -0.1374121 0.9890605 0.05364108 0.1227563 0.9890508 0.08191233 -5.01457e-6 0 1 -5.50884e-6 0 1 1.10169e-5 0 1 -0.9975615 -0.0697934 8.66736e-5 -0.9975721 -0.06964194 0 0.02034592 -0.999793 1.83182e-4 -0.4384913 -0.8987355 0 0.8758047 -0.4826657 0 0.5591707 -0.8290527 6.39517e-6 0.5591657 -0.829056 0 0.8558531 0.5172189 0 0.8557692 0.5173578 8.54328e-5 0.9975608 0.06979113 0.001270771 0.9977315 0.0673204 0 -0.0246858 0.9996922 0.002495646 0.4383803 0.8987869 0.002198994 0.442171 0.8969308 0 -0.8791463 0.4765374 0.003740608 -0.5591818 0.8290451 0 -0.5591701 0.8290529 5.4186e-7 -0.8558423 -0.5172369 -3.1978e-7 -0.8558552 -0.5172155 0 0.1791084 0.1002924 0.9787041 0.02187258 -0.01255482 0.999682 0.05073559 0.03336626 0.9981546 -0.01438391 0.01560616 0.9997748 0.02422171 0.04963713 0.9984736 -0.07440334 0.1062316 0.9915538 -0.04755991 0.05210942 0.9975083 -0.02101367 0.01157897 0.9997122 -0.03315144 0.06164562 0.9975474 0.07898187 0.2860649 0.9549496 -0.01280254 0.6953052 0.7186005 -0.08829617 -0.4665734 0.8800643 -0.3090947 -0.472041 0.8256136 -0.4853582 -0.1896346 0.8535023 -0.04692417 0.03001284 0.9984475 -0.4892008 -0.2141836 0.8454632 -0.6520776 -0.1213212 0.7483824 -0.7655798 0.1945216 0.6132283 -0.5456895 0.4259533 0.7216555 -0.411239 0.4350698 0.8009975 -0.3978055 0.5470519 0.7365359 -0.1910738 0.7425484 0.64196 0.4868363 0.8662078 0.1125813 0.7323575 0.6804916 0.02416211 0.8279649 0.5533273 0.0911206 0.9603085 0.2615867 0.09685045 0.9272851 0.3686305 0.06522208 0.9902631 0.1282373 0.0541684 0.9648823 -0.238298 0.1105278 0.9814265 -0.1752169 0.07811015 0.9087888 -0.4135356 0.05559986 0.6927989 -0.7136944 0.1032967 0.7251501 -0.6841953 0.07768028 0.4284732 -0.9025256 0.04310929 0.2492629 -0.9625275 0.1068133 0.1006927 -0.9932621 0.0573703 -0.1487375 -0.9851946 0.08525663 0.2892508 0.9569668 0.02342379 3.22495e-6 0 -1 -2.74337e-6 0 -1 1.55846e-5 0 -1 -2.52239e-6 0 -1 -6.05304e-6 0 -1 -0.9296516 0.3677065 0.02323615 0.08640176 -0.033692 0.9956906 -0.5182926 0.7792347 0.3523723 -0.5342555 0.7675244 0.354228 -0.1357976 0.7844299 0.6051684 -0.01650893 0.06375652 0.997829 -0.1258289 0.7738922 0.6206915 0.5142362 0.8323601 0.2067316 0.3553647 0.6675885 0.6542488 0.1006037 0.1220095 0.9874172 0.3118367 0.3717016 0.8744118 0.5983116 -0.05845218 0.7991288 0.5748617 -0.07305258 0.8149832 0.131864 -0.03038883 0.9908019 0.6831561 0.1986192 0.7027432 0.3421095 0.1291705 -0.9307396 0.1017851 0.00458008 -0.9947959 0.06472361 -0.007315039 -0.9978765 -0.253313 0.9252034 -0.2825442 -0.4461953 0.8906036 -0.08794957 0.09094977 0.1995513 -0.9756575 0.5251906 0.8418004 -0.1246883 -0.1203082 5.6427e-4 -0.9927365 -0.135389 -0.003410577 -0.9907867 0.06303334 -0.335712 -0.9398534 -0.1962265 -0.3089494 -0.9306156 0.8790711 -0.4691959 0.08419829 0.6380345 -0.7652038 0.08587974 0.6773587 -0.7330599 0.0617122 0.2993882 -0.9497868 0.09094989 0.361932 -0.9302428 0.06044495 -0.08595269 -0.9919641 0.09284132 -0.002497911 -0.9983853 0.05675113 -0.4554128 -0.885132 0.09560585 -0.369773 -0.9275165 0.05460137 -0.7550972 -0.6486515 0.09528708 -0.6824232 -0.7290662 0.05254703 -0.9440878 -0.3144636 0.0990507 -0.9026219 -0.4269086 0.05498105 -0.9912936 0.080913 0.1038761 -0.9961266 -0.06817448 0.05553466 -1.00304e-5 0 1 1.00274e-5 0 1 5.01439e-6 0 1 1.10172e-5 0 1 -5.50823e-6 0 1 -0.09147149 -0.9958078 0 -0.09147053 -0.9958078 0 0.9900012 -0.1410598 0 0.990002 -0.1410535 0 0.8167482 -0.5769943 0 0.8167502 -0.5769916 0 0.6172056 0.7868019 0 0.6172024 0.7868044 0 0.9081082 0.4187347 9.81095e-4 0.9088948 0.4170254 0 -0.3729212 0.9278631 0 -0.3753579 0.9268789 0.001406252 0.09147751 0.9958071 -1.06453e-5 0.09146422 0.9958084 0 -0.9900012 0.1410598 0 -0.9900223 0.1409109 7.58444e-5 -0.8166679 0.5771079 0 -0.8166635 0.5771143 0 -0.6171686 -0.7868309 0 -0.6171715 -0.7868287 0 -0.9081156 -0.4187195 9.26055e-5 -0.908192 -0.418554 0 0.3730368 -0.9278166 0 0.3733893 -0.9276748 2.06376e-4 -0.006669759 -0.0215193 0.9997462 0.004809558 0.02715963 0.9996196 -0.03720873 0.05176311 0.997966 -0.1459544 0.01848274 0.9891187 -0.03053319 0.02439796 0.999236 0.02240514 -0.006438076 0.9997283 0.06109988 0.07417011 0.9953722 0.02404141 0.03630989 0.9990514 -0.01598185 0.006994605 0.9998479 0.05385285 -0.02087181 0.9983308 0.6541335 -0.3558489 0.6674436 0.4798342 -0.5172938 0.7086369 0.2645 -0.4743644 0.8396536 0.1781248 -0.6452123 0.7429487 0.09813302 -0.1439707 0.9847043 -0.06731051 -0.6772511 0.7326666 -0.2331834 -0.4872387 0.8415606 -0.2505494 -0.4834951 0.8387238 -0.3096876 -0.3956539 0.8646108 -0.2980397 -0.1902996 0.9353922 -0.3178303 -0.2264964 0.9206972 -0.7948838 0.2068059 0.5704306 -0.3506702 0.08821892 0.9323347 -0.4303326 0.1434738 0.8911954 -0.8975896 0.4361465 0.06410336 -0.7323009 0.67438 0.09458935 -0.07656866 0.9912543 0.1074805 0.1607342 0.9860417 0.04343104 0.3928509 0.9160244 0.08104151 0.6646184 0.7426977 0.08174681 0.824948 0.5632025 0.04758125 0.9313687 0.3477721 0.1077356 0.9546014 0.2876538 0.07740515 0.9913989 -0.08085423 0.1029111 0.9973778 0.04203832 0.0589115 0.9295766 -0.3676639 0.02665686 6.45611e-4 -6.83167e-4 -0.9999997 5.81378e-6 0 -1 -9.42099e-6 0 -1 6.73143e-4 -2.62676e-4 -0.9999998 3.28826e-6 0 -1 7.20223e-4 -2.55075e-4 -0.9999997 4.00016e-6 0 -1 -9.86855e-7 0 -1 6.29855e-4 -6.88571e-4 -0.9999996

53 0 39 0 41 0 3 1 2 1 47 1 3 2 50 2 2 2 6 3 50 3 3 3 4 4 50 4 6 4 9 5 5 5 4 5 9 6 4 6 6 6 9 7 51 7 5 7 9 8 6 8 7 8 9 9 7 9 39 9 39 10 8 10 51 10 39 11 51 11 9 11 53 12 8 12 39 12 41 13 39 13 37 13 56 14 10 14 14 14 1 15 15 15 52 15 54 16 15 16 1 16 49 17 56 17 2 17 49 18 10 18 56 18 11 19 12 19 10 19 0 20 12 20 11 20 15 21 54 21 41 21 20 22 14 22 17 22 18 23 14 23 20 23 55 24 14 24 18 24 15 25 41 25 42 25 16 26 15 26 42 26 16 27 42 27 19 27 17 28 19 28 43 28 17 29 16 29 19 29 17 30 43 30 45 30 20 31 17 31 45 31 18 32 45 32 55 32 18 33 20 33 45 33 14 34 55 34 47 34 56 35 14 35 47 35 2 36 56 36 47 36 30 37 32 37 33 37 22 38 33 38 34 38 24 39 31 39 30 39 23 40 22 40 21 40 28 38 27 38 24 38 25 38 28 38 23 38 30 41 33 41 22 41 30 42 22 42 23 42 30 43 28 43 24 43 30 38 23 38 28 38 3 44 21 44 22 44 35 45 3 45 22 45 38 46 23 46 21 46 3 47 38 47 21 47 46 48 25 48 23 48 38 49 46 49 23 49 26 50 28 50 25 50 46 51 26 51 25 51 44 52 27 52 28 52 26 53 44 53 28 53 29 54 24 54 27 54 44 55 29 55 27 55 37 56 31 56 24 56 29 57 37 57 24 57 36 58 30 58 31 58 37 59 36 59 31 59 39 60 32 60 30 60 36 61 39 61 30 61 40 62 33 62 32 62 39 63 40 63 32 63 7 64 34 64 33 64 40 65 7 65 33 65 35 66 22 66 34 66 7 67 35 67 34 67 37 68 29 68 44 68 6 69 3 69 35 69 7 70 6 70 35 70 38 71 3 71 46 71 39 72 7 72 40 72 26 73 46 73 44 73 39 74 36 74 37 74 37 75 42 75 41 75 42 76 37 76 44 76 48 77 43 77 19 77 48 78 19 78 42 78 48 79 45 79 43 79 44 80 48 80 42 80 48 81 44 81 46 81 47 82 55 82 45 82 47 83 45 83 48 83 47 84 48 84 46 84 47 85 46 85 3 85 49 86 2 86 50 86 10 87 50 87 4 87 10 88 49 88 50 88 11 89 10 89 4 89 11 90 4 90 5 90 0 91 11 91 5 91 0 92 5 92 51 92 12 93 0 93 51 93 12 94 51 94 8 94 52 95 12 95 8 95 52 96 8 96 53 96 1 97 52 97 53 97 54 98 1 98 53 98 41 99 54 99 53 99 59 100 57 100 10 100 13 101 16 101 17 101 12 100 52 100 60 100 52 102 15 102 60 102 10 103 57 103 14 103 58 100 13 100 17 100 60 104 15 104 13 104 12 105 60 105 59 105 58 106 17 106 14 106 10 107 12 107 59 107 57 100 58 100 14 100 15 108 16 108 13 108 105 109 63 109 66 109 64 110 61 110 63 110 63 111 105 111 64 111 104 112 62 112 64 112 61 113 64 113 62 113 105 114 66 114 106 114 66 115 95 115 93 115 106 116 66 116 93 116 104 117 97 117 89 117 62 118 104 118 89 118 62 119 89 119 65 119 61 120 62 120 65 120 61 121 65 121 88 121 63 122 61 122 88 122 63 123 88 123 95 123 66 124 63 124 95 124 70 38 73 38 74 38 71 125 74 125 75 125 67 38 72 38 70 38 69 38 71 38 78 38 82 38 67 38 68 38 80 38 68 38 69 38 70 38 74 38 71 38 70 38 71 38 69 38 70 38 69 38 68 38 70 38 68 38 67 38 81 126 90 126 72 126 81 127 72 127 67 127 90 128 86 128 70 128 90 128 70 128 72 128 85 129 73 129 70 129 86 129 85 129 70 129 94 130 74 130 73 130 85 131 94 131 73 131 91 132 75 132 74 132 94 133 91 133 74 133 91 134 77 134 71 134 91 135 71 135 75 135 77 136 76 136 78 136 77 137 78 137 71 137 79 138 69 138 78 138 76 138 79 138 78 138 79 139 92 139 80 139 79 139 80 139 69 139 83 140 68 140 80 140 92 141 83 141 80 141 87 142 82 142 68 142 83 143 87 143 68 143 81 144 67 144 82 144 87 145 81 145 82 145 84 38 85 38 97 38 83 146 88 146 87 146 85 38 86 38 97 38 87 38 88 38 65 38 97 147 86 147 89 147 86 38 90 38 89 38 96 38 93 38 76 38 81 148 87 148 65 148 90 38 81 38 65 38 84 149 96 149 91 149 89 150 90 150 65 150 96 38 76 38 77 38 91 38 96 38 77 38 76 38 93 38 79 38 93 38 95 38 92 38 79 38 93 38 92 38 84 151 91 151 94 151 84 38 94 38 85 38 83 152 92 152 88 152 92 38 95 38 88 38 104 153 98 153 97 153 106 154 93 154 96 154 102 155 106 155 96 155 99 156 102 156 96 156 99 157 96 157 84 157 98 158 99 158 84 158 98 159 84 159 97 159 100 160 99 160 98 160 102 161 99 161 101 161 101 162 99 162 100 162 102 163 101 163 103 163 103 164 106 164 102 164 105 165 106 165 103 165 100 166 98 166 104 166 143 167 154 167 138 167 148 168 149 168 161 168 110 169 149 169 148 169 110 170 108 170 149 170 110 171 148 171 136 171 151 172 108 172 110 172 109 173 151 173 110 173 109 174 111 174 151 174 138 175 111 175 109 175 152 176 111 176 138 176 112 177 152 177 138 177 154 178 112 178 138 178 153 179 163 179 113 179 153 180 155 180 163 180 116 181 107 181 108 181 116 182 150 182 107 182 115 183 150 183 116 183 150 184 115 184 151 184 163 185 150 185 117 185 160 186 107 186 159 186 114 187 107 187 160 187 118 188 155 188 156 188 118 189 157 189 155 189 119 190 157 190 118 190 158 191 159 191 157 191 155 192 154 192 120 192 156 193 120 193 142 193 156 194 155 194 120 194 118 195 156 195 142 195 119 196 118 196 144 196 157 197 144 197 145 197 157 198 119 198 144 198 158 199 157 199 145 199 158 200 145 200 147 200 159 201 158 201 147 201 160 202 159 202 147 202 160 203 147 203 161 203 114 204 160 204 161 204 126 205 134 205 125 205 121 206 125 206 135 206 124 38 132 38 126 38 128 207 121 207 122 207 129 38 130 38 124 38 123 208 129 208 128 208 124 209 126 209 125 209 124 210 125 210 121 210 124 211 121 211 128 211 124 38 128 38 129 38 136 212 122 212 121 212 127 213 136 213 121 213 137 214 128 214 122 214 136 215 137 215 122 215 146 216 123 216 128 216 137 217 146 217 128 217 139 218 129 218 123 218 146 219 139 219 123 219 141 220 130 220 129 220 139 221 141 221 129 221 131 222 124 222 130 222 141 223 131 223 130 223 143 224 132 224 124 224 131 225 143 225 124 225 140 226 126 226 132 226 143 227 140 227 132 227 138 228 134 228 126 228 140 229 138 229 126 229 133 230 125 230 134 230 138 231 133 231 134 231 109 232 135 232 125 232 133 233 109 233 125 233 127 234 121 234 135 234 109 235 127 235 135 235 143 236 131 236 141 236 110 237 136 237 127 237 109 238 110 238 127 238 136 239 146 239 137 239 138 240 109 240 133 240 139 241 146 241 141 241 138 242 140 242 143 242 136 243 148 243 146 243 143 244 120 244 154 244 143 245 142 245 120 245 144 246 118 246 142 246 144 247 142 247 143 247 141 248 144 248 143 248 145 249 144 249 141 249 146 250 145 250 141 250 148 251 147 251 145 251 148 252 145 252 146 252 161 253 147 253 148 253 114 254 161 254 149 254 107 255 114 255 149 255 107 256 149 256 108 256 116 257 108 257 151 257 115 258 116 258 151 258 150 259 151 259 111 259 117 260 111 260 152 260 117 261 150 261 111 261 163 262 152 262 112 262 163 263 117 263 152 263 113 264 163 264 112 264 153 265 112 265 154 265 153 266 113 266 112 266 155 267 153 267 154 267 164 100 162 100 157 100 150 100 163 100 167 100 164 268 157 268 159 268 167 269 163 269 162 269 163 270 155 270 162 270 165 100 164 100 159 100 107 100 165 100 159 100 150 271 167 271 166 271 150 272 166 272 107 272 107 100 166 100 165 100 162 100 155 100 157 100 217 273 219 273 174 273 204 274 211 274 226 274 204 275 168 275 201 275 204 276 212 276 211 276 170 277 204 277 201 277 170 278 212 278 204 278 170 279 169 279 212 279 214 280 169 280 170 280 170 281 171 281 214 281 170 282 172 282 171 282 205 283 172 283 170 283 205 284 216 284 172 284 205 285 217 285 216 285 173 286 174 286 215 286 214 287 215 287 213 287 214 288 213 288 169 288 225 289 176 289 224 289 225 290 224 290 178 290 225 291 175 291 176 291 220 292 221 292 219 292 220 293 219 293 218 293 222 294 224 294 221 294 223 295 224 295 222 295 223 296 222 296 209 296 219 297 217 297 218 297 220 298 218 298 179 298 221 299 179 299 206 299 221 300 220 300 179 300 222 301 206 301 209 301 222 302 221 302 206 302 223 303 209 303 210 303 224 304 223 304 210 304 224 305 210 305 180 305 178 306 224 306 180 306 225 307 180 307 181 307 225 308 178 308 180 308 226 309 225 309 181 309 182 38 196 38 183 38 186 38 183 38 198 38 194 310 195 310 182 310 189 38 186 38 187 38 184 38 192 38 194 38 190 311 184 311 189 311 182 38 183 38 186 38 182 312 186 312 189 312 182 313 184 313 194 313 182 314 189 314 184 314 168 315 187 315 186 315 185 316 168 316 186 316 200 317 189 317 187 317 168 318 200 318 187 318 188 319 190 319 189 319 200 320 188 320 189 320 191 321 184 321 190 321 188 322 191 322 190 322 208 323 192 323 184 323 191 324 208 324 184 324 193 325 194 325 192 325 208 326 193 326 192 326 199 327 195 327 194 327 193 328 199 328 194 328 197 329 182 329 195 329 199 330 197 330 195 330 202 331 196 331 182 331 197 331 202 331 182 331 203 332 183 332 196 332 202 333 203 333 196 333 201 334 198 334 183 334 203 335 201 335 183 335 185 336 186 336 198 336 201 337 185 337 198 337 205 338 197 338 199 338 193 339 208 339 199 339 201 340 168 340 185 340 168 341 204 341 200 341 200 342 204 342 188 342 170 343 201 343 203 343 205 344 170 344 202 344 170 345 203 345 202 345 205 346 202 346 197 346 191 347 188 347 208 347 204 348 226 348 181 348 204 349 180 349 188 349 218 350 217 350 205 350 207 351 179 351 218 351 207 352 218 352 205 352 207 353 205 353 199 353 206 354 179 354 207 354 207 355 199 355 208 355 209 356 206 356 207 356 210 357 209 357 207 357 210 358 208 358 188 358 210 359 207 359 208 359 188 360 180 360 210 360 204 361 181 361 180 361 175 362 226 362 211 362 176 363 175 363 211 363 176 364 211 364 212 364 213 365 176 365 212 365 213 366 212 366 169 366 215 367 214 367 171 367 173 368 171 368 172 368 173 369 215 369 171 369 174 370 173 370 172 370 174 371 172 371 216 371 217 372 174 372 216 372 226 373 175 373 225 373 213 374 230 374 229 374 213 100 229 100 176 100 228 375 227 375 221 375 215 376 174 376 231 376 215 377 231 377 230 377 174 378 219 378 231 378 213 100 215 100 230 100 229 379 177 379 176 379 228 100 221 100 224 100 176 380 177 380 224 380 231 100 219 100 227 100 177 100 228 100 224 100 219 381 221 381 227 381 234 382 280 382 235 382 282 383 234 383 235 383 238 384 282 384 235 384 238 385 236 385 237 385 238 386 235 386 236 386 269 387 285 387 238 387 287 388 285 388 269 388 269 389 289 389 287 389 241 390 289 390 269 390 275 391 239 391 289 391 275 392 289 392 241 392 240 393 239 393 275 393 247 394 233 394 242 394 283 395 284 395 281 395 232 396 284 396 283 396 284 397 232 397 238 397 286 398 284 398 288 398 286 399 233 399 284 399 246 400 281 400 249 400 246 401 249 401 293 401 247 402 248 402 233 402 290 403 248 403 291 403 251 404 292 404 248 404 290 405 251 405 248 405 276 406 249 406 292 406 248 407 240 407 250 407 248 408 247 408 240 408 291 409 250 409 274 409 291 410 248 410 250 410 291 411 274 411 290 411 251 412 290 412 252 412 292 413 252 413 276 413 292 414 251 414 252 414 249 415 276 415 253 415 249 416 253 416 279 416 293 417 249 417 279 417 246 418 279 418 280 418 246 419 293 419 279 419 262 38 263 38 264 38 255 420 264 420 265 420 260 38 261 38 262 38 254 421 255 421 257 421 256 38 259 38 260 38 258 38 256 38 254 38 264 38 260 38 262 38 264 38 255 38 254 38 264 38 256 38 260 38 264 422 254 422 256 422 236 423 257 423 255 423 266 424 236 424 255 424 268 425 254 425 257 425 236 426 268 426 257 426 270 427 258 427 254 427 268 428 270 428 254 428 272 429 256 429 258 429 270 430 272 430 258 430 278 431 259 431 256 431 272 432 278 432 256 432 267 433 260 433 259 433 278 434 267 434 259 434 277 435 261 435 260 435 267 436 277 436 260 436 273 437 262 437 261 437 277 438 273 438 261 438 241 439 263 439 262 439 273 440 241 440 262 440 271 441 264 441 263 441 241 442 271 442 263 442 237 443 265 443 264 443 271 444 237 444 264 444 266 445 255 445 265 445 237 446 266 446 265 446 267 447 278 447 277 447 237 448 236 448 266 448 238 449 237 449 269 449 268 450 236 450 270 450 269 451 237 451 271 451 270 452 279 452 278 452 270 453 278 453 272 453 269 454 271 454 241 454 241 455 273 455 275 455 275 456 273 456 277 456 275 457 250 457 240 457 274 458 250 458 275 458 274 459 275 459 277 459 252 460 290 460 274 460 278 461 252 461 274 461 278 462 274 462 277 462 278 463 276 463 252 463 278 464 253 464 276 464 279 465 253 465 278 465 235 466 280 466 279 466 235 467 279 467 270 467 235 468 270 468 236 468 281 469 280 469 234 469 283 470 281 470 234 470 283 471 234 471 282 471 232 472 283 472 282 472 238 473 232 473 282 473 288 474 238 474 285 474 288 475 284 475 238 475 288 476 285 476 287 476 286 477 288 477 287 477 286 478 287 478 289 478 233 479 286 479 289 479 242 480 289 480 239 480 242 481 233 481 289 481 242 482 239 482 240 482 247 483 242 483 240 483 280 484 281 484 246 484 244 485 248 485 292 485 284 486 296 486 281 486 294 100 244 100 292 100 233 487 243 487 295 487 284 100 233 100 295 100 233 488 248 488 243 488 296 489 245 489 281 489 294 100 292 100 249 100 281 100 245 100 249 100 245 100 294 100 249 100 243 100 248 100 244 100 284 100 295 100 296 100 339 490 349 490 305 490 339 491 342 491 349 491 346 492 300 492 319 492 301 493 319 493 341 493 301 494 346 494 319 494 304 495 346 495 301 495 304 496 302 496 346 496 304 497 348 497 302 497 304 498 303 498 339 498 339 499 348 499 304 499 305 500 348 500 339 500 308 501 313 501 299 501 308 502 299 502 306 502 297 503 310 503 307 503 347 504 310 504 297 504 309 505 310 505 347 505 299 506 310 506 298 506 354 507 359 507 315 507 354 508 315 508 355 508 313 509 308 509 342 509 350 510 352 510 313 510 352 511 350 511 351 511 314 512 315 512 352 512 315 513 314 513 353 513 313 514 342 514 343 514 350 515 313 515 343 515 350 516 343 516 351 516 352 517 351 517 316 517 314 518 316 518 353 518 314 519 352 519 316 519 315 520 353 520 355 520 354 521 355 521 317 521 354 522 317 522 318 522 359 523 354 523 318 523 319 524 359 524 318 524 328 38 330 38 320 38 325 38 320 38 332 38 327 525 329 525 328 525 323 38 325 38 321 38 324 38 326 38 327 38 322 311 324 311 323 311 328 38 320 38 325 38 328 526 325 526 323 526 328 527 324 527 327 527 328 314 323 314 324 314 301 528 321 528 325 528 334 529 301 529 325 529 335 317 323 317 321 317 301 530 335 530 321 530 336 531 322 531 323 531 335 531 336 531 323 531 338 321 324 321 322 321 336 532 338 532 322 532 340 533 326 533 324 533 338 534 340 534 324 534 333 535 327 535 326 535 340 536 333 536 326 536 344 537 329 537 327 537 333 538 344 538 327 538 331 329 328 329 329 329 344 539 331 539 329 539 339 540 330 540 328 540 331 541 339 541 328 541 337 332 320 332 330 332 339 542 337 542 330 542 303 543 332 543 320 543 337 544 303 544 320 544 334 545 325 545 332 545 303 546 334 546 332 546 344 547 333 547 340 547 304 548 301 548 334 548 303 549 304 549 334 549 301 550 341 550 335 550 335 551 341 551 336 551 339 552 303 552 337 552 336 553 341 553 345 553 336 554 345 554 338 554 338 555 345 555 340 555 339 556 331 556 344 556 341 557 319 557 318 557 342 558 339 558 344 558 343 559 342 559 344 559 344 560 351 560 343 560 345 561 344 561 340 561 345 562 351 562 344 562 345 563 316 563 351 563 345 564 353 564 316 564 345 565 355 565 353 565 341 566 355 566 345 566 317 567 355 567 341 567 318 568 317 568 341 568 307 569 319 569 300 569 297 570 307 570 300 570 297 571 300 571 346 571 347 572 346 572 302 572 347 573 297 573 346 573 309 574 347 574 302 574 310 575 302 575 348 575 310 576 309 576 302 576 298 577 310 577 348 577 299 578 348 578 305 578 299 579 298 579 348 579 306 580 299 580 305 580 306 581 305 581 349 581 308 582 306 582 349 582 342 583 308 583 349 583 319 584 307 584 359 584 357 100 313 100 352 100 311 100 357 100 352 100 356 585 312 585 307 585 307 100 312 100 359 100 310 100 299 100 358 100 299 586 313 586 357 586 358 100 299 100 357 100 311 587 352 587 315 587 312 100 311 100 315 100 359 588 312 588 315 588 310 100 358 100 356 100 310 589 356 589 307 589 367 590 419 590 404 590 396 591 397 591 392 591 361 592 406 592 405 592 361 593 360 593 406 593 361 594 408 594 360 594 362 595 361 595 389 595 362 596 408 596 361 596 364 597 410 597 408 597 364 598 408 598 362 598 365 599 362 599 363 599 365 600 364 600 362 600 395 601 366 601 365 601 397 602 395 602 365 602 397 603 365 603 392 603 366 604 364 604 365 604 413 605 368 605 411 605 412 606 414 606 368 606 412 607 368 607 413 607 360 608 367 608 406 608 407 609 367 609 360 609 409 610 368 610 407 610 410 611 368 611 409 611 419 612 367 612 418 612 419 613 418 613 373 613 416 614 369 614 415 614 417 615 418 615 369 615 414 616 395 616 370 616 415 617 370 617 398 617 415 618 414 618 370 618 416 619 398 619 371 619 416 620 415 620 398 620 369 621 371 621 372 621 369 622 416 622 371 622 417 623 372 623 400 623 417 624 369 624 372 624 418 625 400 625 401 625 418 626 417 626 400 626 373 627 401 627 403 627 373 628 418 628 401 628 419 629 403 629 404 629 419 630 373 630 403 630 387 631 374 631 378 631 379 632 378 632 375 632 380 38 386 38 387 38 377 38 379 38 383 38 385 633 376 633 380 633 384 38 385 38 377 38 380 634 387 634 378 634 380 38 378 38 379 38 380 635 379 635 377 635 380 38 377 38 385 38 389 636 383 636 379 636 381 637 389 637 379 637 382 638 377 638 383 638 389 639 382 639 383 639 402 640 384 640 377 640 382 641 402 641 377 641 391 642 385 642 384 642 402 643 391 643 384 643 399 644 376 644 385 644 391 645 399 645 385 645 388 646 380 646 376 646 399 647 388 647 376 647 396 648 386 648 380 648 388 649 396 649 380 649 393 650 387 650 386 650 396 651 393 651 386 651 392 652 374 652 387 652 393 653 392 653 387 653 390 654 378 654 374 654 392 655 390 655 374 655 363 656 375 656 378 656 390 657 363 657 378 657 381 658 379 658 375 658 363 659 381 659 375 659 388 660 394 660 396 660 362 661 389 661 381 661 362 662 381 662 363 662 389 663 361 663 382 663 382 664 361 664 402 664 365 665 363 665 390 665 402 666 399 666 391 666 365 667 390 667 392 667 392 668 393 668 396 668 388 669 399 669 394 669 397 670 370 670 395 670 397 671 398 671 370 671 394 672 398 672 397 672 394 673 371 673 398 673 397 674 396 674 394 674 394 675 372 675 371 675 399 676 372 676 394 676 399 677 400 677 372 677 401 678 400 678 399 678 403 679 401 679 399 679 403 680 399 680 402 680 403 681 405 681 404 681 361 682 403 682 402 682 361 683 405 683 403 683 367 684 404 684 405 684 367 685 405 685 406 685 407 686 360 686 408 686 409 687 407 687 408 687 410 688 409 688 408 688 368 689 410 689 364 689 411 690 368 690 364 690 411 691 364 691 366 691 413 692 411 692 366 692 412 693 366 693 395 693 412 694 413 694 366 694 414 695 412 695 395 695 420 696 415 696 369 696 422 697 420 697 369 697 424 100 423 100 367 100 368 698 421 698 425 698 368 699 414 699 421 699 422 100 369 100 418 100 407 700 368 700 425 700 367 100 423 100 418 100 421 701 414 701 420 701 423 702 422 702 418 702 407 703 425 703 424 703 414 704 415 704 420 704 367 100 407 100 424 100

 0.6859207 -0.3240135 0.6515582 7.481132 0.7276763 0.3054208 -0.6141704 -6.50764 0 0.8953956 0.4452714 5.343665 0 0 0 1 -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 0.6859207 -0.3240134 0.6515582 7.481132 0.7276763 0.3054208 -0.6141704 -6.50764 -4.01133e-9 0.8953956 0.4452714 5.343665 0 0 0 1 -0.2908646 -0.7711008 0.5663933 4.076245 0.9551712 -0.1998833 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ================================================ FILE: test/fcl_resources/rob.obj ================================================ 648 216 v -500 -395 375 v 500 -395 375 v 462 -203.6 375 v -500 -395 375 v 462 -203.6 375 v 353.5 -41.5 375 v -500 -395 375 v 353.5 -41.5 375 v 191.4 66.4 375 v -500 -395 375 v 191.4 66.4 375 v -500 66.4 375 v -500 66.4 75 v 191.4 66.4 75 v 353.5 -41.5 75 v -500 66.4 75 v 353.5 -41.5 75 v 462 -203.6 75 v -500 66.4 75 v 462 -203.6 75 v 500 -395 75 v -500 66.4 75 v 500 -395 75 v -500 -395 75 v -500 -395 375 v -500 -395 75 v 500 -395 75 v -500 -395 375 v 500 -395 75 v 500 -395 375 v 500 -395 375 v 500 -395 75 v 462 -203.6 75 v 500 -395 375 v 462 -203.6 75 v 462 -203.6 375 v 462 -203.6 375 v 462 -203.6 75 v 353.5 -41.5 75 v 462 -203.6 375 v 353.5 -41.5 75 v 353.5 -41.5 375 v 353.5 -41.5 375 v 353.5 -41.5 75 v 191.4 66.4 75 v 353.5 -41.5 375 v 191.4 66.4 75 v 191.4 66.4 375 v 191.4 66.4 375 v 191.4 66.4 75 v -500 66.4 75 v 191.4 66.4 375 v -500 66.4 75 v -500 66.4 375 v -500 66.4 375 v -500 66.4 75 v -500 -395 75 v -500 66.4 375 v -500 -395 75 v -500 -395 375 v -500 66.4 375 v 191.4 66.4 375 v -38 296.4 375 v -500 66.4 375 v -38 296.4 375 v -500 296.4 375 v -500 296.4 75 v -38 296.4 75 v 191.4 66.4 75 v -500 296.4 75 v 191.4 66.4 75 v -500 66.4 75 v -500 66.4 375 v -500 66.4 75 v 191.4 66.4 75 v -500 66.4 375 v 191.4 66.4 75 v 191.4 66.4 375 v 191.4 66.4 375 v 191.4 66.4 75 v -38 296.4 75 v 191.4 66.4 375 v -38 296.4 75 v -38 296.4 375 v -38 296.4 375 v -38 296.4 75 v -500 296.4 75 v -38 296.4 375 v -500 296.4 75 v -500 296.4 375 v -500 296.4 375 v -500 296.4 75 v -500 66.4 75 v -500 296.4 375 v -500 66.4 75 v -500 66.4 375 v -500 296.4 375 v -38 296.4 375 v -146.5 458.6 375 v -500 296.4 375 v -146.5 458.6 375 v -308.7 566.4 375 v -500 296.4 375 v -308.7 566.4 375 v -500 605 375 v -500 605 75 v -308.7 566.4 75 v -146.5 458.6 75 v -500 605 75 v -146.5 458.6 75 v -38 296.4 75 v -500 605 75 v -38 296.4 75 v -500 296.4 75 v -500 296.4 375 v -500 296.4 75 v -38 296.4 75 v -500 296.4 375 v -38 296.4 75 v -38 296.4 375 v -38 296.4 375 v -38 296.4 75 v -146.5 458.6 75 v -38 296.4 375 v -146.5 458.6 75 v -146.5 458.6 375 v -146.5 458.6 375 v -146.5 458.6 75 v -308.7 566.4 75 v -146.5 458.6 375 v -308.7 566.4 75 v -308.7 566.4 375 v -308.7 566.4 375 v -308.7 566.4 75 v -500 605 75 v -308.7 566.4 375 v -500 605 75 v -500 605 375 v -500 605 375 v -500 605 75 v -500 296.4 75 v -500 605 375 v -500 296.4 75 v -500 296.4 375 v -500 -595 175 v 500 -595 175 v -500 -395 175 v -500 -395 175 v 500 -595 175 v 500 -395 175 v 500 -595 75 v -500 -595 75 v 500 -395 75 v 500 -395 75 v -500 -595 75 v -500 -395 75 v 500 -595 175 v 500 -595 75 v 500 -395 175 v 500 -395 175 v 500 -595 75 v 500 -395 75 v -500 -595 75 v -500 -595 175 v -500 -395 75 v -500 -395 75 v -500 -595 175 v -500 -395 175 v -500 -395 75 v -500 -395 175 v 500 -395 75 v 500 -395 75 v -500 -395 175 v 500 -395 175 v -500 -595 75 v 500 -595 75 v -500 -595 175 v -500 -595 175 v 500 -595 75 v 500 -595 175 v -500 -605 195 v 500 -605 195 v -500 -395 195 v -500 -395 195 v 500 -605 195 v 500 -395 195 v 500 -605 175 v -500 -605 175 v 500 -395 175 v 500 -395 175 v -500 -605 175 v -500 -395 175 v 500 -605 195 v 500 -605 175 v 500 -395 195 v 500 -395 195 v 500 -605 175 v 500 -395 175 v -500 -605 175 v -500 -605 195 v -500 -395 175 v -500 -395 175 v -500 -605 195 v -500 -395 195 v -500 -395 175 v -500 -395 195 v 500 -395 175 v 500 -395 175 v -500 -395 195 v 500 -395 195 v -500 -605 175 v 500 -605 175 v -500 -605 195 v -500 -605 195 v 500 -605 175 v 500 -605 195 v -430 -495 75 v -431.522 -487.346 75 v -430 -495 -425 v -430 -495 -425 v -431.522 -487.346 75 v -431.522 -487.346 -425 v -431.522 -487.346 75 v -435.858 -480.858 75 v -431.522 -487.346 -425 v -431.522 -487.346 -425 v -435.858 -480.858 75 v -435.858 -480.858 -425 v -435.858 -480.858 75 v -442.346 -476.522 75 v -435.858 -480.858 -425 v -435.858 -480.858 -425 v -442.346 -476.522 75 v -442.346 -476.522 -425 v -442.346 -476.522 75 v -450 -475 75 v -442.346 -476.522 -425 v -442.346 -476.522 -425 v -450 -475 75 v -450 -475 -425 v -450 -475 75 v -457.654 -476.522 75 v -450 -475 -425 v -450 -475 -425 v -457.654 -476.522 75 v -457.654 -476.522 -425 v -457.654 -476.522 75 v -464.142 -480.858 75 v -457.654 -476.522 -425 v -457.654 -476.522 -425 v -464.142 -480.858 75 v -464.142 -480.858 -425 v -464.142 -480.858 75 v -468.478 -487.346 75 v -464.142 -480.858 -425 v -464.142 -480.858 -425 v -468.478 -487.346 75 v -468.478 -487.346 -425 v -468.478 -487.346 75 v -470 -495 75 v -468.478 -487.346 -425 v -468.478 -487.346 -425 v -470 -495 75 v -470 -495 -425 v -470 -495 75 v -468.478 -502.654 75 v -470 -495 -425 v -470 -495 -425 v -468.478 -502.654 75 v -468.478 -502.654 -425 v -468.478 -502.654 75 v -464.142 -509.142 75 v -468.478 -502.654 -425 v -468.478 -502.654 -425 v -464.142 -509.142 75 v -464.142 -509.142 -425 v -464.142 -509.142 75 v -457.654 -513.478 75 v -464.142 -509.142 -425 v -464.142 -509.142 -425 v -457.654 -513.478 75 v -457.654 -513.478 -425 v -457.654 -513.478 75 v -450 -515 75 v -457.654 -513.478 -425 v -457.654 -513.478 -425 v -450 -515 75 v -450 -515 -425 v -450 -515 75 v -442.346 -513.478 75 v -450 -515 -425 v -450 -515 -425 v -442.346 -513.478 75 v -442.346 -513.478 -425 v -442.346 -513.478 75 v -435.858 -509.142 75 v -442.346 -513.478 -425 v -442.346 -513.478 -425 v -435.858 -509.142 75 v -435.858 -509.142 -425 v -435.858 -509.142 75 v -431.522 -502.654 75 v -435.858 -509.142 -425 v -435.858 -509.142 -425 v -431.522 -502.654 75 v -431.522 -502.654 -425 v -431.522 -502.654 75 v -430 -495 75 v -431.522 -502.654 -425 v -431.522 -502.654 -425 v -430 -495 75 v -430 -495 -425 v 470 -495 75 v 468.478 -487.346 75 v 470 -495 -425 v 470 -495 -425 v 468.478 -487.346 75 v 468.478 -487.346 -425 v 468.478 -487.346 75 v 464.142 -480.858 75 v 468.478 -487.346 -425 v 468.478 -487.346 -425 v 464.142 -480.858 75 v 464.142 -480.858 -425 v 464.142 -480.858 75 v 457.654 -476.522 75 v 464.142 -480.858 -425 v 464.142 -480.858 -425 v 457.654 -476.522 75 v 457.654 -476.522 -425 v 457.654 -476.522 75 v 450 -475 75 v 457.654 -476.522 -425 v 457.654 -476.522 -425 v 450 -475 75 v 450 -475 -425 v 450 -475 75 v 442.346 -476.522 75 v 450 -475 -425 v 450 -475 -425 v 442.346 -476.522 75 v 442.346 -476.522 -425 v 442.346 -476.522 75 v 435.858 -480.858 75 v 442.346 -476.522 -425 v 442.346 -476.522 -425 v 435.858 -480.858 75 v 435.858 -480.858 -425 v 435.858 -480.858 75 v 431.522 -487.346 75 v 435.858 -480.858 -425 v 435.858 -480.858 -425 v 431.522 -487.346 75 v 431.522 -487.346 -425 v 431.522 -487.346 75 v 430 -495 75 v 431.522 -487.346 -425 v 431.522 -487.346 -425 v 430 -495 75 v 430 -495 -425 v 430 -495 75 v 431.522 -502.654 75 v 430 -495 -425 v 430 -495 -425 v 431.522 -502.654 75 v 431.522 -502.654 -425 v 431.522 -502.654 75 v 435.858 -509.142 75 v 431.522 -502.654 -425 v 431.522 -502.654 -425 v 435.858 -509.142 75 v 435.858 -509.142 -425 v 435.858 -509.142 75 v 442.346 -513.478 75 v 435.858 -509.142 -425 v 435.858 -509.142 -425 v 442.346 -513.478 75 v 442.346 -513.478 -425 v 442.346 -513.478 75 v 450 -515 75 v 442.346 -513.478 -425 v 442.346 -513.478 -425 v 450 -515 75 v 450 -515 -425 v 450 -515 75 v 457.654 -513.478 75 v 450 -515 -425 v 450 -515 -425 v 457.654 -513.478 75 v 457.654 -513.478 -425 v 457.654 -513.478 75 v 464.142 -509.142 75 v 457.654 -513.478 -425 v 457.654 -513.478 -425 v 464.142 -509.142 75 v 464.142 -509.142 -425 v 464.142 -509.142 75 v 468.478 -502.654 75 v 464.142 -509.142 -425 v 464.142 -509.142 -425 v 468.478 -502.654 75 v 468.478 -502.654 -425 v 468.478 -502.654 75 v 470 -495 75 v 468.478 -502.654 -425 v 468.478 -502.654 -425 v 470 -495 75 v 470 -495 -425 v -380 505 75 v -381.522 512.654 75 v -380 505 -425 v -380 505 -425 v -381.522 512.654 75 v -381.522 512.654 -425 v -381.522 512.654 75 v -385.858 519.142 75 v -381.522 512.654 -425 v -381.522 512.654 -425 v -385.858 519.142 75 v -385.858 519.142 -425 v -385.858 519.142 75 v -392.346 523.478 75 v -385.858 519.142 -425 v -385.858 519.142 -425 v -392.346 523.478 75 v -392.346 523.478 -425 v -392.346 523.478 75 v -400 525 75 v -392.346 523.478 -425 v -392.346 523.478 -425 v -400 525 75 v -400 525 -425 v -400 525 75 v -407.654 523.478 75 v -400 525 -425 v -400 525 -425 v -407.654 523.478 75 v -407.654 523.478 -425 v -407.654 523.478 75 v -414.142 519.142 75 v -407.654 523.478 -425 v -407.654 523.478 -425 v -414.142 519.142 75 v -414.142 519.142 -425 v -414.142 519.142 75 v -418.478 512.654 75 v -414.142 519.142 -425 v -414.142 519.142 -425 v -418.478 512.654 75 v -418.478 512.654 -425 v -418.478 512.654 75 v -420 505 75 v -418.478 512.654 -425 v -418.478 512.654 -425 v -420 505 75 v -420 505 -425 v -420 505 75 v -418.478 497.346 75 v -420 505 -425 v -420 505 -425 v -418.478 497.346 75 v -418.478 497.346 -425 v -418.478 497.346 75 v -414.142 490.858 75 v -418.478 497.346 -425 v -418.478 497.346 -425 v -414.142 490.858 75 v -414.142 490.858 -425 v -414.142 490.858 75 v -407.654 486.522 75 v -414.142 490.858 -425 v -414.142 490.858 -425 v -407.654 486.522 75 v -407.654 486.522 -425 v -407.654 486.522 75 v -400 485 75 v -407.654 486.522 -425 v -407.654 486.522 -425 v -400 485 75 v -400 485 -425 v -400 485 75 v -392.346 486.522 75 v -400 485 -425 v -400 485 -425 v -392.346 486.522 75 v -392.346 486.522 -425 v -392.346 486.522 75 v -385.858 490.858 75 v -392.346 486.522 -425 v -392.346 486.522 -425 v -385.858 490.858 75 v -385.858 490.858 -425 v -385.858 490.858 75 v -381.522 497.346 75 v -385.858 490.858 -425 v -385.858 490.858 -425 v -381.522 497.346 75 v -381.522 497.346 -425 v -381.522 497.346 75 v -380 505 75 v -381.522 497.346 -425 v -381.522 497.346 -425 v -380 505 75 v -380 505 -425 v -500 -395 425 v 500 -395 425 v 462 -203.6 425 v -500 -395 425 v 462 -203.6 425 v 353.5 -41.5 425 v -500 -395 425 v 353.5 -41.5 425 v 191.4 66.4 425 v -500 -395 425 v 191.4 66.4 425 v -500 66.4 425 v -500 66.4 375 v 191.4 66.4 375 v 353.5 -41.5 375 v -500 66.4 375 v 353.5 -41.5 375 v 462 -203.6 375 v -500 66.4 375 v 462 -203.6 375 v 500 -395 375 v -500 66.4 375 v 500 -395 375 v -500 -395 375 v -500 -395 425 v -500 -395 375 v 500 -395 375 v -500 -395 425 v 500 -395 375 v 500 -395 425 v 500 -395 425 v 500 -395 375 v 462 -203.6 375 v 500 -395 425 v 462 -203.6 375 v 462 -203.6 425 v 462 -203.6 425 v 462 -203.6 375 v 353.5 -41.5 375 v 462 -203.6 425 v 353.5 -41.5 375 v 353.5 -41.5 425 v 353.5 -41.5 425 v 353.5 -41.5 375 v 191.4 66.4 375 v 353.5 -41.5 425 v 191.4 66.4 375 v 191.4 66.4 425 v 191.4 66.4 425 v 191.4 66.4 375 v -500 66.4 375 v 191.4 66.4 425 v -500 66.4 375 v -500 66.4 425 v -500 66.4 425 v -500 66.4 375 v -500 -395 375 v -500 66.4 425 v -500 -395 375 v -500 -395 425 v -500 66.4 425 v 191.4 66.4 425 v -38 296.4 425 v -500 66.4 425 v -38 296.4 425 v -500 296.4 425 v -500 296.4 375 v -38 296.4 375 v 191.4 66.4 375 v -500 296.4 375 v 191.4 66.4 375 v -500 66.4 375 v -500 66.4 425 v -500 66.4 375 v 191.4 66.4 375 v -500 66.4 425 v 191.4 66.4 375 v 191.4 66.4 425 v 191.4 66.4 425 v 191.4 66.4 375 v -38 296.4 375 v 191.4 66.4 425 v -38 296.4 375 v -38 296.4 425 v -38 296.4 425 v -38 296.4 375 v -500 296.4 375 v -38 296.4 425 v -500 296.4 375 v -500 296.4 425 v -500 296.4 425 v -500 296.4 375 v -500 66.4 375 v -500 296.4 425 v -500 66.4 375 v -500 66.4 425 v -500 296.4 425 v -38 296.4 425 v -146.5 458.6 425 v -500 296.4 425 v -146.5 458.6 425 v -308.7 566.4 425 v -500 296.4 425 v -308.7 566.4 425 v -500 605 425 v -500 605 375 v -308.7 566.4 375 v -146.5 458.6 375 v -500 605 375 v -146.5 458.6 375 v -38 296.4 375 v -500 605 375 v -38 296.4 375 v -500 296.4 375 v -500 296.4 425 v -500 296.4 375 v -38 296.4 375 v -500 296.4 425 v -38 296.4 375 v -38 296.4 425 v -38 296.4 425 v -38 296.4 375 v -146.5 458.6 375 v -38 296.4 425 v -146.5 458.6 375 v -146.5 458.6 425 v -146.5 458.6 425 v -146.5 458.6 375 v -308.7 566.4 375 v -146.5 458.6 425 v -308.7 566.4 375 v -308.7 566.4 425 v -308.7 566.4 425 v -308.7 566.4 375 v -500 605 375 v -308.7 566.4 425 v -500 605 375 v -500 605 425 v -500 605 425 v -500 605 375 v -500 296.4 375 v -500 605 425 v -500 296.4 375 v -500 296.4 425 f 1 2 3 f 4 5 6 f 7 8 9 f 10 11 12 f 13 14 15 f 16 17 18 f 19 20 21 f 22 23 24 f 25 26 27 f 28 29 30 f 31 32 33 f 34 35 36 f 37 38 39 f 40 41 42 f 43 44 45 f 46 47 48 f 49 50 51 f 52 53 54 f 55 56 57 f 58 59 60 f 61 62 63 f 64 65 66 f 67 68 69 f 70 71 72 f 73 74 75 f 76 77 78 f 79 80 81 f 82 83 84 f 85 86 87 f 88 89 90 f 91 92 93 f 94 95 96 f 97 98 99 f 100 101 102 f 103 104 105 f 106 107 108 f 109 110 111 f 112 113 114 f 115 116 117 f 118 119 120 f 121 122 123 f 124 125 126 f 127 128 129 f 130 131 132 f 133 134 135 f 136 137 138 f 139 140 141 f 142 143 144 f 145 146 147 f 148 149 150 f 151 152 153 f 154 155 156 f 157 158 159 f 160 161 162 f 163 164 165 f 166 167 168 f 169 170 171 f 172 173 174 f 175 176 177 f 178 179 180 f 181 182 183 f 184 185 186 f 187 188 189 f 190 191 192 f 193 194 195 f 196 197 198 f 199 200 201 f 202 203 204 f 205 206 207 f 208 209 210 f 211 212 213 f 214 215 216 f 217 218 219 f 220 221 222 f 223 224 225 f 226 227 228 f 229 230 231 f 232 233 234 f 235 236 237 f 238 239 240 f 241 242 243 f 244 245 246 f 247 248 249 f 250 251 252 f 253 254 255 f 256 257 258 f 259 260 261 f 262 263 264 f 265 266 267 f 268 269 270 f 271 272 273 f 274 275 276 f 277 278 279 f 280 281 282 f 283 284 285 f 286 287 288 f 289 290 291 f 292 293 294 f 295 296 297 f 298 299 300 f 301 302 303 f 304 305 306 f 307 308 309 f 310 311 312 f 313 314 315 f 316 317 318 f 319 320 321 f 322 323 324 f 325 326 327 f 328 329 330 f 331 332 333 f 334 335 336 f 337 338 339 f 340 341 342 f 343 344 345 f 346 347 348 f 349 350 351 f 352 353 354 f 355 356 357 f 358 359 360 f 361 362 363 f 364 365 366 f 367 368 369 f 370 371 372 f 373 374 375 f 376 377 378 f 379 380 381 f 382 383 384 f 385 386 387 f 388 389 390 f 391 392 393 f 394 395 396 f 397 398 399 f 400 401 402 f 403 404 405 f 406 407 408 f 409 410 411 f 412 413 414 f 415 416 417 f 418 419 420 f 421 422 423 f 424 425 426 f 427 428 429 f 430 431 432 f 433 434 435 f 436 437 438 f 439 440 441 f 442 443 444 f 445 446 447 f 448 449 450 f 451 452 453 f 454 455 456 f 457 458 459 f 460 461 462 f 463 464 465 f 466 467 468 f 469 470 471 f 472 473 474 f 475 476 477 f 478 479 480 f 481 482 483 f 484 485 486 f 487 488 489 f 490 491 492 f 493 494 495 f 496 497 498 f 499 500 501 f 502 503 504 f 505 506 507 f 508 509 510 f 511 512 513 f 514 515 516 f 517 518 519 f 520 521 522 f 523 524 525 f 526 527 528 f 529 530 531 f 532 533 534 f 535 536 537 f 538 539 540 f 541 542 543 f 544 545 546 f 547 548 549 f 550 551 552 f 553 554 555 f 556 557 558 f 559 560 561 f 562 563 564 f 565 566 567 f 568 569 570 f 571 572 573 f 574 575 576 f 577 578 579 f 580 581 582 f 583 584 585 f 586 587 588 f 589 590 591 f 592 593 594 f 595 596 597 f 598 599 600 f 601 602 603 f 604 605 606 f 607 608 609 f 610 611 612 f 613 614 615 f 616 617 618 f 619 620 621 f 622 623 624 f 625 626 627 f 628 629 630 f 631 632 633 f 634 635 636 f 637 638 639 f 640 641 642 f 643 644 645 f 646 647 648 ================================================ FILE: test/fcl_resources/staircases_koroibot_hr.dae ================================================ Blender User Blender 2.76.0 commit date:2015-11-03, commit time:10:56, hash:f337fea 2015-12-22T11:49:14 2015-12-22T11:49:14 Z_UP 49.13434 1.777778 0.1 100 0 0 0 1 1 1 1 0 0.00111109 0.000999987 1 0.1 0.1 1 1 1 2 0 1 1 1 1 1 0 2880 2 30.002 1.000799 0.04999995 29.99998 1 2 0 0 1 1 1 1 8192 1 1 0 1 1 1 3 0 0 0 0 0 1 1 1 3 0.15 75 1 1 0 1 1 0 0 0 0 1 0 0 0 1 0.8 0.4 0 1 0.5 0.5 0.5 1 50 1 0 0 0 1 0 0 0 1 0.8 0.8 0.8 1 0.5 0.5 0.5 1 50 1 0.3 0.5 0 0.3 -0.5 0 0 -0.5 0 0 0.5 0 0.3 0.5 0.07999998 0.3 -0.5 0.07999998 0 -0.5 0.07999998 0 0.5 0.07999998 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 2 0.5 0 2 -0.5 0 0 -0.5 0 0 0.5 0 2 0.5 0.07999998 2 -0.5 0.07999998 0 -0.5 0.07999998 0 0.5 0.07999998 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 1 0.5 0 1 -0.5 0 0 -0.5 0 0 0.5 0 1 0.5 0.07999998 1 -0.5 0.07999998 0 -0.5 0.07999998 0 0.5 0.07999998 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0.03999996 0.01999998 0 0.03999996 -0.01999998 0 0 -0.01999998 0 0 0.01999998 0 0.03999996 0.01999998 0.01999998 0.03999996 -0.01999998 0.01999998 0 -0.01999998 0.01999998 0 0.01999998 0.01999998 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0.03999996 0.01999998 0 0.03999996 -0.01999998 0 0 -0.01999998 0 0 0.01999998 0 0.03999996 0.01999998 0.12 0.03999996 -0.01999998 0.12 0 -0.01999998 0.12 0 0.01999998 0.12 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0.03999996 0.01999998 0 0.03999996 -0.01999998 0 0 -0.01999998 0 0 0.01999998 0 0.03999996 0.01999998 0.22 0.03999996 -0.01999998 0.22 0 -0.01999998 0.22 0 0.01999998 0.22 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0.03999996 0.01999998 0 0.03999996 -0.01999998 0 0 -0.01999998 0 0 0.01999998 0 0.03999996 0.01999998 0.32 0.03999996 -0.01999998 0.32 0 -0.01999998 0.32 0 0.01999998 0.32 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0.03999996 0.01999998 0 0.03999996 -0.01999998 0 0 -0.01999998 0 0 0.01999998 0 0.03999996 0.01999998 0.42 0.03999996 -0.01999998 0.42 0 -0.01999998 0.42 0 0.01999998 0.42 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0.03999996 0.01999998 0 0.03999996 -0.01999998 0 0 -0.01999998 0 0 0.01999998 0 0.03999996 0.01999998 0.52 0.03999996 -0.01999998 0.52 0 -0.01999998 0.52 0 0.01999998 0.52 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0.03999996 0.01999998 0 0.03999996 -0.01999998 0 0 -0.01999998 0 0 0.01999998 0 0.03999996 0.01999998 0.37 0.03999996 -0.01999998 0.37 0 -0.01999998 0.37 0 0.01999998 0.37 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0.03999996 0.01999998 0 0.03999996 -0.01999998 0 0 -0.01999998 0 0 0.01999998 0 0.03999996 0.01999998 0.06999999 0.03999996 -0.01999998 0.06999999 0 -0.01999998 0.06999999 0 0.01999998 0.06999999 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 0 0 0 1 0 3 3 3 3 3 3 3 3 3 3 3 3

1 0 2 0 3 0 7 1 6 1 5 1 4 2 5 2 1 2 5 3 6 3 2 3 6 4 7 4 3 4 0 5 3 5 7 5 0 0 1 0 3 0 4 1 7 1 5 1 0 2 4 2 1 2 1 3 5 3 2 3 2 4 6 4 3 4 4 5 0 5 7 5

 0 1 -1 0 1 1 0.1950902 0.9807853 -1 0.1950902 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314697 0.5555702 -1 0.8314697 0.5555702 1 0.9238795 0.3826834 -1 0.9238795 0.3826834 1 0.9807853 0.1950902 -1 0.9807853 0.1950902 1 1 0 -1 1 0 1 0.9807853 -0.1950902 -1 0.9807853 -0.1950902 1 0.9238796 -0.3826833 -1 0.9238796 -0.3826833 1 0.8314697 -0.5555702 -1 0.8314697 -0.5555702 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555702 -0.8314697 -1 0.5555702 -0.8314697 1 0.3826833 -0.9238796 -1 0.3826833 -0.9238796 1 0.1950901 -0.9807853 -1 0.1950901 -0.9807853 1 -3.25841e-7 -1 -1 -3.25841e-7 -1 1 -0.1950907 -0.9807852 -1 -0.1950907 -0.9807852 1 -0.3826839 -0.9238793 -1 -0.3826839 -0.9238793 1 -0.5555707 -0.8314693 -1 -0.5555707 -0.8314693 1 -0.7071073 -0.7071064 -1 -0.7071073 -0.7071064 1 -0.83147 -0.5555697 -1 -0.83147 -0.5555697 1 -0.9238799 -0.3826827 -1 -0.9238799 -0.3826827 1 -0.9807854 -0.1950893 -1 -0.9807854 -0.1950893 1 -1 9.65599e-7 -1 -1 9.65599e-7 1 -0.9807851 0.1950913 -1 -0.9807851 0.1950913 1 -0.9238791 0.3826845 -1 -0.9238791 0.3826845 1 -0.8314689 0.5555713 -1 -0.8314689 0.5555713 1 -0.7071059 0.7071077 -1 -0.7071059 0.7071077 1 -0.5555691 0.8314704 -1 -0.5555691 0.8314704 1 -0.3826821 0.9238801 -1 -0.3826821 0.9238801 1 -0.1950888 0.9807856 -1 -0.1950888 0.9807856 1 0.09801697 0.9951848 0 0.2902849 0.9569404 0 0.4713966 0.8819214 0 0.6343933 0.7730104 0 0.7730107 0.634393 0 0.8819215 0.4713965 0 0.9569402 0.2902852 0 0.9951848 0.09801667 0 0.9951848 -0.09801733 0 0.9569404 -0.2902847 0 0.8819212 -0.4713968 0 0.7730104 -0.6343934 0 0.6343934 -0.7730104 0 0.4713965 -0.8819214 0 0.2902843 -0.9569405 0 0.09801691 -0.9951848 0 -0.09801757 -0.9951848 0 -0.2902851 -0.9569403 0 -0.4713971 -0.8819211 0 -0.6343934 -0.7730104 0 -0.7730111 -0.6343926 0 -0.8819215 -0.4713966 0 -0.9569408 -0.2902833 0 -0.9951848 -0.09801661 0 -0.9951847 0.09801846 0 -0.95694 0.2902857 0 -0.8819207 0.4713978 0 -0.77301 0.6343941 0 -0.634392 0.7730115 0 -0.4713955 0.881922 0 0 0 1 -0.0980162 0.9951849 0 -0.2902832 0.9569408 0 0 0 -1 0.2902848 0.9569404 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730104 0.6343934 0 0.8819217 0.471396 0 0.9569404 0.2902847 0 0.9951848 0.09801727 0 0.9951848 -0.09801673 0 0.9569406 -0.2902842 0 0.8819216 -0.4713963 0 0.7730107 -0.634393 0 0.6343931 -0.7730107 0 0.4713966 -0.8819214 0 0.2902842 -0.9569405 0 -0.2902851 -0.9569402 0 -0.4713972 -0.8819211 0 -0.8819217 -0.471396 0 -0.9951849 -0.09801608 0 -0.9951847 0.09801787 0 -0.7730096 0.6343944 0 -0.6343923 0.7730113 0 -0.4713956 0.8819219 0 0 0 1 -3.97512e-6 0 1 1.36853e-6 0 1 1.36853e-6 0 1 -3.88857e-7 0 1 1.02308e-6 0 1 -1.94429e-7 0 1 -2.87796e-7 0 1 0 0 1 -0.2902833 0.9569408 0 0 0 -1 1.36853e-6 0 -1 3.88857e-7 0 -1 2.87797e-7 0 -1 0 0 -1 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

1 0 3 0 2 0 3 1 5 1 4 1 5 2 7 2 6 2 7 3 9 3 8 3 9 4 11 4 10 4 11 5 13 5 12 5 13 6 15 6 14 6 15 7 17 7 16 7 17 8 19 8 18 8 19 9 21 9 20 9 21 10 23 10 22 10 23 11 25 11 24 11 25 12 27 12 26 12 27 13 29 13 28 13 29 14 31 14 30 14 31 15 33 15 32 15 33 16 35 16 34 16 35 17 37 17 36 17 37 18 39 18 38 18 39 19 41 19 40 19 41 20 43 20 42 20 43 21 45 21 44 21 45 22 47 22 46 22 47 23 49 23 48 23 49 24 51 24 50 24 51 25 53 25 52 25 53 26 55 26 54 26 55 27 57 27 56 27 57 28 59 28 58 28 59 29 61 29 60 29 37 30 21 30 53 30 63 31 1 31 0 31 61 32 63 32 62 32 30 33 46 33 14 33 0 0 1 0 2 0 2 34 3 34 4 34 4 35 5 35 6 35 6 36 7 36 8 36 8 37 9 37 10 37 10 38 11 38 12 38 12 39 13 39 14 39 14 40 15 40 16 40 16 41 17 41 18 41 18 42 19 42 20 42 20 43 21 43 22 43 22 44 23 44 24 44 24 45 25 45 26 45 26 46 27 46 28 46 28 47 29 47 30 47 30 15 31 15 32 15 32 16 33 16 34 16 34 48 35 48 36 48 36 49 37 49 38 49 38 19 39 19 40 19 40 20 41 20 42 20 42 50 43 50 44 50 44 22 45 22 46 22 46 51 47 51 48 51 48 52 49 52 50 52 50 25 51 25 52 25 52 26 53 26 54 26 54 53 55 53 56 53 56 54 57 54 58 54 58 55 59 55 60 55 5 56 3 56 1 56 1 56 63 56 5 56 61 56 59 56 57 56 57 56 55 56 53 56 53 57 51 57 49 57 49 56 47 56 53 56 45 58 43 58 37 58 41 56 39 56 37 56 37 56 35 56 33 56 33 56 31 56 29 56 29 56 27 56 25 56 25 56 23 56 21 56 21 56 19 56 17 56 17 56 15 56 21 56 13 56 11 56 9 56 9 56 7 56 5 56 5 56 63 56 61 56 61 56 57 56 5 56 53 59 47 59 45 59 43 56 41 56 37 56 37 60 33 60 21 60 29 61 25 61 21 61 21 56 15 56 13 56 13 56 9 56 21 56 5 62 57 62 53 62 53 63 45 63 37 63 33 56 29 56 21 56 21 56 9 56 5 56 5 64 53 64 21 64 62 31 63 31 0 31 60 65 61 65 62 65 62 66 0 66 2 66 2 66 4 66 6 66 6 66 8 66 10 66 10 66 12 66 6 66 14 66 16 66 18 66 18 66 20 66 14 66 22 66 24 66 30 66 26 66 28 66 30 66 30 66 32 66 34 66 34 66 36 66 38 66 38 66 40 66 42 66 42 66 44 66 46 66 46 66 48 66 50 66 50 66 52 66 54 66 54 66 56 66 62 66 58 66 60 66 62 66 62 66 2 66 14 66 6 66 12 66 14 66 14 67 20 67 22 67 24 66 26 66 30 66 30 66 34 66 46 66 38 66 42 66 46 66 46 68 50 68 62 68 56 66 58 66 62 66 2 66 6 66 14 66 14 69 22 69 30 69 34 66 38 66 46 66 50 66 54 66 62 66 62 70 14 70 46 70

 0 1 -1 0 1 1 0.1950902 0.9807853 -1 0.1950902 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314697 0.5555702 -1 0.8314697 0.5555702 1 0.9238795 0.3826834 -1 0.9238795 0.3826834 1 0.9807853 0.1950902 -1 0.9807853 0.1950902 1 1 0 -1 1 0 1 0.9807853 -0.1950902 -1 0.9807853 -0.1950902 1 0.9238796 -0.3826833 -1 0.9238796 -0.3826833 1 0.8314697 -0.5555702 -1 0.8314697 -0.5555702 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555702 -0.8314697 -1 0.5555702 -0.8314697 1 0.3826833 -0.9238796 -1 0.3826833 -0.9238796 0.9999216 0.1950901 -0.9807853 -1 0.1950901 -0.9807853 1 -3.25841e-7 -1 -1 -3.25841e-7 -1 1 -0.1950907 -0.9807852 -1 -0.1950907 -0.9807852 1 -0.3826839 -0.9238793 -1 -0.3826839 -0.9238793 1 -0.5555707 -0.8314693 -1 -0.5555707 -0.8314693 1 -0.7071073 -0.7071064 -1 -0.7071073 -0.7071064 1 -0.83147 -0.5555697 -1 -0.83147 -0.5555697 1 -0.9238799 -0.3826827 -1 -0.9238799 -0.3826827 1 -0.9807854 -0.1950893 -1 -0.9807854 -0.1950893 1 -1 9.65599e-7 -1 -1 9.65599e-7 1 -0.9807851 0.1950913 -1 -0.9807851 0.1950913 1 -0.9238791 0.3826845 -1 -0.9238791 0.3826845 1 -0.8314689 0.5555713 -1 -0.8314689 0.5555713 1 -0.7071059 0.7071077 -1 -0.7071059 0.7071077 1 -0.5555691 0.8314704 -1 -0.5555691 0.8314704 1 -0.3826821 0.9238801 -1 -0.3826821 0.9238801 1 -0.1950888 0.9807856 -1 -0.1950888 0.9807856 1 0.09801697 0.9951848 0 0.2902849 0.9569404 0 0.4713966 0.8819214 0 0.6343933 0.7730104 0 0.7730107 0.634393 0 0.8819215 0.4713965 0 0.9569402 0.2902852 0 0.9951848 0.09801667 0 0.9951848 -0.09801733 0 0.9569404 -0.2902847 0 0.8819212 -0.4713968 0 0.7730104 -0.6343934 0 0.6343934 -0.7730104 0 0.4713966 -0.8819214 0 0.2902843 -0.9569405 0 0.09801691 -0.9951848 0 -0.09801757 -0.9951848 0 -0.2902851 -0.9569403 0 -0.4713971 -0.8819211 0 -0.6343934 -0.7730104 0 -0.7730111 -0.6343926 0 -0.8819215 -0.4713966 0 -0.9569408 -0.2902833 0 -0.9951848 -0.09801661 0 -0.9951847 0.09801846 0 -0.95694 0.2902857 0 -0.8819207 0.4713978 0 -0.77301 0.6343941 0 -0.634392 0.7730115 0 -0.4713955 0.881922 0 0 0 1 -0.0980162 0.9951849 0 -0.2902832 0.9569408 0 0 0 -1 0.2902848 0.9569404 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730104 0.6343934 0 0.8819217 0.471396 0 0.9569404 0.2902847 0 0.9951848 0.09801727 0 0.9951848 -0.09801673 0 0.9569406 -0.2902842 0 0.8819216 -0.4713963 0 0.7730107 -0.634393 0 0.6343931 -0.7730107 0 0.4713964 -0.8819215 0 0.2902843 -0.9569405 0 -0.2902851 -0.9569402 0 -0.4713972 -0.8819211 0 -0.8819217 -0.471396 0 -0.9951849 -0.09801608 0 -0.9951847 0.09801787 0 -0.7730096 0.6343944 0 -0.6343923 0.7730113 0 -0.4713956 0.8819219 0 0 0 1 -3.97512e-6 0 1 1.36853e-6 0 1 -2.01736e-4 0.002039551 0.9999979 -0.001303791 0.001584231 0.999998 1.36853e-6 0 1 -3.88857e-7 0 1 -4.34808e-4 2.91635e-4 0.9999999 -1.94429e-7 0 1 -2.87796e-7 0 1 2.91716e-4 -4.36462e-4 1 0 0 1 -0.2902833 0.9569408 0 0 0 -1 1.36853e-6 0 -1 3.88857e-7 0 -1 2.87797e-7 0 -1 0 0 -1 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

1 0 3 0 2 0 3 1 5 1 4 1 5 2 7 2 6 2 7 3 9 3 8 3 9 4 11 4 10 4 11 5 13 5 12 5 13 6 15 6 14 6 15 7 17 7 16 7 17 8 19 8 18 8 19 9 21 9 20 9 21 10 23 10 22 10 23 11 25 11 24 11 25 12 27 12 26 12 26 13 27 13 29 13 29 14 31 14 30 14 31 15 33 15 32 15 33 16 35 16 34 16 35 17 37 17 36 17 37 18 39 18 38 18 39 19 41 19 40 19 41 20 43 20 42 20 43 21 45 21 44 21 45 22 47 22 46 22 47 23 49 23 48 23 49 24 51 24 50 24 51 25 53 25 52 25 53 26 55 26 54 26 55 27 57 27 56 27 57 28 59 28 58 28 59 29 61 29 60 29 37 30 21 30 53 30 63 31 1 31 0 31 61 32 63 32 62 32 30 33 46 33 14 33 0 0 1 0 2 0 2 34 3 34 4 34 4 35 5 35 6 35 6 36 7 36 8 36 8 37 9 37 10 37 10 38 11 38 12 38 12 39 13 39 14 39 14 40 15 40 16 40 16 41 17 41 18 41 18 42 19 42 20 42 20 43 21 43 22 43 22 44 23 44 24 44 24 45 25 45 26 45 28 46 26 46 29 46 28 47 29 47 30 47 30 15 31 15 32 15 32 16 33 16 34 16 34 48 35 48 36 48 36 49 37 49 38 49 38 19 39 19 40 19 40 20 41 20 42 20 42 50 43 50 44 50 44 22 45 22 46 22 46 51 47 51 48 51 48 52 49 52 50 52 50 25 51 25 52 25 52 26 53 26 54 26 54 53 55 53 56 53 56 54 57 54 58 54 58 55 59 55 60 55 5 56 3 56 1 56 1 56 63 56 5 56 61 56 59 56 57 56 57 56 55 56 53 56 53 57 51 57 49 57 49 56 47 56 53 56 45 58 43 58 37 58 41 56 39 56 37 56 37 56 35 56 33 56 33 59 31 59 29 59 29 60 27 60 25 60 25 56 23 56 21 56 21 56 19 56 17 56 17 56 15 56 21 56 13 56 11 56 9 56 9 56 7 56 5 56 5 56 63 56 61 56 61 56 57 56 5 56 53 61 47 61 45 61 43 56 41 56 37 56 37 62 33 62 21 62 29 63 25 63 21 63 21 56 15 56 13 56 13 56 9 56 21 56 5 64 57 64 53 64 53 65 45 65 37 65 33 66 29 66 21 66 21 56 9 56 5 56 5 67 53 67 21 67 62 31 63 31 0 31 60 68 61 68 62 68 62 69 0 69 2 69 2 69 4 69 6 69 6 69 8 69 10 69 10 69 12 69 6 69 14 69 16 69 18 69 18 69 20 69 14 69 22 69 24 69 30 69 26 69 28 69 30 69 30 69 32 69 34 69 34 69 36 69 38 69 38 69 40 69 42 69 42 69 44 69 46 69 46 69 48 69 50 69 50 69 52 69 54 69 54 69 56 69 62 69 58 69 60 69 62 69 62 69 2 69 14 69 6 69 12 69 14 69 14 70 20 70 22 70 24 69 26 69 30 69 30 69 34 69 46 69 38 69 42 69 46 69 46 71 50 71 62 71 56 69 58 69 62 69 2 69 6 69 14 69 14 72 22 72 30 72 34 69 38 69 46 69 50 69 54 69 62 69 62 73 14 73 46 73

 0 1 -1 0 1 1 0.1950902 0.9807853 -1 0.1950902 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314697 0.5555702 -1 0.8314697 0.5555702 1 0.9238795 0.3826834 -1 0.9238795 0.3826834 1 0.9807853 0.1950902 -1 0.9807853 0.1950902 1 1 0 -1 1 0 1 0.9807853 -0.1950902 -1 0.9807853 -0.1950902 1 0.9238796 -0.3826833 -1 0.9238796 -0.3826833 1 0.8314697 -0.5555702 -1 0.8314697 -0.5555702 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555702 -0.8314697 -1 0.5555702 -0.8314697 1 0.3826833 -0.9238796 -1 0.3826833 -0.9238796 1 0.1950901 -0.9807853 -1 0.1950901 -0.9807853 1 -3.25841e-7 -1 -1 -3.25841e-7 -1 1 -0.1950907 -0.9807852 -1 -0.1950907 -0.9807852 1 -0.3826839 -0.9238793 -1 -0.3826839 -0.9238793 1 -0.5555707 -0.8314693 -1 -0.5555707 -0.8314693 1 -0.7071073 -0.7071064 -1 -0.7071073 -0.7071064 1 -0.83147 -0.5555697 -1 -0.83147 -0.5555697 1 -0.9238799 -0.3826827 -1 -0.9238799 -0.3826827 1 -0.9807854 -0.1950893 -1 -0.9807854 -0.1950893 1 -1 9.65599e-7 -1 -1 9.65599e-7 1 -0.9807851 0.1950913 -1 -0.9807851 0.1950913 1 -0.9238791 0.3826845 -1 -0.9238791 0.3826845 1 -0.8314689 0.5555713 -1 -0.8314689 0.5555713 1 -0.7071059 0.7071077 -1 -0.7071059 0.7071077 1 -0.5555691 0.8314704 -1 -0.5555691 0.8314704 1 -0.3826821 0.9238801 -1 -0.3826821 0.9238801 1 -0.1950888 0.9807856 -1 -0.1950888 0.9807856 1 0.09801697 0.9951848 0 0.2902849 0.9569404 0 0.4713966 0.8819214 0 0.6343933 0.7730104 0 0.7730107 0.634393 0 0.8819215 0.4713965 0 0.9569402 0.2902852 0 0.9951848 0.09801667 0 0.9951848 -0.09801733 0 0.9569404 -0.2902847 0 0.8819212 -0.4713968 0 0.7730104 -0.6343934 0 0.6343934 -0.7730104 0 0.4713965 -0.8819214 0 0.2902843 -0.9569405 0 0.09801691 -0.9951848 0 -0.09801757 -0.9951848 0 -0.2902851 -0.9569403 0 -0.4713971 -0.8819211 0 -0.6343934 -0.7730104 0 -0.7730111 -0.6343926 0 -0.8819215 -0.4713966 0 -0.9569408 -0.2902833 0 -0.9951848 -0.09801661 0 -0.9951847 0.09801846 0 -0.95694 0.2902857 0 -0.8819207 0.4713978 0 -0.77301 0.6343941 0 -0.634392 0.7730115 0 -0.4713955 0.881922 0 0 0 1 -0.0980162 0.9951849 0 -0.2902832 0.9569408 0 0 0 -1 0.2902848 0.9569404 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730104 0.6343934 0 0.8819217 0.471396 0 0.9569404 0.2902847 0 0.9951848 0.09801727 0 0.9951848 -0.09801673 0 0.9569406 -0.2902842 0 0.8819216 -0.4713963 0 0.7730107 -0.634393 0 0.6343931 -0.7730107 0 0.4713966 -0.8819214 0 0.2902842 -0.9569405 0 -0.2902851 -0.9569402 0 -0.4713972 -0.8819211 0 -0.8819217 -0.471396 0 -0.9951849 -0.09801608 0 -0.9951847 0.09801787 0 -0.7730096 0.6343944 0 -0.6343923 0.7730113 0 -0.4713956 0.8819219 0 0 0 1 -3.97512e-6 0 1 1.36853e-6 0 1 1.36853e-6 0 1 -3.88857e-7 0 1 1.02308e-6 0 1 -1.94429e-7 0 1 -2.87796e-7 0 1 0 0 1 -0.2902833 0.9569408 0 0 0 -1 1.36853e-6 0 -1 3.88857e-7 0 -1 2.87797e-7 0 -1 0 0 -1 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

1 0 3 0 2 0 3 1 5 1 4 1 5 2 7 2 6 2 7 3 9 3 8 3 9 4 11 4 10 4 11 5 13 5 12 5 13 6 15 6 14 6 15 7 17 7 16 7 17 8 19 8 18 8 19 9 21 9 20 9 21 10 23 10 22 10 23 11 25 11 24 11 25 12 27 12 26 12 27 13 29 13 28 13 29 14 31 14 30 14 31 15 33 15 32 15 33 16 35 16 34 16 35 17 37 17 36 17 37 18 39 18 38 18 39 19 41 19 40 19 41 20 43 20 42 20 43 21 45 21 44 21 45 22 47 22 46 22 47 23 49 23 48 23 49 24 51 24 50 24 51 25 53 25 52 25 53 26 55 26 54 26 55 27 57 27 56 27 57 28 59 28 58 28 59 29 61 29 60 29 37 30 21 30 53 30 63 31 1 31 0 31 61 32 63 32 62 32 30 33 46 33 14 33 0 0 1 0 2 0 2 34 3 34 4 34 4 35 5 35 6 35 6 36 7 36 8 36 8 37 9 37 10 37 10 38 11 38 12 38 12 39 13 39 14 39 14 40 15 40 16 40 16 41 17 41 18 41 18 42 19 42 20 42 20 43 21 43 22 43 22 44 23 44 24 44 24 45 25 45 26 45 26 46 27 46 28 46 28 47 29 47 30 47 30 15 31 15 32 15 32 16 33 16 34 16 34 48 35 48 36 48 36 49 37 49 38 49 38 19 39 19 40 19 40 20 41 20 42 20 42 50 43 50 44 50 44 22 45 22 46 22 46 51 47 51 48 51 48 52 49 52 50 52 50 25 51 25 52 25 52 26 53 26 54 26 54 53 55 53 56 53 56 54 57 54 58 54 58 55 59 55 60 55 5 56 3 56 1 56 1 56 63 56 5 56 61 56 59 56 57 56 57 56 55 56 53 56 53 57 51 57 49 57 49 56 47 56 53 56 45 58 43 58 37 58 41 56 39 56 37 56 37 56 35 56 33 56 33 56 31 56 29 56 29 56 27 56 25 56 25 56 23 56 21 56 21 56 19 56 17 56 17 56 15 56 21 56 13 56 11 56 9 56 9 56 7 56 5 56 5 56 63 56 61 56 61 56 57 56 5 56 53 59 47 59 45 59 43 56 41 56 37 56 37 60 33 60 21 60 29 61 25 61 21 61 21 56 15 56 13 56 13 56 9 56 21 56 5 62 57 62 53 62 53 63 45 63 37 63 33 56 29 56 21 56 21 56 9 56 5 56 5 64 53 64 21 64 62 31 63 31 0 31 60 65 61 65 62 65 62 66 0 66 2 66 2 66 4 66 6 66 6 66 8 66 10 66 10 66 12 66 6 66 14 66 16 66 18 66 18 66 20 66 14 66 22 66 24 66 30 66 26 66 28 66 30 66 30 66 32 66 34 66 34 66 36 66 38 66 38 66 40 66 42 66 42 66 44 66 46 66 46 66 48 66 50 66 50 66 52 66 54 66 54 66 56 66 62 66 58 66 60 66 62 66 62 66 2 66 14 66 6 66 12 66 14 66 14 67 20 67 22 67 24 66 26 66 30 66 30 66 34 66 46 66 38 66 42 66 46 66 46 68 50 68 62 68 56 66 58 66 62 66 2 66 6 66 14 66 14 69 22 69 30 69 34 66 38 66 46 66 50 66 54 66 62 66 62 70 14 70 46 70

 0 1 -1 0 1 1 0.1950902 0.9807853 -1 0.1950902 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314697 0.5555702 -1 0.8314697 0.5555702 1 0.9238795 0.3826834 -1 0.9238795 0.3826834 1 0.9807853 0.1950902 -1 0.9807853 0.1950902 1 1 0 -1 1 0 1 0.9807853 -0.1950902 -1 0.9807853 -0.1950902 1 0.9238796 -0.3826833 -1 0.9238796 -0.3826833 1 0.8314697 -0.5555702 -1 0.8314697 -0.5555702 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555702 -0.8314697 -1 0.5555702 -0.8314697 1 0.3826833 -0.9238796 -1 0.3826833 -0.9238796 1 0.1950901 -0.9807853 -1 0.1950901 -0.9807853 1 -3.25841e-7 -1 -1 -3.25841e-7 -1 1 -0.1950907 -0.9807852 -1 -0.1950907 -0.9807852 1 -0.3826839 -0.9238793 -1 -0.3826839 -0.9238793 1 -0.5555707 -0.8314693 -1 -0.5555707 -0.8314693 1 -0.7071073 -0.7071064 -1 -0.7071073 -0.7071064 1 -0.83147 -0.5555697 -1 -0.83147 -0.5555697 1 -0.9238799 -0.3826827 -1 -0.9238799 -0.3826827 1 -0.9807854 -0.1950893 -1 -0.9807854 -0.1950893 1 -1 9.65599e-7 -1 -1 9.65599e-7 1 -0.9807851 0.1950913 -1 -0.9807851 0.1950913 1 -0.9238791 0.3826845 -1 -0.9238791 0.3826845 1 -0.8314689 0.5555713 -1 -0.8314689 0.5555713 1 -0.7071059 0.7071077 -1 -0.7071059 0.7071077 1 -0.5555691 0.8314704 -1 -0.5555691 0.8314704 1 -0.3826821 0.9238801 -1 -0.3826821 0.9238801 1 -0.1950888 0.9807856 -1 -0.1950888 0.9807856 1 0.09801697 0.9951848 0 0.2902849 0.9569404 0 0.4713966 0.8819214 0 0.6343933 0.7730104 0 0.7730107 0.634393 0 0.8819215 0.4713965 0 0.9569402 0.2902852 0 0.9951848 0.09801667 0 0.9951848 -0.09801733 0 0.9569404 -0.2902847 0 0.8819212 -0.4713968 0 0.7730104 -0.6343934 0 0.6343934 -0.7730104 0 0.4713965 -0.8819214 0 0.2902843 -0.9569405 0 0.09801691 -0.9951848 0 -0.09801757 -0.9951848 0 -0.2902851 -0.9569403 0 -0.4713971 -0.8819211 0 -0.6343934 -0.7730104 0 -0.7730111 -0.6343926 0 -0.8819215 -0.4713966 0 -0.9569408 -0.2902833 0 -0.9951848 -0.09801661 0 -0.9951847 0.09801846 0 -0.95694 0.2902857 0 -0.8819207 0.4713978 0 -0.77301 0.6343941 0 -0.634392 0.7730115 0 -0.4713955 0.881922 0 0 0 1 -0.0980162 0.9951849 0 -0.2902832 0.9569408 0 0 0 -1 0.2902848 0.9569404 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730104 0.6343934 0 0.8819217 0.471396 0 0.9569404 0.2902847 0 0.9951848 0.09801727 0 0.9951848 -0.09801673 0 0.9569406 -0.2902842 0 0.8819216 -0.4713963 0 0.7730107 -0.634393 0 0.6343931 -0.7730107 0 0.4713966 -0.8819214 0 0.2902842 -0.9569405 0 -0.2902851 -0.9569402 0 -0.4713972 -0.8819211 0 -0.8819217 -0.471396 0 -0.9951849 -0.09801608 0 -0.9951847 0.09801787 0 -0.7730096 0.6343944 0 -0.6343923 0.7730113 0 -0.4713956 0.8819219 0 0 0 1 -3.97512e-6 0 1 1.36853e-6 0 1 1.36853e-6 0 1 -3.88857e-7 0 1 1.02308e-6 0 1 -1.94429e-7 0 1 -2.87796e-7 0 1 0 0 1 -0.2902833 0.9569408 0 0 0 -1 1.36853e-6 0 -1 3.88857e-7 0 -1 2.87797e-7 0 -1 0 0 -1 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

1 0 3 0 2 0 3 1 5 1 4 1 5 2 7 2 6 2 7 3 9 3 8 3 9 4 11 4 10 4 11 5 13 5 12 5 13 6 15 6 14 6 15 7 17 7 16 7 17 8 19 8 18 8 19 9 21 9 20 9 21 10 23 10 22 10 23 11 25 11 24 11 25 12 27 12 26 12 27 13 29 13 28 13 29 14 31 14 30 14 31 15 33 15 32 15 33 16 35 16 34 16 35 17 37 17 36 17 37 18 39 18 38 18 39 19 41 19 40 19 41 20 43 20 42 20 43 21 45 21 44 21 45 22 47 22 46 22 47 23 49 23 48 23 49 24 51 24 50 24 51 25 53 25 52 25 53 26 55 26 54 26 55 27 57 27 56 27 57 28 59 28 58 28 59 29 61 29 60 29 37 30 21 30 53 30 63 31 1 31 0 31 61 32 63 32 62 32 30 33 46 33 14 33 0 0 1 0 2 0 2 34 3 34 4 34 4 35 5 35 6 35 6 36 7 36 8 36 8 37 9 37 10 37 10 38 11 38 12 38 12 39 13 39 14 39 14 40 15 40 16 40 16 41 17 41 18 41 18 42 19 42 20 42 20 43 21 43 22 43 22 44 23 44 24 44 24 45 25 45 26 45 26 46 27 46 28 46 28 47 29 47 30 47 30 15 31 15 32 15 32 16 33 16 34 16 34 48 35 48 36 48 36 49 37 49 38 49 38 19 39 19 40 19 40 20 41 20 42 20 42 50 43 50 44 50 44 22 45 22 46 22 46 51 47 51 48 51 48 52 49 52 50 52 50 25 51 25 52 25 52 26 53 26 54 26 54 53 55 53 56 53 56 54 57 54 58 54 58 55 59 55 60 55 5 56 3 56 1 56 1 56 63 56 5 56 61 56 59 56 57 56 57 56 55 56 53 56 53 57 51 57 49 57 49 56 47 56 53 56 45 58 43 58 37 58 41 56 39 56 37 56 37 56 35 56 33 56 33 56 31 56 29 56 29 56 27 56 25 56 25 56 23 56 21 56 21 56 19 56 17 56 17 56 15 56 21 56 13 56 11 56 9 56 9 56 7 56 5 56 5 56 63 56 61 56 61 56 57 56 5 56 53 59 47 59 45 59 43 56 41 56 37 56 37 60 33 60 21 60 29 61 25 61 21 61 21 56 15 56 13 56 13 56 9 56 21 56 5 62 57 62 53 62 53 63 45 63 37 63 33 56 29 56 21 56 21 56 9 56 5 56 5 64 53 64 21 64 62 31 63 31 0 31 60 65 61 65 62 65 62 66 0 66 2 66 2 66 4 66 6 66 6 66 8 66 10 66 10 66 12 66 6 66 14 66 16 66 18 66 18 66 20 66 14 66 22 66 24 66 30 66 26 66 28 66 30 66 30 66 32 66 34 66 34 66 36 66 38 66 38 66 40 66 42 66 42 66 44 66 46 66 46 66 48 66 50 66 50 66 52 66 54 66 54 66 56 66 62 66 58 66 60 66 62 66 62 66 2 66 14 66 6 66 12 66 14 66 14 67 20 67 22 67 24 66 26 66 30 66 30 66 34 66 46 66 38 66 42 66 46 66 46 68 50 68 62 68 56 66 58 66 62 66 2 66 6 66 14 66 14 69 22 69 30 69 34 66 38 66 46 66 50 66 54 66 62 66 62 70 14 70 46 70

 0.6858805 -0.3173701 0.6548619 7.481132 0.7276338 0.3124686 -0.6106656 -6.50764 -0.01081678 0.8953432 0.4452454 5.343665 0 0 0 1 -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 1 0 0 0.25 0 1 0 0.8 0 0 1 0.02 0 0 0 1 1 0 0 0.55 0 1 0 0.8 0 0 1 0.12 0 0 0 1 1 0 0 0.85 0 1 0 0.8 0 0 1 0.22 0 0 0 1 1 0 0 1.15 0 1 0 0.8 0 0 1 0.32 0 0 0 1 1 0 0 1.45 0 1 0 0.8 0 0 1 0.42 0 0 0 1 -4.37114e-8 -1 0 2.25 1 -4.37114e-8 0 -0.3 0 0 0.9999999 0.52 0 0 0 1 -4.37114e-8 -1 0 1.25 1 -4.37114e-8 0 0 0 0 0.9999999 0.52 0 0 0 1 -4.37114e-8 -1 0 0.25 1 -4.37114e-8 0 0 0 0 0.9999999 0.52 0 0 0 1 -4.37114e-8 -1 0 -0.75 1 -4.37114e-8 0 0 0 0 0.9999999 0.52 0 0 0 1 -4.37114e-8 -1 0 -1.75 1 -4.37114e-8 0 -0.3 0 0 0.9999999 0.52 0 0 0 1 -4.37114e-8 -1 0 -1.75 1 -4.37114e-8 0 0.7 0 0 0.9999999 0.37 0 0 0 1 -4.37114e-8 -1 0 -1.75 1 -4.37114e-8 0 1.3 0 0 0.9999999 0.07 0 0 0 1 -4.37114e-8 -1 0 -1.75 1 -4.37114e-8 0 1 0 0 0.9999999 0.22 0 0 0 1 1 0 0 0.3 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 0.46 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 0.46 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 0.3 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 0.6 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 0.76 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 0.76 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 0.6 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 0.9 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 1.06 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 1.06 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 0.9 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 1.2 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 1.36 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 1.36 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 1.2 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 1.5 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 1.66 0 1 0 1.2 0 0 1 0 0 0 0 1 1 0 0 1.66 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 1.5 0 1 0 0.39 0 0 1 0 0 0 0 1 1 0 0 2.62 0 1 0 1.61 0 0 1 0 0 0 0 1 1 0 0 2.62 0 1 0 -0.21 0 0 1 0 0 0 0 1 1 0 0 1.8 0 1 0 -0.21 0 0 1 0 0 0 0 1 1 0 0 1.8 0 1 0 1.61 0 0 1 0 0 0 0 1 1 0 0 1.7 0 1 0 0.08 0 0 1 0 0 0 0 1 1 0 0 0.84 0 1 0 0.08 0 0 1 0 0 0 0 1 1 0 0 0.84 0 1 0 0.24 0 0 1 0 0 0 0 1 1 0 0 1.7 0 1 0 0.24 0 0 1 0 0 0 0 1 1 0 0 0.7 0 1 0 0.08 0 0 1 0 0 0 0 1 1 0 0 -0.16 0 1 0 0.08 0 0 1 0 0 0 0 1 1 0 0 -0.16 0 1 0 0.24 0 0 1 0 0 0 0 1 1 0 0 0.7 0 1 0 0.24 0 0 1 0 0 0 0 1 1 0 0 -0.3 0 1 0 0.08 0 0 1 0 0 0 0 1 1 0 0 -1.2 0 1 0 0.08 0 0 1 0 0 0 0 1 1 0 0 -1.2 0 1 0 0.24 0 0 1 0 0 0 0 1 1 0 0 -0.3 0 1 0 0.24 0 0 1 0 0 0 0 1 1 0 0 -1.34 0 1 0 0.6 0 0 1 0 0 0 0 1 1 0 0 -1.34 0 1 0 -0.24 0 0 1 0 0 0 0 1 1 0 0 -2.18 0 1 0 -0.24 0 0 1 0 0 0 0 1 1 0 0 -2.18 0 1 0 0.6 0 0 1 0 0 0 0 1 1 0 0 -1.34 0 1 0 0.75 0 0 1 0 0 0 0 1 1 0 0 -2.18 0 1 0 0.75 0 0 1 0 0 0 0 1 1 0 0 -1.34 0 1 0 0.96 0 0 1 0 0 0 0 1 1 0 0 -2.18 0 1 0 0.96 0 0 1 0 0 0 0 1 1 0 0 -1.34 0 1 0 1.05 0 0 1 0 0 0 0 1 1 0 0 -2.18 0 1 0 1.05 0 0 1 0 0 0 0 1 1 0 0 -1.34 0 1 0 1.26 0 0 1 0 0 0 0 1 1 0 0 -2.18 0 1 0 1.26 0 0 1 0 0 0 0 1 1 0 0 -1.34 0 1 0 1.36 0 0 1 0 0 0 0 1 1 0 0 -2.18 0 1 0 1.36 0 0 1 0 0 0 0 1 1 0 0 -1.34 0 1 0 1.55 0 0 1 0 0 0 0 1 1 0 0 -2.18 0 1 0 1.55 0 0 1 0 0 0 0 1 0.0075 0 0 -2.27 0 0.0075 0 1.57 0 0 0.415 0.43 0 0 0 1 0.0075 0 0 -2.27 0 0.0075 0 0.75 0 0 0.4825 0.769 0 0 0 1 0.0075 0 0 -2.27 0 0.003358189 -0.4068403 1.16 0 0.00670616 0.2037301 0.6939636 0 0 0 1 0.0075 0 0 -2.27 0 0.003358189 -0.4113111 1.16234 0 0.00670616 0.2059689 1.039 0 0 0 1 ================================================ FILE: test/frontlist.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE COAL_FRONT_LIST #include #include "coal/internal/traversal_node_bvhs.h" #include "coal/internal/traversal_node_setup.h" #include <../src/collision_node.h> #include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" #include using namespace coal; namespace utf = boost::unit_test::framework; template bool collide_front_list_Test(const Transform3s& tf1, const Transform3s& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose); template bool collide_front_list_Test_Oriented(const Transform3s& tf1, const Transform3s& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template bool collide_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); // TODO: randomly still have some runtime error BOOST_AUTO_TEST_CASE(front_list) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); std::vector transforms; // t0 std::vector transforms2; // t1 Scalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; Scalar delta_trans[] = {1, 1, 1}; #ifndef NDEBUG // if debug mode std::size_t n = 2; #else std::size_t n = 20; #endif n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n); bool verbose = false; generateRandomTransforms(extents, delta_trans, Scalar(0.005 * 2 * 3.1415), transforms, transforms2, n); bool res, res2; for (std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for (std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for (std::size_t i = 0; i < transforms.size(); ++i) { // Disabled broken test lines. Please see #25. // res = collide_Test(transforms2[i], p1, t1, p2, t2, // SPLIT_METHOD_MEDIAN, verbose); res2 = // collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, // t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for (std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test >( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for (std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test >( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for (std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test >( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for (std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test_Oriented( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test_Oriented( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test_Oriented( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(res == res2); } for (std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test_Oriented( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test_Oriented( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test_Oriented( transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(res == res2); } } template bool collide_front_list_Test(const Transform3s& tf1, const Transform3s& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); BVHFrontList front_list; std::vector vertices1_new(vertices1.size()); for (std::size_t i = 0; i < vertices1_new.size(); ++i) { vertices1_new[i] = tf1.transform(vertices1[i]); } m1.beginModel(); m1.addSubModel(vertices1_new, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3s pose1, pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); MeshCollisionTraversalNode node(request); bool success = initialize(node, m1, pose1, m2, pose2, local_result); BOOST_REQUIRE(success); node.enable_statistics = verbose; collide(&node, request, local_result, &front_list); if (verbose) std::cout << "front list size " << front_list.size() << std::endl; // update the mesh for (std::size_t i = 0; i < vertices1.size(); ++i) { vertices1_new[i] = tf2.transform(vertices1[i]); } m1.beginReplaceModel(); m1.replaceSubModel(vertices1_new); m1.endReplaceModel(true, refit_bottomup); m2.beginReplaceModel(); m2.replaceSubModel(vertices2); m2.endReplaceModel(true, refit_bottomup); local_result.clear(); collide(&node, request, local_result, &front_list); if (local_result.numContacts() > 0) return true; else return false; } template bool collide_front_list_Test_Oriented(const Transform3s& tf1, const Transform3s& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); BVHFrontList front_list; m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3s pose1(tf1), pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); TraversalNode node(request); bool success = initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, local_result); BOOST_REQUIRE(success); node.enable_statistics = verbose; collide(&node, request, local_result, &front_list); if (verbose) std::cout << "front list size " << front_list.size() << std::endl; // update the mesh pose1 = tf2; success = initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, local_result); BOOST_REQUIRE(success); local_result.clear(); collide(&node, request, local_result, &front_list); if (local_result.numContacts() > 0) return true; else return false; } template bool collide_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3s pose1(tf), pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); MeshCollisionTraversalNode node(request); bool success = initialize(node, m1, pose1, m2, pose2, local_result); BOOST_REQUIRE(success); node.enable_statistics = verbose; collide(&node, request, local_result); if (local_result.numContacts() > 0) return true; else return false; } ================================================ FILE: test/geometric_shapes.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #include "coal/narrowphase/narrowphase.h" #include "coal/collision.h" #include "coal/distance.h" #include "utility.h" #include #include "coal/internal/tools.h" #include "coal/shape/geometric_shape_to_BVH_model.h" #include "coal/internal/shape_shape_func.h" using namespace coal; Scalar extents[6] = {0, 0, 0, 10, 10, 10}; Scalar tol_gjk = Scalar(0.01); GJKSolver solver1; GJKSolver solver2; int line; #define SET_LINE line = __LINE__ #define FCL_CHECK(cond) \ BOOST_CHECK_MESSAGE(cond, "from line " << line << ": " #cond) #define FCL_CHECK_EQUAL(a, b) \ BOOST_CHECK_MESSAGE((a) == (b), "from line " << line << ": " #a "[" << (a) \ << "] != " #b "[" << (b) \ << "].") #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) namespace coal { std::ostream& operator<<(std::ostream& os, const ShapeBase&) { return os << "a_shape"; } std::ostream& operator<<(std::ostream& os, const Box& b) { return os << "Box(" << 2 * b.halfSide.transpose() << ')'; } } // namespace coal template void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2, const Vec3s& contact_or_normal, const Vec3s& expected_contact_or_normal, bool check_opposite_normal, Scalar tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << ".\n" << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl << "tf1.translation: " << tf1.getTranslation().transpose() << std::endl << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl << "tf2.translation: " << tf2.getTranslation().transpose() << std::endl << comparison_type << ": " << contact_or_normal.transpose() << std::endl << "expected_" << comparison_type << ": " << expected_contact_or_normal.transpose(); if (check_opposite_normal) std::cout << " or " << -expected_contact_or_normal.transpose(); std::cout << std::endl << "difference: " << (contact_or_normal - expected_contact_or_normal).norm() << std::endl << "tolerance: " << tol << std::endl; } template void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2, Scalar depth, Scalar expected_depth, Scalar tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << ".\n" << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl << "tf1.translation: " << tf1.getTranslation() << std::endl << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl << "tf2.translation: " << tf2.getTranslation() << std::endl << "depth: " << depth << std::endl << "expected_depth: " << expected_depth << std::endl << "difference: " << std::fabs(depth - expected_depth) << std::endl << "tolerance: " << tol << std::endl; } template void compareContact(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2, const Vec3s& contact, Vec3s* expected_point, Scalar depth, Scalar* expected_depth, const Vec3s& normal, Vec3s* expected_normal, bool check_opposite_normal, Scalar tol) { if (expected_point) { bool contact_equal = isEqual(contact, *expected_point, tol); FCL_CHECK(contact_equal); if (!contact_equal) printComparisonError("contact", s1, tf1, s2, tf2, contact, *expected_point, false, tol); } if (expected_depth) { bool depth_equal = std::fabs(depth - *expected_depth) < tol; FCL_CHECK(depth_equal); if (!depth_equal) printComparisonError("depth", s1, tf1, s2, tf2, depth, *expected_depth, tol); } if (expected_normal) { bool normal_equal = isEqual(normal, *expected_normal, tol); if (!normal_equal && check_opposite_normal) normal_equal = isEqual(normal, -(*expected_normal), tol); FCL_CHECK(normal_equal); if (!normal_equal) printComparisonError("normal", s1, tf1, s2, tf2, normal, *expected_normal, check_opposite_normal, tol); } } template void testShapeCollide(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2, bool expect_collision, Vec3s* expected_point = NULL, Scalar* expected_depth = NULL, Vec3s* expected_normal = NULL, bool check_opposite_normal = false, Scalar tol = Scalar(1e-9)) { CollisionRequest request; CollisionResult result; Vec3s contact; Vec3s normal; // normal direction should be from object 1 to object 2 bool collision; bool check_failed = false; request.enable_contact = false; result.clear(); collision = (collide(&s1, tf1, &s2, tf2, request, result) > 0); FCL_CHECK_EQUAL(collision, expect_collision); check_failed = check_failed || (collision != expect_collision); request.enable_contact = true; result.clear(); collision = (collide(&s1, tf1, &s2, tf2, request, result) > 0); FCL_CHECK_EQUAL(collision, expect_collision); check_failed = check_failed || (collision != expect_collision); if (check_failed) { BOOST_TEST_MESSAGE("Failure occured between " << s1 << " and " << s2 << " at transformations\n" << tf1 << '\n' << tf2); } if (expect_collision) { FCL_CHECK_EQUAL(result.numContacts(), 1); if (result.numContacts() == 1) { Contact contact = result.getContact(0); compareContact(s1, tf1, s2, tf2, contact.pos, expected_point, contact.penetration_depth, expected_depth, contact.normal, expected_normal, check_opposite_normal, tol); } } } BOOST_AUTO_TEST_CASE(box_to_bvh) { Box shape(1, 1, 1); BVHModel bvh; generateBVHModel(bvh, shape, Transform3s()); } BOOST_AUTO_TEST_CASE(sphere_to_bvh) { Sphere shape(1); BVHModel bvh; generateBVHModel(bvh, shape, Transform3s(), 10, 10); generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(cylinder_to_bvh) { Cylinder shape(1, 1); BVHModel bvh; generateBVHModel(bvh, shape, Transform3s(), 10, 10); generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(cone_to_bvh) { Cone shape(1, 1); BVHModel bvh; generateBVHModel(bvh, shape, Transform3s(), 10, 10); generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(collide_spheresphere) { Sphere s1(20); Sphere s2(10); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(Vec3s(40, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(40, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(30, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(30.01), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(30.01), 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(29.9), 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(29.9), 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3s(); tf2 = Transform3s(); normal << 1, 0, 0; // If the centers of two sphere are at the same position, // the normal is (1, 0, 0) SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; normal << 1, 0, 0; // If the centers of two sphere are at the same position, // the normal is (1, 0, 0) SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-29.9), 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-29.9), 0, 0)); normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-30.0, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-30.01), 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-30.01), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } bool compareContactPoints(const Vec3s& c1, const Vec3s& c2) { return c1[2] < c2[2]; } // Ascending order void testBoxBoxContactPoints(const Matrix3s& R) { Box s1(100, 100, 100); Box s2(10, 20, 30); // Vertices of s2 std::vector vertices(8); vertices[0] << 1, 1, 1; vertices[1] << 1, 1, -1; vertices[2] << 1, -1, 1; vertices[3] << 1, -1, -1; vertices[4] << -1, 1, 1; vertices[5] << -1, 1, -1; vertices[6] << -1, -1, 1; vertices[7] << -1, -1, -1; for (std::size_t i = 0; i < 8; ++i) { vertices[i].array() *= s2.halfSide.array(); } Transform3s tf1 = Transform3s(Vec3s(0, 0, -50)); Transform3s tf2 = Transform3s(R); Vec3s normal; Vec3s p1, p2; // Make sure the two boxes are colliding solver1.gjk_tolerance = Scalar(1e-5); solver1.epa_tolerance = Scalar(1e-5); const bool compute_penetration = true; Scalar distance = solver1.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); FCL_CHECK(distance <= 0); // Compute global vertices for (std::size_t i = 0; i < 8; ++i) vertices[i] = tf2.transform(vertices[i]); // Sort the vertices so that the lowest vertex along z-axis comes first std::sort(vertices.begin(), vertices.end(), compareContactPoints); // The lowest vertex along z-axis should be the contact point FCL_CHECK(normal.isApprox(Vec3s(0, 0, 1), Scalar(1e-6))); Vec3s point = Scalar(0.5) * (p1 + p2); FCL_CHECK(vertices[0].head<2>().isApprox(point.head<2>(), Scalar(1e-6))); FCL_CHECK(vertices[0][2] <= point[2] && point[2] < 0); } BOOST_AUTO_TEST_CASE(collide_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // Scalar depth; Vec3s normal; Quats q; q = AngleAxis((Scalar)3.140 / 6, UnitZ); tf1 = Transform3s(); tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (1, 0, 0). normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (1, 0, 0). normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(15, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, Scalar(1e-8)); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(15.01), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(15.01), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(q); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = transform; tf2 = transform * Transform3s(q); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); int numTests = 100; for (int i = 0; i < numTests; ++i) { Transform3s tf; generateRandomTransform(extents, tf); SET_LINE; testBoxBoxContactPoints(tf.getRotation()); } } BOOST_AUTO_TEST_CASE(collide_spherebox) { Sphere s1(20); Box s2(5, 5, 5); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (-1, 0, 0). normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(22.50001), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(22.501), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(22.4), 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(22.4), 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); } BOOST_AUTO_TEST_CASE(distance_spherebox) { coal::Matrix3s rotSphere; rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; coal::Vec3s trSphere(0.0, 0.0, 0.0); coal::Matrix3s rotBox; rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; coal::Vec3s trBox(0.0, 5.0, 3.0); coal::Sphere sphere(1); coal::Box box(10, 2, 10); coal::DistanceResult result; distance(&sphere, Transform3s(rotSphere, trSphere), &box, Transform3s(rotBox, trBox), DistanceRequest(true), result); Scalar eps = Eigen::NumTraits::epsilon(); BOOST_CHECK_CLOSE(result.min_distance, 3., eps); EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3s(0, 1, 0), eps); EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3s(0, 4, 0), eps); EIGEN_VECTOR_IS_APPROX(result.normal, Vec3s(0, 1, 0), eps); } BOOST_AUTO_TEST_CASE(collide_spherecapsule) { Sphere s1(20); Capsule s2(5, 10); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(24.9), 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(24.9), 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(25, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(24.999999), 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(25.1), 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(25.1), 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } BOOST_AUTO_TEST_CASE(collide_cylindercylinder) { Cylinder s1(5, 15); Cylinder s2(5, 15); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(9.9), 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(9.9), 0)); normal << 0, 1, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(9.9), 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(9.9), 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(10.01), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(10.01), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } BOOST_AUTO_TEST_CASE(collide_conecone) { Cone s1(5, 10); Cone s2(5, 10); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3s(); // z=0 is a singular points. Two normals could be returned. tf2 = Transform3s(Vec3s(Scalar(9.9), 0, Scalar(0.00001))); normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) .normalized(); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform * tf1; tf2 = transform * tf2; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(9.9), 0, Scalar(0.00001))); normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) .normalized(); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, true, tol_gjk); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(10.1), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(10.001), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(10.001), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(9.9))); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(9.9))); normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); } BOOST_AUTO_TEST_CASE(collide_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(9.9), 0, 0)); normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius)) .normalized(); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(9.9), 0, 0)); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(9.9), 0, Scalar(0.1))); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(9.9), 0, Scalar(0.1))); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(10.01), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(10.01), 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(10, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform * Transform3s(Vec3s(10, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(9.9))); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(9.9))); normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(10.01))); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(10.01))); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, 10)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(10.1))); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } BOOST_AUTO_TEST_CASE(collide_spheretriangle) { Sphere s(10); Transform3s transform; generateRandomTransform(extents, transform); Vec3s normal; // // Testing collision x, y, z // { Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); // identity tf_tri.setTranslation(Vec3s(0, 0, Scalar(0.001))); normal << 0, 0, 1; SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, 0, Scalar(-0.001))); normal << 0, 0, -1; SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); } { Vec3s t[3]; t[0] << 30, 0, 0; t[1] << Scalar(9.9), -20, 0; t[2] << Scalar(9.9), 20, 0; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0, Scalar(0.001))); normal << Scalar(9.9), 0, Scalar(0.001); normal.normalize(); SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, 0, Scalar(-0.001))); normal << Scalar(9.9), 0, Scalar(-0.001); normal.normalize(); SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); } { Vec3s t[3]; t[0] << 30, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, Scalar(0.001), 0)); normal << 0, 1, 0; SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, Scalar(-0.001), 0)); normal << 0, -1, 0; SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); } { Vec3s t[3]; t[0] << 0, 30, 0; t[1] << 0, -10, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(Scalar(0.001), 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(Scalar(-0.001), 0, 0)); normal << -1, 0, 0; testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); SET_LINE; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); } // // Testing no collision x, y, z // { Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0, Scalar(10.1))); SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } { Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, Scalar(10.1), 0)); SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0, Scalar(-10.1), 0)); SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } { Vec3s t[3]; t[0] << 0, 20, 0; t[1] << 0, -20, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(Scalar(10.1), 0, 0)); SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(Scalar(-10.1), 0, 0)); SET_LINE; testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } } BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { Halfspace hs(Vec3s(0, 0, 1), 0); Transform3s transform; generateRandomTransform(extents, transform); Vec3s normal; normal = hs.n; // with halfspaces, it's simple: normal is always // the normal of the halfspace. { Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); // identity tf_tri.setTranslation(Vec3s(0, 0, Scalar(-0.001))); normal = hs.n; SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, 0, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(1, 1, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(-1, -1, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } { Vec3s t[3]; t[0] << 30, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0, Scalar(-0.001))); normal = hs.n; SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, 0, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(1, 1, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(-1, -1, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } { Vec3s t[3]; t[0] << 0, 30, 0; t[1] << 0, -10, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0, Scalar(-0.001))); normal = hs.n; SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, 0, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(1, 1, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(-1, -1, Scalar(0.001))); SET_LINE; testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } } BOOST_AUTO_TEST_CASE(collide_planetriangle) { Transform3s transform; generateRandomTransform(extents, transform); Vec3s normal; { Vec3s t[3]; t[0] << 20, 0, Scalar(0.05); t[1] << -20, 0, Scalar(0.05); t[2] << 0, 20, Scalar(-0.1); Plane pl(Vec3s(0, 0, 1), 0); TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, 0, Scalar(0.05))); normal = pl.n; SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, 0, Scalar(-0.06))); SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0, 0, Scalar(0.11))); SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } { Vec3s t[3]; t[0] << 30, Scalar(0.05), 0; t[1] << -20, Scalar(0.05), 0; t[2] << 0, Scalar(-0.1), 20; Plane pl(Vec3s(0, 1, 0), 0); TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, Scalar(0.05), 0)); normal = pl.n; SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(0, Scalar(-0.06), 0)); SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0, Scalar(0.11), 0)); SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } { Vec3s t[3]; t[0] << Scalar(0.05), 30, 0; t[1] << Scalar(0.05), -10, 0; t[2] << Scalar(-0.1), 0, 20; Plane pl(Vec3s(1, 0, 0), 0); TriangleP tri(t[0], t[1], t[2]); Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(Scalar(0.05), 0, 0)); normal = pl.n; SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); tf_tri.setTranslation(Vec3s(Scalar(-0.06), 0, 0)); SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(Scalar(0.11), 0, 0)); SET_LINE; testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } } BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { Sphere s(10); Halfspace hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); contact << -5, 0, 0; depth = -10; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(-5, 0, 0)); depth = -10; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(5, 0, 0)); contact << -2.5, 0, 0; depth = -15; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(5, 0, 0)); contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -15; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-5, 0, 0)); contact << -7.5, 0, 0; depth = -5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-5, 0, 0)); contact = transform.transform(Vec3s(-7.5, 0, 0)); depth = -5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-10.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-10.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(10.1), 0, 0)); contact << Scalar(0.05), 0, 0; depth = Scalar(-20.1); normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(10.1), 0, 0)); contact = transform.transform(Vec3s(Scalar(0.05), 0, 0)); depth = Scalar(-20.1); normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); } BOOST_AUTO_TEST_CASE(collide_planesphere) { Sphere s(10); Plane hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; Vec3s p1, p2; Scalar eps = Scalar(1e-6); tf1 = Transform3s(Vec3s(eps, 0, 0)); tf2 = Transform3s(); depth = -10 + eps; p1 << -10 + eps, 0, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; normal << -1, 0, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); normal = transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = Scalar(-1e-6); tf1 = Transform3s(Vec3s(eps, 0, 0)); tf2 = Transform3s(); depth = -10 - eps; p1 << 10 + eps, 0, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(5, 0, 0)); p1 << 10, 0, 0; p2 << 5, 0, 0; contact << (p1 + p2) / 2; depth = -5; normal << 1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -5; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-5, 0, 0)); p1 << -10, 0, 0; p2 << -5, 0, 0; contact << (p1 + p2) / 2; depth = -5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-10.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-10.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(10.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(10.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacebox) { Box s(5, 10, 20); Halfspace hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); contact << -1.25, 0, 0; depth = -2.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(1.25, 0, 0)); contact << -0.625, 0, 0; depth = -3.75; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(1.25, 0, 0)); contact = transform.transform(Vec3s(-0.625, 0, 0)); depth = -3.75; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-1.25, 0, 0)); contact << -1.875, 0, 0; depth = -1.25; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-1.25, 0, 0)); contact = transform.transform(Vec3s(-1.875, 0, 0)); depth = -1.25; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(2.51), 0, 0)); contact << Scalar(0.005), 0, 0; depth = Scalar(-5.01); normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(2.51), 0, 0)); contact = transform.transform(Vec3s(Scalar(0.005), 0, 0)); depth = Scalar(-5.01); normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-2.51), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-2.51), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(transform.getRotation()); tf2 = Transform3s(); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true); } BOOST_AUTO_TEST_CASE(collide_planebox) { Box s(5, 10, 20); Plane hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); Vec3s p1(2.5, 0, 0); Vec3s p2(0, 0, 0); contact << (p1 + p2) / 2; depth = -2.5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(1.25, 0, 0)); p2 << 1.25, 0, 0; contact << (p1 + p2) / 2; depth = -1.25; normal << 1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(1.25, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -1.25; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-1.25, 0, 0)); p1 << -2.5, 0, 0; p2 << -1.25, 0, 0; contact << (p1 + p2) / 2; depth = -1.25; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-1.25, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -1.25; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(2.51), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(2.51), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-2.51), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-2.51), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(transform.getRotation()); tf2 = Transform3s(); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true); } BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { Capsule s(5, 10); Halfspace hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); contact << -2.5, 0, 0; depth = -5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, 0; depth = -7.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -7.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = -2.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform(Vec3s(-3.75, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(5.1), 0, 0)); contact << Scalar(0.05), 0, 0; depth = Scalar(-10.1); normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(5.1), 0, 0)); contact = transform.transform(Vec3s(Scalar(0.05), 0, 0)); depth = Scalar(-10.1); normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 1, 0), 0); tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, -2.5, 0; depth = -5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, 0; depth = -7.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform(Vec3s(0, -1.25, 0)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, 0; depth = -2.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform(Vec3s(0, -3.75, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(5.1), 0)); contact << 0, Scalar(0.05), 0; depth = Scalar(-10.1); normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(5.1), 0)); contact = transform.transform(Vec3s(0, Scalar(0.05), 0)); depth = Scalar(-10.1); normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 0, 1), 0); tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, 0, -5; depth = -10; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, 0, -5)); depth = -10; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -3.75; depth = -12.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -12.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -6.25; depth = -7.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform(Vec3s(0, 0, -6.25)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(10.1))); contact << 0, 0, Scalar(0.05); depth = Scalar(-20.1); normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(10.1))); contact = transform.transform(Vec3s(0, 0, Scalar(0.05))); depth = Scalar(-20.1); normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecapsule) { Capsule s(5, 10); Plane hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, 0, 0; depth = -5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, 0, 0)); depth = -5; normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << 2.5, 0, 0; depth = -2.5; normal << 1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform(Vec3s(2.5, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -2.5, 0, 0; depth = -2.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 1, 0), 0); tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, 0, 0; depth = -5; normal << 0, 1, 0; // (0, 1, 0) or (0, -1, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, 0, 0)); depth = -5; normal = transform.getRotation() * Vec3s(0, 1, 0); // (0, 1, 0) or (0, -1, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, 2.5, 0; depth = -2.5; normal << 0, 1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform(Vec3s(0, 2.5, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -2.5, 0; depth = -2.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 0, 1), 0); tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, 0, 0; depth = -10; normal << 0, 0, 1; // (0, 0, 1) or (0, 0, -1) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, 0, 0)); depth = -10; normal = transform.getRotation() * Vec3s(0, 0, 1); // (0, 0, 1) or (0, 0, -1) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, 2.5; depth = -7.5; normal << 0, 0, 1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform(Vec3s(0, 0, 2.5)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -2.5; depth = -7.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { Cylinder s(5, 10); Halfspace hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); contact << -2.5, 0, 0; depth = -5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, 0; depth = -7.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -7.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = -2.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform(Vec3s(-3.75, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(5.1), 0, 0)); contact << Scalar(0.05), 0, 0; depth = Scalar(-10.1); normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(5.1), 0, 0)); contact = transform.transform(Vec3s(Scalar(0.05), 0, 0)); depth = Scalar(-10.1); normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 1, 0), 0); tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, -2.5, 0; depth = -5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, 0; depth = -7.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform(Vec3s(0, -1.25, 0)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, 0; depth = -2.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform(Vec3s(0, -3.75, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(5.1), 0)); contact << 0, Scalar(0.05), 0; depth = Scalar(-10.1); normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(5.1), 0)); contact = transform.transform(Vec3s(0, Scalar(0.05), 0)); depth = Scalar(-10.1); normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 0, 1), 0); tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, 0, -2.5; depth = -5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -1.25; depth = -7.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform(Vec3s(0, 0, -1.25)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -3.75; depth = -2.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(5.1))); contact << 0, 0, Scalar(0.05); depth = Scalar(-10.1); normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(5.1))); contact = transform.transform(Vec3s(0, 0, Scalar(0.05))); depth = Scalar(-10.1); normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(-5.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(-5.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecylinder) { Cylinder s(5, 10); Plane hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; Vec3s p1, p2; Scalar eps = Scalar(1e-6); tf1 = Transform3s(Vec3s(eps, 0, 0)); tf2 = Transform3s(); p1 << -5 + eps, 0, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; depth = -5 + eps; normal << -1, 0, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = Scalar(-1e-6); tf1 = Transform3s(Vec3s(eps, 0, 0)); tf2 = Transform3s(); p1 << 5 + eps, 0, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; depth = -5 - eps; normal << -1, 0, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(2.5, 0, 0)); p1 << 5, 0, 0; p2 << 2.5, 0, 0; contact << (p1 + p2) / 2; depth = -2.5; normal << 1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-2.5, 0, 0)); p1 << -5, 0, 0; p2 << -2.5, 0, 0; contact << (p1 + p2) / 2; depth = -2.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 1, 0), 0); eps = Scalar(1e-6); tf1 = Transform3s(Vec3s(0, eps, 0)); tf2 = Transform3s(); p1 << 0, -5 + eps, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; depth = -5 + eps; normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = Scalar(-1e-6); tf1 = Transform3s(Vec3s(0, eps, 0)); tf2 = Transform3s(); p1 << 0, 5 + eps, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; depth = -5 - eps; normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 2.5, 0)); p1 << 0, 5, 0; p2 << 0, 2.5, 0; contact << (p1 + p2) / 2; depth = -2.5; normal << 0, 1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, -2.5, 0)); p1 << 0, -5, 0; p2 << 0, -2.5, 0; contact << (p1 + p2) / 2; depth = -2.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 0, 1), 0); eps = Scalar(1e-6); tf1 = Transform3s(Vec3s(0, 0, eps)); tf2 = Transform3s(); p1 << 0, 0, -5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; depth = -5 + eps; normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = Scalar(-1e-6); tf1 = Transform3s(Vec3s(0, 0, eps)); tf2 = Transform3s(); p1 << 0, 0, 5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; depth = -5 - eps; normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, 2.5)); p1 << 0, 0, 5; p2 << 0, 0, 2.5; contact << (p1 + p2) / 2; depth = -2.5; normal << 0, 0, 1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, -2.5)); p1 << 0, 0, -5.; p2 << 0, 0, -2.5; contact << (p1 + p2) / 2; depth = -2.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacecone) { Cone s(5, 10); Halfspace hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; tf1 = Transform3s(); tf2 = Transform3s(); contact << -2.5, 0, -5; depth = -5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(-2.5, 0, -5)); depth = -5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, -5; depth = -7.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform(Vec3s(-1.25, 0, -5)); depth = -7.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, -5; depth = -2.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform(Vec3s(-3.75, 0, -5)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(5.1), 0, 0)); contact << Scalar(0.05), 0, -5; depth = Scalar(-10.1); normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(5.1), 0, 0)); contact = transform.transform(Vec3s(Scalar(0.05), 0, -5)); depth = Scalar(-10.1); normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 1, 0), 0); tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, -2.5, -5; depth = -5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, -2.5, -5)); depth = -5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, -5; depth = -7.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform(Vec3s(0, -1.25, -5)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, -5; depth = -2.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform(Vec3s(0, -3.75, -5)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(5.1), 0)); contact << 0, Scalar(0.05), -5; depth = Scalar(-10.1); normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(5.1), 0)); contact = transform.transform(Vec3s(0, Scalar(0.05), -5)); depth = Scalar(-10.1); normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 0, 1), 0); tf1 = Transform3s(); tf2 = Transform3s(); contact << 0, 0, -2.5; depth = -5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -1.25; depth = -7.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform(Vec3s(0, 0, -1.25)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -3.75; depth = -2.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(5.1))); contact << 0, 0, Scalar(0.05); depth = Scalar(-10.1); normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(5.1))); contact = transform.transform(Vec3s(0, 0, Scalar(0.05))); depth = Scalar(-10.1); normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(-5.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(-5.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecone) { Cone s(5, 10); Plane hs(Vec3s(1, 0, 0), 0); Transform3s tf1; Transform3s tf2; Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; Scalar depth; Vec3s normal; Vec3s p1, p2; Scalar eps = Scalar(1e-6); tf1 = Transform3s(Vec3s(eps, 0, 0)); tf2 = Transform3s(); p1 << -5 + eps, 0, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; depth = -5 + eps; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = Scalar(-1e-6); tf1 = Transform3s(Vec3s(eps, 0, 0)); tf2 = Transform3s(); p1 << 5 + eps, 0, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; depth = -5 - eps; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(2.5, 0, 0)); p1 << 5, 0, -5; p2 << 2.5, 0, -5; contact << (p1 + p2) / 2; depth = -2.5; normal << 1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(-2.5, 0, 0)); p1 << -5, 0, -5; p2 << -2.5, 0, -5; contact << (p1 + p2) / 2; depth = -2.5; normal << -1, 0, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(Scalar(-5.1), 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 1, 0), 0); eps = Scalar(1e-6); tf1 = Transform3s(Vec3s(0, eps, 0)); tf2 = Transform3s(); p1 << 0, -5 + eps, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; depth = -5 + eps; normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = Scalar(-1e-6); tf1 = Transform3s(Vec3s(0, eps, 0)); tf2 = Transform3s(); p1 << 0, 5 + eps, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; depth = -5 - eps; normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 2.5, 0)); p1 << 0, 5, -5; p2 << 0, 2.5, -5; contact << (p1 + p2) / 2; depth = -2.5; normal << 0, 1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, -2.5, 0)); p1 << 0, -5, -5; p2 << 0, -2.5, -5; contact << (p1 + p2) / 2; depth = -2.5; normal << 0, -1, 0; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, Scalar(-5.1), 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 0, 1), 0); eps = Scalar(1e-6); tf1 = Transform3s(Vec3s(0, 0, eps)); tf2 = Transform3s(); p1 << 0, 0, -5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; depth = -5 + eps; normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = Scalar(-1e-6); tf1 = Transform3s(Vec3s(0, 0, eps)); tf2 = Transform3s(); p1 << 0, 0, 5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; depth = -5 - eps; normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, 2.5)); p1 << 0, 0, 5; p2 << 0, 0, 2.5; contact << (p1 + p2) / 2; depth = -2.5; normal << 0, 0, 1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, -2.5)); p1 << 0, 0, -5; p2 << 0, 0, -2.5; contact << (p1 + p2) / 2; depth = -2.5; normal << 0, 0, -1; SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3s(); tf2 = Transform3s(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3s(Vec3s(0, 0, Scalar(-10.1))); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planeplane) { Transform3s tf1; Transform3s tf2; Vec3s normal; Vec3s contact; Scalar distance; Transform3s transform; generateRandomTransform(extents, transform); { Vec3s n = Vec3s::Random().normalized(); Scalar offset = Scalar(3.14); Plane plane1(n, offset); Plane plane2(n, offset); tf1.setIdentity(); tf2.setIdentity(); normal = n; contact = plane1.n * plane1.d; distance = 0.; SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, true, &contact, &distance, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; contact = transform.getRotation() * plane1.n * (plane1.d + (transform.getRotation() * plane1.n).dot(transform.getTranslation())); SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, true, &contact, &distance, &normal); } { Vec3s n = Vec3s::Random().normalized(); Scalar offset1 = Scalar(3.14); Scalar offset2 = offset1 + Scalar(1.19841); Plane plane1(n, offset1); Plane plane2(n, offset2); tf1.setIdentity(); tf2.setIdentity(); SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, false); tf1 = transform; tf2 = transform; SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, false); } { Vec3s n = Vec3s::Random().normalized(); Scalar offset1 = Scalar(3.14); Scalar offset2 = offset1 - Scalar(1.19841); Plane plane1(n, offset1); Plane plane2(n, offset2); tf1.setIdentity(); tf2.setIdentity(); SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, false); tf1 = transform; tf2 = transform; SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, false); } { Vec3s n1(1, 0, 0); Scalar offset1 = Scalar(3.14); Plane plane1(n1, offset1); Vec3s n2(0, 0, 1); Scalar offset2 = Scalar(-2.13); Plane plane2(n2, offset2); tf1.setIdentity(); tf2.setIdentity(); normal << 0, -1, 0; contact << offset1, 0, offset2; SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, true, &contact, NULL, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, true, NULL, NULL, &normal); } { Vec3s n1(1, 0, 0); Scalar offset1 = Scalar(3.14); Plane plane1(n1, offset1); Vec3s n2(1, 1, 1); Scalar offset2 = Scalar(-2.13); Plane plane2(n2, offset2); tf1.setIdentity(); tf2.setIdentity(); normal << 0, Scalar(-0.5774), Scalar(0.5774); SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, true, NULL, NULL, &normal, false, Scalar(1e-3)); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(plane1, tf1, plane2, tf2, true, NULL, NULL, &normal, false, Scalar(1e-3)); } } BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { Transform3s tf1; Transform3s tf2; Vec3s normal; Vec3s contact; Scalar distance; Transform3s transform; generateRandomTransform(extents, transform); { Vec3s n = Vec3s::Random().normalized(); Scalar offset = Scalar(3.14); Halfspace hf1(n, offset); Halfspace hf2(n, offset); tf1.setIdentity(); tf2.setIdentity(); normal = n; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, NULL, &normal); } { Vec3s n = Vec3s::Random().normalized(); Scalar offset1 = Scalar(3.14); Scalar offset2 = offset1 + Scalar(1.19841); Halfspace hf1(n, offset1); Halfspace hf2(n, offset2); tf1.setIdentity(); tf2.setIdentity(); normal = n; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, NULL, &normal); } { Vec3s n = Vec3s::Random().normalized(); Scalar offset1 = Scalar(3.14); Scalar offset2 = offset1 - Scalar(1.19841); Halfspace hf1(n, offset1); Halfspace hf2(-n, -offset2); tf1.setIdentity(); tf2.setIdentity(); normal = n; distance = offset2 - offset1; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, &distance, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, &distance, &normal); } { Vec3s n1(1, 0, 0); Scalar offset1 = Scalar(3.14); Halfspace hf1(n1, offset1); Vec3s n2(0, 0, 1); Scalar offset2 = Scalar(-2.13); Halfspace hf2(n2, offset2); tf1.setIdentity(); tf2.setIdentity(); normal << 0, -1, 0; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, NULL, &normal); } { Vec3s n1(1, 0, 0); Scalar offset1 = Scalar(3.14); Halfspace hf1(n1, offset1); Vec3s n2(1, 1, 1); Scalar offset2 = Scalar(-2.13); Halfspace hf2(n2, offset2); tf1.setIdentity(); tf2.setIdentity(); normal << 0, Scalar(-0.5774), Scalar(0.5774); SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, NULL, &normal, false, Scalar(1e-3)); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf1, tf1, hf2, tf2, true, NULL, NULL, &normal, false, Scalar(1e-3)); } } BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { Transform3s tf1; Transform3s tf2; Vec3s normal; Vec3s contact; Scalar distance; Transform3s transform; generateRandomTransform(extents, transform); { Vec3s n = Vec3s::Random().normalized(); Scalar offset = Scalar(3.14); Halfspace hf(n, offset); Plane plane(n, offset); tf1.setIdentity(); tf2.setIdentity(); normal = n; distance = 0; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, true, NULL, &distance, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, true, NULL, &distance, &normal); } { Vec3s n = Vec3s::Random().normalized(); Scalar offset1 = Scalar(3.14); Scalar offset2 = offset1 + Scalar(1.19841); Halfspace hf(n, offset1); Plane plane(n, offset2); tf1.setIdentity(); tf2.setIdentity(); normal = n; distance = offset2 - offset1; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, false); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, false); } { Vec3s n = Vec3s::Random().normalized(); Scalar offset1 = Scalar(3.14); Scalar offset2 = offset1 - Scalar(1.19841); Halfspace hf(n, offset1); Plane plane(n, offset2); tf1.setIdentity(); tf2.setIdentity(); normal = n; distance = offset2 - offset1; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, true, NULL, &distance, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, true, NULL, &distance, &normal); } { Vec3s n1(1, 0, 0); Scalar offset1 = Scalar(3.14); Halfspace hf(n1, offset1); Vec3s n2(0, 0, 1); Scalar offset2 = Scalar(-2.13); Plane plane(n2, offset2); tf1.setIdentity(); tf2.setIdentity(); normal << 0, -1, 0; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, true, NULL, NULL, &normal); } { Vec3s n1(1, 0, 0); Scalar offset1 = Scalar(3.14); Halfspace hf(n1, offset1); Vec3s n2(1, 1, 1); Scalar offset2 = Scalar(-2.13); Plane plane(n2, offset2); tf1.setIdentity(); tf2.setIdentity(); normal << 0, Scalar(-0.5774), Scalar(0.5774); SET_LINE; testShapeCollide(hf, tf1, plane, tf2, true, NULL, NULL, &normal, false, Scalar(1e-3)); tf1 = transform; tf2 = transform; normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hf, tf1, plane, tf2, true, NULL, NULL, &normal, false, Scalar(1e-3)); } } BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) { Sphere s1(20); Sphere s2(10); Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3s transform; generateRandomTransform(extents, transform); Scalar dist = -1; dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(Scalar(30.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(Scalar(29.9), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, Transform3s(Vec3s(40, 0, 0)), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance(s1, Transform3s(Vec3s(Scalar(30.1), 0, 0)), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance(s1, Transform3s(Vec3s(Scalar(29.9), 0, 0)), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(Scalar(30.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(Scalar(29.9), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(40, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( s1, transform * Transform3s(Vec3s(Scalar(30.1), 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, transform * Transform3s(Vec3s(Scalar(29.9), 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); } BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3s transform; generateRandomTransform(extents, transform); Scalar dist; dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance( s2, Transform3s(), s2, Transform3s(Vec3s(Scalar(10.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s2, Transform3s(), s2, Transform3s(Vec3s(Scalar(20.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver1.shapeDistance( s2, Transform3s(), s2, Transform3s(Vec3s(0, Scalar(20.2), 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.2) < 0.001); dist = solver1.shapeDistance( s2, Transform3s(), s2, Transform3s(Vec3s(Scalar(10.1), Scalar(10.1), 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); dist = solver2.shapeDistance( s2, Transform3s(), s2, Transform3s(Vec3s(Scalar(10.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver2.shapeDistance( s2, Transform3s(), s2, Transform3s(Vec3s(Scalar(20.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver2.shapeDistance( s2, Transform3s(), s2, Transform3s(Vec3s(0, Scalar(20.1), 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver2.shapeDistance( s2, Transform3s(), s2, Transform3s(Vec3s(Scalar(10.1), Scalar(10.1), 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(Scalar(15.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(20, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(20, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); } BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) { Cylinder s1(Scalar(0.029), Scalar(0.1)); Box s2(Scalar(1.6), Scalar(0.6), Scalar(0.025)); using Quat = Eigen::Quaternion; Transform3s tf1( Quat(Scalar(0.5279170511703305), Scalar(-0.50981118132505521), Scalar(-0.67596178682051911), Scalar(0.0668715876735793)), Vec3s(Scalar(0.041218354748013122), Scalar(1.2022554710435607), Scalar(0.77338855025700015))); Transform3s tf2( Quat(Scalar(0.70738826916719977), 0, 0, Scalar(0.70682518110536596)), Vec3s(Scalar(-0.29936284351096382), Scalar(0.80023864435868775), Scalar(0.71750000000000003))); GJKSolver solver; Vec3s p1, p2, normal; bool compute_penetration = true; solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); // If objects are not colliding, p2 should be outside the cylinder and // p1 should be outside the box Vec3s p2Loc(tf1.inverse().transform(p2)); bool p2_in_cylinder((fabs(p2Loc[2]) <= s1.halfLength) && (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius)); Vec3s p1Loc(tf2.inverse().transform(p1)); bool p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all(); std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl; std::cout << "p1 in box = (" << p1Loc.transpose() << ")" << std::endl; BOOST_CHECK((!p2_in_cylinder && !p1_in_box) || (p2_in_cylinder && p1_in_box)); solver.shapeDistance(s2, tf2, s1, tf1, compute_penetration, p2, p1, normal); // If objects are not colliding, p2 should be outside the cylinder and // p1 should be outside the box p2Loc = tf1.inverse().transform(p2); p2_in_cylinder = (fabs(p2Loc[2]) <= s1.halfLength) && (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius); p1Loc = tf2.inverse().transform(p1); p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all(); std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl; std::cout << "p1 in box = (" << p1.transpose() << ")" << std::endl; BOOST_CHECK((!p2_in_cylinder && !p1_in_box) || (p2_in_cylinder && p1_in_box)); s1 = Cylinder(Scalar(0.06), Scalar(0.1)); tf1.setTranslation(Vec3s(Scalar(-0.66734052046473924), Scalar(0.22219183277457269), Scalar(0.76825248755616293))); using Quat = Eigen::Quaternion; tf1.setQuatRotation( Quat(Scalar(0.52613359459338371), Scalar(0.32189408354839893), Scalar(0.70415587451837913), Scalar(-0.35175580165512249))); solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); } BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { Sphere s1(20); Box s2(5, 5, 5); Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3s transform; generateRandomTransform(extents, transform); Scalar dist; int N = 10; for (int i = 0; i < N + 1; ++i) { Scalar dbox = Scalar(0.0001) + (s1.radius + s2.halfSide(0)) * Scalar(i) * 4 / (3 * Scalar(N)); dist = solver1.shapeDistance(s1, Transform3s(Vec3s(dbox, 0., 0.)), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), Scalar(1e-6)); EIGEN_VECTOR_IS_APPROX(normal, -Vec3s(1, 0, 0), Scalar(1e-6)); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); dist = solver1.shapeDistance( s1, transform * Transform3s(Vec3s(dbox, 0., 0.)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), Scalar(1e-6)); EIGEN_VECTOR_IS_APPROX(normal, -transform.getRotation().col(0), Scalar(1e-6)); } dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(Scalar(22.6), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(Scalar(22.6), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); } BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3s transform; generateRandomTransform(extents, transform); Scalar dist; { // The following situations corresponds to the case where the two cylinders // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. Scalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = Scalar(1e-2); solver1.epa_max_iterations = 1000; dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); // We restore the original values of the EPA parameters solver1.epa_tolerance = epa_tolerance_backup; solver1.epa_max_iterations = epa_max_iterations_backup; } dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(Scalar(10.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(Scalar(10.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); } BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { Cone s1(5, 10); Cone s2(5, 10); Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3s transform; generateRandomTransform(extents, transform); Scalar dist; { // The following situations corresponds to the case where the two cones // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. Scalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = Scalar(1e-2); solver1.epa_max_iterations = 1000; dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); // We restore the original values of the EPA parameters solver1.epa_tolerance = epa_tolerance_backup; solver1.epa_max_iterations = epa_max_iterations_backup; } dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(Scalar(10.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(Scalar(10.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(0, 0, 40)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(0, 0, 40)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); } BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3s transform; generateRandomTransform(extents, transform); Scalar dist; { // The following situations corresponds to the case where the two cones // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. Scalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = Scalar(1e-2); solver1.epa_max_iterations = 1000; dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); // We restore the original values of the EPA parameters solver1.epa_tolerance = epa_tolerance_backup; solver1.epa_max_iterations = epa_max_iterations_backup; } dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(Scalar(10.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(Scalar(10.1), 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.02); dist = solver1.shapeDistance( s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.01); dist = solver1.shapeDistance( s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.1); } template void testReversibleShapeDistance(const S1& s1, const S2& s2, Scalar distance) { const Scalar half = Scalar(0.5); Transform3s tf1(Vec3s(-half * distance, 0.0, 0.0)); Transform3s tf2(Vec3s(+half * distance, 0.0, 0.0)); Scalar distA; Scalar distB; Vec3s p1A; Vec3s p1B; Vec3s p2A; Vec3s p2B; Vec3s normalA, normalB; bool compute_penetration = true; const Scalar tol = Scalar(1e-6); distA = solver1.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1A, p2A, normalA); distB = solver1.shapeDistance(s2, tf2, s1, tf1, compute_penetration, p1B, p2B, normalB); assert((distA <= 0 && distB <= 0) || (distA > 0 && distB > 0)); BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same BOOST_CHECK( isEqual(p1A, p2B, tol)); // closest points should in reverse order BOOST_CHECK(isEqual(p2A, p1B, tol)); distA = solver2.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1A, p2A, normalA); distB = solver2.shapeDistance(s2, tf2, s1, tf1, compute_penetration, p1B, p2B, normalB); assert((distA <= 0 && distB <= 0) || (distA > 0 && distB > 0)); BOOST_CHECK(solver1.gjk.status == solver2.gjk.status); BOOST_CHECK(solver1.epa.status == solver2.epa.status); BOOST_CHECK_CLOSE(distA, distB, tol); BOOST_CHECK(isEqual(p1A, p2B, tol)); BOOST_CHECK(isEqual(p2A, p1B, tol)); } BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) { // This test check whether a shape distance algorithm is called for the // reverse case as well. For example, if FCL has sphere-capsule distance // algorithm, then this algorithm should be called for capsule-sphere case. // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, // cylinder, plane, halfspace Box box(10, 10, 10); Sphere sphere(5); Capsule capsule(5, 10); Cone cone(5, 10); Cylinder cylinder(5, 10); Plane plane(Vec3s(0, 0, 0), 0.0); Halfspace halfspace(Vec3s(0, 0, 0), 0.0); // Use sufficiently long distance so that all the primitive shapes CANNOT // intersect Scalar distance = 15.0; // If new shape distance algorithm is added for two distinct primitive // shapes, uncomment associated lines. For example, box-sphere intersection // algorithm is added, then uncomment box-sphere. // testReversibleShapeDistance(box, sphere, distance); // testReversibleShapeDistance(box, capsule, distance); // testReversibleShapeDistance(box, cone, distance); // testReversibleShapeDistance(box, cylinder, distance); // testReversibleShapeDistance(box, plane, distance); // testReversibleShapeDistance(box, halfspace, distance); SET_LINE; testReversibleShapeDistance(sphere, capsule, distance); // testReversibleShapeDistance(sphere, cone, distance); // testReversibleShapeDistance(sphere, cylinder, distance); // testReversibleShapeDistance(sphere, plane, distance); // testReversibleShapeDistance(sphere, halfspace, distance); // testReversibleShapeDistance(capsule, cone, distance); // testReversibleShapeDistance(capsule, cylinder, distance); // testReversibleShapeDistance(capsule, plane, distance); // testReversibleShapeDistance(capsule, halfspace, distance); // testReversibleShapeDistance(cone, cylinder, distance); // testReversibleShapeDistance(cone, plane, distance); // testReversibleShapeDistance(cone, halfspace, distance); // testReversibleShapeDistance(cylinder, plane, distance); // testReversibleShapeDistance(cylinder, halfspace, distance); // testReversibleShapeDistance(plane, halfspace, distance); } ================================================ FILE: test/gjk-geometric-tools-benchmark ================================================ Result after running gjk.cpp in Release mode. nCol = 2831 nDiff = 41 Total time gjk: 21461 Total time gte: 59498 Number times GJK better than Geometric tools: 9988 -- Collisions ------------------------- Total time gjk: 5219 Total time gte: 14334 Number times GJK better than Geometric tools: 2828 -- No collisions ------------------------- Total time gjk: 16242 Total time gte: 45164 Number times GJK better than Geometric tools: 7160 ================================================ FILE: test/gjk.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2018, CNRS-LAAS. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #define BOOST_TEST_MODULE COAL_GJK #include #include #include #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/tools.h" #include "coal/internal/shape_shape_func.h" #include "utility.h" using coal::GJKSolver; using coal::GJKVariant; using coal::Matrix3s; using coal::Quats; using coal::Scalar; using coal::Transform3s; using coal::TriangleP; using coal::Vec3s; typedef Eigen::Matrix vector_t; typedef Eigen::Matrix vector6_t; typedef Eigen::Matrix vector4_t; typedef Eigen::Matrix matrix_t; struct Result { bool collision; clock_t timeGjk; clock_t timeGte; }; // struct benchmark typedef std::vector Results_t; void test_gjk_distance_triangle_triangle( bool enable_gjk_nesterov_acceleration) { Eigen::IOFormat numpy(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ", "np.array ((", "))", "", ""); Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); std::size_t N = 10000; GJKSolver solver; if (enable_gjk_nesterov_acceleration) solver.gjk.gjk_variant = GJKVariant::NesterovAcceleration; Transform3s tf1, tf2; Vec3s p1, p2, a1, a2; Matrix3s M; Scalar distance(sqrt(-Scalar(1))); clock_t start, end; std::size_t nCol = 0, nDiff = 0; Scalar eps = Scalar(1e-7); Results_t results(N); for (std::size_t i = 0; i < N; ++i) { Vec3s P1_loc(Vec3s::Random()), P2_loc(Vec3s::Random()), P3_loc(Vec3s::Random()); Vec3s Q1_loc(Vec3s::Random()), Q2_loc(Vec3s::Random()), Q3_loc(Vec3s::Random()); if (i == 0) { P1_loc = Vec3s(Scalar(0.063996093749999997), Scalar(00.15320971679687501), Scalar(-0.42799999999999999)); P2_loc = Vec3s(Scalar(0.069105957031249998), Scalar(-0.150722900390625), Scalar(-0.42999999999999999)); P3_loc = Vec3s(Scalar(0.063996093749999997), Scalar(-0.15320971679687501), Scalar(-0.42999999999999999)); Q1_loc = Vec3s(Scalar(-25.655000000000001), Scalar(-1.2858199462890625), Scalar(3.7249809570312502)); Q2_loc = Vec3s(Scalar(-10.926), Scalar(-1.284259033203125), Scalar(3.7281499023437501)); Q3_loc = Vec3s(Scalar(-10.926), Scalar(-1.2866180419921875), Scalar(3.72335400390625)); Transform3s tf( Quats(Scalar(-0.42437287410898855), Scalar(-0.26862477561450587), Scalar(-0.46249645019513175), Scalar(0.73064726592483387)), Vec3s(Scalar(-12.824601270753471), Scalar(-1.6840516940066426), Scalar(3.8914453043793844))); tf1 = tf; } else if (i == 1) { P1_loc = Vec3s(Scalar(-0.8027043342590332), Scalar(-0.30276307463645935), Scalar(-0.4372950792312622)); P2_loc = Vec3s(Scalar(-0.8027043342590332), Scalar(0.30276307463645935), Scalar(-0.4372950792312622)); P3_loc = Vec3s(Scalar(0.8027043342590332), Scalar(0.30276307463645935), Scalar(-0.4372950792312622)); Q1_loc = Vec3s(Scalar(-0.224713996052742), Scalar(-0.7417119741439819), Scalar(0.19999997317790985)); Q2_loc = Vec3s(Scalar(-0.5247139930725098), Scalar(-0.7417119741439819), Scalar(0.19999997317790985)); Q3_loc = Vec3s(Scalar(-0.224713996052742), Scalar(-0.7417119741439819), Scalar(0.09999997168779373)); Matrix3s R; Vec3s T; R << Scalar(0.9657787025454787), Scalar(0.09400415350535746), Scalar(0.24173273843919627), Scalar(-0.06713698817647556), Scalar(0.9908494114820345), Scalar(-0.11709000206805695), Scalar(-0.25052768814676646), Scalar(0.09685382227587608), Scalar(0.9632524147814993); T << Scalar(-0.13491177905469953), -1, Scalar(0.6000449621843792); tf1.setRotation(R); tf1.setTranslation(T); } else { tf1 = Transform3s(); } TriangleP tri1(P1_loc, P2_loc, P3_loc); TriangleP tri2(Q1_loc, Q2_loc, Q3_loc); Vec3s normal; const bool compute_penetration = true; coal::DistanceRequest request(compute_penetration, compute_penetration); coal::DistanceResult result; start = clock(); // The specialized function TriangleP-TriangleP calls GJK to check for // collision and compute the witness points but it does not use EPA to // compute the penetration depth. distance = coal::ShapeShapeDistance( &tri1, tf1, &tri2, tf2, &solver, request, result); end = clock(); p1 = result.nearest_points[0]; p2 = result.nearest_points[1]; normal = result.normal; bool res = (distance <= 0); results[i].timeGjk = end - start; results[i].collision = res; if (res) { Vec3s c1, c2, normal2; ++nCol; // check that moving triangle 2 by the penetration depth in the // direction of the normal makes the triangles collision free. Scalar penetration_depth(-distance); assert(penetration_depth >= 0); tf2.setTranslation((penetration_depth + 10 - 4) * normal); result.clear(); distance = coal::ShapeShapeDistance( &tri1, tf1, &tri2, tf2, &solver, request, result); c1 = result.nearest_points[0]; c2 = result.nearest_points[1]; normal2 = result.normal; res = (distance <= 0); if (res) { std::cerr << "P1 = " << P1_loc.format(tuple) << std::endl; std::cerr << "P2 = " << P2_loc.format(tuple) << std::endl; std::cerr << "P3 = " << P3_loc.format(tuple) << std::endl; std::cerr << "Q1 = " << Q1_loc.format(tuple) << std::endl; std::cerr << "Q2 = " << Q2_loc.format(tuple) << std::endl; std::cerr << "Q3 = " << Q3_loc.format(tuple) << std::endl; std::cerr << "p1 = " << c1.format(tuple) << std::endl; std::cerr << "p2 = " << c2.format(tuple) << std::endl; std::cerr << "tf1 = " << tf1.getTranslation().format(tuple) << " + " << tf1.getQuatRotation().coeffs().format(tuple) << std::endl; std::cerr << "tf2 = " << tf2.getTranslation().format(tuple) << " + " << tf2.getQuatRotation().coeffs().format(tuple) << std::endl; std::cerr << "normal = " << normal.format(tuple) << std::endl; abort(); } distance = 0; tf2.setIdentity(); } // Compute vectors between vertices Vec3s P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)), P3(tf1.transform(P3_loc)), Q1(tf2.transform(Q1_loc)), Q2(tf2.transform(Q2_loc)), Q3(tf2.transform(Q3_loc)); Vec3s u1(P2 - P1); Vec3s v1(P3 - P1); Vec3s w1(u1.cross(v1)); Vec3s u2(Q2 - Q1); Vec3s v2(Q3 - Q1); Vec3s w2(u2.cross(v2)); BOOST_CHECK(w1.squaredNorm() > eps * eps); M.col(0) = u1; M.col(1) = v1; M.col(2) = w1; // Compute a1 such that p1 = P1 + a11 u1 + a12 v1 + a13 u1 x v1 a1 = M.inverse() * (p1 - P1); EIGEN_VECTOR_IS_APPROX(p1, P1 + a1[0] * u1 + a1[1] * v1, eps); BOOST_CHECK(w2.squaredNorm() > eps * eps); // Compute a2 such that p2 = Q1 + a21 u2 + a22 v2 + a23 u2 x v2 M.col(0) = u2; M.col(1) = v2; M.col(2) = w2; a2 = M.inverse() * (p2 - Q1); EIGEN_VECTOR_IS_APPROX(p2, Q1 + a2[0] * u2 + a2[1] * v2, eps); // minimal distance and closest points can be considered as a constrained // optimisation problem: // // min f (a1[0],a1[1], a2[0],a2[1]) // g1 (a1[0],a1[1], a2[0],a2[1]) <=0 // ... // g6 (a1[0],a1[1], a2[0],a2[1]) <=0 // with // f (a1[0],a1[1], a2[0],a2[1]) = // 1 2 // --- dist (P1 + a1[0] u1 + a1[1] v1, Q1 + a2[0] u2 + a2[1] v2), // 2 // g1 (a1[0],a1[1], a2[0],a2[1]) = -a1[0] // g2 (a1[0],a1[1], a2[0],a2[1]) = -a1[1] // g3 (a1[0],a1[1], a2[0],a2[1]) = a1[0] + a1[1] - 1 // g4 (a1[0],a1[1], a2[0],a2[1]) = -a2[0] // g5 (a1[0],a1[1], a2[0],a2[1]) = -a2[1] // g6 (a1[0],a1[1], a2[0],a2[1]) = a2[0] + a2[1] - 1 // Compute gradient of f vector4_t grad_f; grad_f[0] = -(p2 - p1).dot(u1); grad_f[1] = -(p2 - p1).dot(v1); grad_f[2] = (p2 - p1).dot(u2); grad_f[3] = (p2 - p1).dot(v2); vector6_t g; g[0] = -a1[0]; g[1] = -a1[1]; g[2] = a1[0] + a1[1] - 1; g[3] = -a2[0]; g[4] = -a2[1]; g[5] = a2[0] + a2[1] - 1; matrix_t grad_g(4, 6); grad_g.setZero(); grad_g(0, 0) = -1.; grad_g(1, 1) = -1; grad_g(0, 2) = 1; grad_g(1, 2) = 1; grad_g(2, 3) = -1; grad_g(3, 4) = -1; grad_g(2, 5) = 1; grad_g(3, 5) = 1; // Check that closest points are on triangles planes // Projection of [P1p1] on line normal to triangle 1 plane is equal to 0 BOOST_CHECK_SMALL(a1[2], eps); // Projection of [Q1p2] on line normal to triangle 2 plane is equal to 0 BOOST_CHECK_SMALL(a2[2], eps); /* Check Karush–Kuhn–Tucker conditions 6 __ \ -grad f = /_ c grad g i=1 i i where c >= 0, and i c g = 0 for i between 1 and 6 i i */ matrix_t Mkkt(4, 6); matrix_t::Index col = 0; // Check that constraints are satisfied for (vector6_t::Index j = 0; j < 6; ++j) { BOOST_CHECK(g[j] <= eps); // if constraint is saturated, add gradient in matrix if (fabs(g[j]) <= eps) { Mkkt.col(col) = grad_g.col(j); ++col; } } if (col > 0) { Mkkt.conservativeResize(4, col); // Compute KKT coefficients ci by inverting // Mkkt.c = -grad_f Eigen::JacobiSVD svd(Mkkt, Eigen::ComputeThinU | Eigen::ComputeThinV); vector_t c(svd.solve(-grad_f)); for (vector_t::Index j = 0; j < c.size(); ++j) { BOOST_CHECK_MESSAGE(c[j] >= -eps, "c[" << j << "]{" << c[j] << "} is below " << -eps); } } } std::cerr << "nCol / nTotal = " << nCol << " / " << N << std::endl; std::cerr << "nDiff = " << nDiff << std::endl; // statistics clock_t totalTimeGjkColl = 0; clock_t totalTimeGjkNoColl = 0; for (std::size_t i = 0; i < N; ++i) { if (results[i].collision) { totalTimeGjkColl += results[i].timeGjk; } else { totalTimeGjkNoColl += results[i].timeGjk; } } std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl + totalTimeGjkColl << ", " << Scalar(totalTimeGjkNoColl + totalTimeGjkColl) / Scalar(CLOCKS_PER_SEC * N) << "s" << std::endl; std::cerr << "-- Collisions -------------------------" << std::endl; std::cerr << "Total / average time gjk: " << totalTimeGjkColl << ", " << Scalar(totalTimeGjkColl) / Scalar(CLOCKS_PER_SEC * nCol) << "s" << std::endl; std::cerr << "-- No collisions -------------------------" << std::endl; std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl << ", " << Scalar(totalTimeGjkNoColl) / Scalar(CLOCKS_PER_SEC * (N - nCol)) << "s" << std::endl; } BOOST_AUTO_TEST_CASE(distance_triangle_triangle) { test_gjk_distance_triangle_triangle(false); } BOOST_AUTO_TEST_CASE(distance_triangle_triangle_nesterov) { test_gjk_distance_triangle_triangle(true); } void test_gjk_unit_sphere(Scalar center_distance, Vec3s ray, Scalar swept_sphere_radius, bool use_gjk_nesterov_acceleration) { using namespace coal; const Scalar r = 1.0; Sphere sphere(r); sphere.setSweptSphereRadius(swept_sphere_radius); typedef Eigen::Matrix Vec4f; Transform3s tf0(Quats(Vec4f::Random().normalized()), Vec3s::Zero()); Transform3s tf1(Quats(Vec4f::Random().normalized()), center_distance * ray); bool expect_collision = center_distance <= 2 * (r + swept_sphere_radius); details::MinkowskiDiff shape; shape.set(&sphere, &sphere, tf0, tf1); BOOST_CHECK_EQUAL(shape.swept_sphere_radius[0], sphere.radius + sphere.getSweptSphereRadius()); BOOST_CHECK_EQUAL(shape.swept_sphere_radius[1], sphere.radius + sphere.getSweptSphereRadius()); details::GJK gjk(2, Scalar(1e-6)); if (use_gjk_nesterov_acceleration) gjk.gjk_variant = GJKVariant::NesterovAcceleration; details::GJK::Status status = gjk.evaluate(shape, Vec3ps(1, 0, 0)); if (expect_collision) { BOOST_CHECK((status == details::GJK::Collision) || (status == details::GJK::CollisionWithPenetrationInformation)); // For sphere-sphere, if the distance between the centers is above GJK's // tolerance, the `Collision` status should never be returned. BOOST_CHECK(status == details::GJK::CollisionWithPenetrationInformation && center_distance > gjk.getTolerance()); } else { BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); } Vec3ps w0_, w1_, normal_; gjk.getWitnessPointsAndNormal(shape, w0_, w1_, normal_); Vec3s w0 = w0_.cast(); Vec3s w1 = w1_.cast(); Vec3s normal = normal_.cast(); Vec3s w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) + swept_sphere_radius * normal); Vec3s w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) - swept_sphere_radius * normal); EIGEN_VECTOR_IS_APPROX(w0, w0_expected, Scalar(1e-10)); EIGEN_VECTOR_IS_APPROX(w1, w1_expected, Scalar(1e-10)); } BOOST_AUTO_TEST_CASE(sphere_sphere) { std::array use_nesterov_acceleration = {false, true}; std::array swept_sphere_radius = {0., Scalar(0.1), 1, 10, 100}; for (bool nesterov_acceleration : use_nesterov_acceleration) { for (Scalar ssr : swept_sphere_radius) { test_gjk_unit_sphere(3, Vec3s(1, 0, 0), ssr, nesterov_acceleration); test_gjk_unit_sphere(Scalar(2.01), Vec3s(1, 0, 0), ssr, nesterov_acceleration); test_gjk_unit_sphere(2, Vec3s(1, 0, 0), ssr, nesterov_acceleration); test_gjk_unit_sphere(1, Vec3s(1, 0, 0), ssr, nesterov_acceleration); // Random rotation test_gjk_unit_sphere(3, Vec3s::Random().normalized(), ssr, nesterov_acceleration); test_gjk_unit_sphere(Scalar(2.01), Vec3s::Random().normalized(), ssr, nesterov_acceleration); test_gjk_unit_sphere(2, Vec3s::Random().normalized(), ssr, nesterov_acceleration); test_gjk_unit_sphere(1, Vec3s::Random().normalized(), ssr, nesterov_acceleration); } } } void test_gjk_triangle_capsule(Vec3s T, bool expect_collision, bool use_gjk_nesterov_acceleration, Vec3s w0_expected, Vec3s w1_expected) { using namespace coal; Capsule capsule(1., 2.); // Radius 1 and length 2 TriangleP triangle(Vec3s(0., 0., 0.), Vec3s(1., 0., 0.), Vec3s(1., 1., 0.)); Transform3s tf0, tf1; tf1.setTranslation(T); details::MinkowskiDiff shape; // No need to take into account swept-sphere radius in supports computation // when using GJK/EPA; after they have converged, these algos will correctly // handle the swept-sphere radius of the shapes. shape.set(&capsule, &triangle, tf0, tf1); BOOST_CHECK_EQUAL(shape.swept_sphere_radius[0], capsule.radius); BOOST_CHECK_EQUAL(shape.swept_sphere_radius[1], 0.); details::GJK gjk(10, SolverScalar(1e-6)); if (use_gjk_nesterov_acceleration) gjk.gjk_variant = GJKVariant::NesterovAcceleration; details::GJK::Status status = gjk.evaluate(shape, Vec3ps(1, 0, 0)); if (expect_collision) { BOOST_CHECK((status == details::GJK::Collision) || (status == details::GJK::CollisionWithPenetrationInformation)); } else { BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); // Check that guess works as expected Vec3ps guess = gjk.getGuessFromSimplex(); details::GJK gjk2(3, SolverScalar(1e-6)); details::GJK::Status status2 = gjk2.evaluate(shape, guess); BOOST_CHECK_EQUAL(status2, details::GJK::NoCollision); } Vec3ps w0_, w1_, normal_; if (status == details::GJK::NoCollision || status == details::GJK::CollisionWithPenetrationInformation) { gjk.getWitnessPointsAndNormal(shape, w0_, w1_, normal_); } else { details::EPA epa(64, SolverScalar(1e-6)); details::EPA::Status epa_status = epa.evaluate(gjk, Vec3ps(1, 0, 0)); BOOST_CHECK_EQUAL(epa_status, details::EPA::AccuracyReached); epa.getWitnessPointsAndNormal(shape, w0_, w1_, normal_); } Vec3s w0 = w0_.cast(); Vec3s w1 = w1_.cast(); Vec3s normal = normal_.cast(); COAL_UNUSED_VARIABLE(normal); EIGEN_VECTOR_IS_APPROX(w0, w0_expected, Scalar(1e-10)); EIGEN_VECTOR_IS_APPROX(w1 - T, w1_expected, Scalar(1e-10)); } BOOST_AUTO_TEST_CASE(triangle_capsule) { // GJK -> no collision test_gjk_triangle_capsule(Vec3s(Scalar(1.01), 0, 0), false, false, Vec3s(1, 0, 0), Vec3s(0., 0, 0)); // GJK + Nesterov acceleration -> no collision test_gjk_triangle_capsule(Vec3s(Scalar(1.01), 0, 0), false, true, Vec3s(1, 0, 0), Vec3s(0., 0, 0)); // GJK -> collision test_gjk_triangle_capsule(Vec3s(Scalar(0.5), 0, 0), true, false, Vec3s(1, 0, 0), Vec3s(0., 0, 0)); // GJK + Nesterov acceleration -> collision test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, true, Vec3s(1., 0, 0), Vec3s(0., 0, 0)); // GJK + EPA -> collision test_gjk_triangle_capsule(Vec3s(Scalar(-0.5), Scalar(-0.01), 0), true, false, Vec3s(0, 1, 0), Vec3s(Scalar(0.5), 0, 0)); // GJK + Nesterov accleration + EPA -> collision test_gjk_triangle_capsule(Vec3s(Scalar(-0.5), Scalar(-0.01), 0), true, true, Vec3s(0, 1, 0), Vec3s(Scalar(0.5), 0, 0)); } ================================================ FILE: test/gjk_asserts.cpp ================================================ #define BOOST_TEST_MODULE COAL_GJK_ASSERTS #include #include #include "coal/BVH/BVH_model.h" #include "coal/collision.h" using namespace coal; constexpr Scalar pi = boost::math::constants::pi(); Scalar DegToRad(const Scalar& deg) { static Scalar degToRad = pi / Scalar(180); return deg * degToRad; } std::vector dirs{Vec3s::UnitZ(), -Vec3s::UnitZ(), Vec3s::UnitY(), -Vec3s::UnitY(), Vec3s::UnitX(), -Vec3s::UnitX()}; void CreateSphereMesh(BVHModel& model, const Scalar& radius) { size_t polarSteps{32}; size_t azimuthSteps{32}; const float PI = static_cast(pi); const float polarStep = PI / (float)(polarSteps - 1); const float azimuthStep = 2.0f * PI / (float)(azimuthSteps - 1); std::vector vertices; std::vector triangles; for (size_t p = 0; p < polarSteps; ++p) { for (size_t a = 0; a < azimuthSteps; ++a) { const float x = std::sin((float)p * polarStep) * std::cos((float)a * azimuthStep); const float y = std::sin((float)p * polarStep) * std::sin((float)a * azimuthStep); const float z = std::cos((float)p * polarStep); vertices.emplace_back(radius * x, radius * y, radius * z); } } for (size_t p = 0; p < polarSteps - 1; ++p) { for (size_t a = 0; a < azimuthSteps - 1; ++a) { size_t p0 = p * azimuthSteps + a; size_t p1 = p * azimuthSteps + (a + 1); size_t p2 = (p + 1) * azimuthSteps + (a + 1); size_t p3 = (p + 1) * azimuthSteps + a; triangles.emplace_back(p0, p2, p1); triangles.emplace_back(p0, p3, p2); } } model.beginModel(); model.addSubModel(vertices, triangles); model.endModel(); } BOOST_AUTO_TEST_CASE(TestSpheres) { BVHModel sphere1{}; BVHModel sphere2{}; CreateSphereMesh(sphere1, 1.); CreateSphereMesh(sphere2, 2.); CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); ComputeCollision compute(&sphere2, &sphere1); Transform3s sphere1Tf = Transform3s::Identity(); Transform3s sphere2Tf = Transform3s::Identity(); for (int i = 0; i < 360; ++i) { for (int j = 0; j < 180; ++j) { if ( /// assertion: src/narrowphase/gjk.cpp:331 (i == 5 && j == 48) || (i == 64 && j == 151) || (i == 98 && j == 47) || (i == 355 && j == 48) || /// assertion: src/narrowphase/gjk.cpp:1263 (i == 86 && j == 52) || (i == 89 && j == 17) || (i == 89 && j == 58) || (i == 89 && j == 145)) { sphere2Tf.setQuatRotation( Eigen::AngleAxis(DegToRad(Scalar(i)), Vec3s::UnitZ()) * Eigen::AngleAxis(DegToRad(Scalar(j)), Vec3s::UnitY())); for (const Vec3s& dir : dirs) { sphere2Tf.setTranslation(dir); CollisionResult result; BOOST_CHECK_NO_THROW(compute(sphere2Tf, sphere1Tf, request, result)); } } } } } ================================================ FILE: test/gjk_convergence_criterion.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2022, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #define BOOST_TEST_MODULE COAL_NESTEROV_GJK #include #include #include #include "coal/data_types.h" #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shapes.h" #include "coal/internal/tools.h" #include "utility.h" using coal::Box; using coal::GJKConvergenceCriterion; using coal::GJKConvergenceCriterionType; using coal::GJKSolver; using coal::Scalar; using coal::ShapeBase; using coal::SolverScalar; using coal::support_func_guess_t; using coal::Transform3s; using coal::Vec3ps; using coal::Vec3s; using coal::details::GJK; using coal::details::MinkowskiDiff; using std::size_t; BOOST_AUTO_TEST_CASE(set_cv_criterion) { GJKSolver solver; GJK gjk(128, Scalar(1e-6)); // Checking defaults BOOST_CHECK(solver.gjk.convergence_criterion == GJKConvergenceCriterion::Default); BOOST_CHECK(solver.gjk.convergence_criterion_type == GJKConvergenceCriterionType::Relative); BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::Default); BOOST_CHECK(gjk.convergence_criterion_type == GJKConvergenceCriterionType::Relative); // Checking set solver.gjk.convergence_criterion = GJKConvergenceCriterion::DualityGap; gjk.convergence_criterion = GJKConvergenceCriterion::DualityGap; solver.gjk.convergence_criterion_type = GJKConvergenceCriterionType::Absolute; gjk.convergence_criterion_type = GJKConvergenceCriterionType::Absolute; BOOST_CHECK(solver.gjk.convergence_criterion == GJKConvergenceCriterion::DualityGap); BOOST_CHECK(solver.gjk.convergence_criterion_type == GJKConvergenceCriterionType::Absolute); BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::DualityGap); BOOST_CHECK(gjk.convergence_criterion_type == GJKConvergenceCriterionType::Absolute); } void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, const GJKConvergenceCriterionType cv_type) { // Solvers unsigned int max_iterations = 128; // by default GJK uses the VDB convergence criterion, which is relative. GJK gjk1(max_iterations, Scalar(1e-6)); Scalar tol; switch (cv_type) { // need to lower the tolerance when absolute case GJKConvergenceCriterionType::Absolute: tol = Scalar(1e-8); break; case GJKConvergenceCriterionType::Relative: tol = Scalar(1e-6); break; default: throw std::logic_error("Wrong convergence criterion type"); } GJK gjk2(max_iterations, tol); gjk2.convergence_criterion = GJKConvergenceCriterion::DualityGap; gjk2.convergence_criterion_type = cv_type; GJK gjk3(max_iterations, tol); gjk3.convergence_criterion = GJKConvergenceCriterion::Hybrid; gjk3.convergence_criterion_type = cv_type; // Minkowski difference MinkowskiDiff mink_diff; // Generate random transforms size_t n = 1000; Scalar extents[] = {-3., -3., 0, 3., 3., 3.}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Transform3s identity = Transform3s::Identity(); // Same init for both solvers Vec3s init_guess = Vec3s(1, 0, 0); support_func_guess_t init_support_guess; init_support_guess.setZero(); // Run for 3 different cv criterions for (size_t i = 0; i < n; ++i) { mink_diff.set(&shape0, &shape1, identity, transforms[i]); GJK::Status res1 = gjk1.evaluate(mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(gjk1.getNumIterations() <= max_iterations); Vec3ps ray1 = gjk1.ray; res1 = gjk1.evaluate(mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(res1 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk1.ray, ray1, SolverScalar(1e-8)); GJK::Status res2 = gjk2.evaluate(mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(gjk2.getNumIterations() <= max_iterations); Vec3ps ray2 = gjk2.ray; res2 = gjk2.evaluate(mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(res2 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk2.ray, ray2, SolverScalar(1e-8)); GJK::Status res3 = gjk3.evaluate(mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(gjk3.getNumIterations() <= max_iterations); Vec3ps ray3 = gjk3.ray; res3 = gjk3.evaluate(mink_diff, init_guess.cast(), init_support_guess); BOOST_CHECK(res3 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk3.ray, ray3, SolverScalar(1e-8)); // check that solutions are close enough EIGEN_VECTOR_IS_APPROX(gjk1.ray, gjk2.ray, SolverScalar(1e-4)); EIGEN_VECTOR_IS_APPROX(gjk1.ray, gjk3.ray, SolverScalar(1e-4)); } } BOOST_AUTO_TEST_CASE(cv_criterion_same_solution) { Box box0 = Box(Scalar(0.1), Scalar(0.2), Scalar(0.3)); Box box1 = Box(Scalar(1.1), Scalar(1.2), Scalar(1.3)); test_gjk_cv_criterion(box0, box1, GJKConvergenceCriterionType::Absolute); test_gjk_cv_criterion(box0, box1, GJKConvergenceCriterionType::Relative); } ================================================ FILE: test/hfields.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Justin Carpentier */ #define BOOST_TEST_MODULE COAL_HEIGHT_FIELDS #include #include #include "fcl_resources/config.h" #include "coal/collision.h" #include "coal/hfield.h" #include "coal/math/transform.h" #include "coal/shape/geometric_shapes.h" #include "coal/mesh_loader/assimp.h" #include "coal/mesh_loader/loader.h" #include "coal/collision.h" #include "coal/internal/traversal_node_hfield_shape.h" #include "utility.h" #include using namespace coal; using RowVector = Eigen::RowVector; template void test_constant_hfields(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny, const Scalar min_altitude, const Scalar max_altitude) { const Scalar x_dim = 1., y_dim = 2.; const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); BOOST_CHECK(hfield.getXDim() == x_dim); BOOST_CHECK(hfield.getYDim() == y_dim); const VecXs& x_grid = hfield.getXGrid(); BOOST_CHECK(x_grid[0] == -x_dim / 2.); BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.); const VecXs& y_grid = hfield.getYGrid(); BOOST_CHECK(y_grid[0] == y_dim / 2.); BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.); // Test local AABB hfield.computeLocalAABB(); for (Eigen::DenseIndex i = 0; i < nx; ++i) { for (Eigen::DenseIndex j = 0; j < ny; ++j) { Vec3s point(x_grid[i], y_grid[j], heights(j, i)); BOOST_CHECK(hfield.aabb_local.contain(point)); } } // Test clone { HeightField* hfield_clone = hfield.clone(); hfield_clone->computeLocalAABB(); BOOST_CHECK(*hfield_clone == hfield); delete hfield_clone; } // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); const Transform3s box_placement( Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / Scalar(2))); // Test collision const Sphere sphere(1.); static const Transform3s IdTransform; const Box box(Vec3s::Ones()); Transform3s M_sphere, M_box; // No collision case { const Scalar eps_no_collision = +Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(!result.isCollision()); CollisionResult result_check_sphere; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check_sphere); BOOST_CHECK(!result_check_sphere.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(!result_check_box.isCollision()); } // Collision case { const Scalar eps_collision = -Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(result.isCollision()); CollisionResult result_check; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check); BOOST_CHECK(result_check.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(result_check_box.isCollision()); } // Update height hfield.updateHeights( MatrixXs::Constant(ny, nx, max_altitude / 2)); // We change nothing // No collision case { const Scalar eps_no_collision = +Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(!result.isCollision()); CollisionResult result_check_sphere; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check_sphere); BOOST_CHECK(!result_check_sphere.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(!result_check_box.isCollision()); } // Collision case { const Scalar eps_collision = -Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(!result.isCollision()); CollisionResult result_check; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check); BOOST_CHECK(result_check.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(result_check_box.isCollision()); } // Restore height hfield.updateHeights( MatrixXs::Constant(ny, nx, max_altitude)); // We change nothing // Collision case { const Scalar eps_collision = -Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(result.isCollision()); CollisionResult result_check; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check); BOOST_CHECK(result_check.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(result_check_box.isCollision()); } } BOOST_AUTO_TEST_CASE(building_constant_hfields) { const Scalar max_altitude = 1., min_altitude = 0.; test_constant_hfields(2, 2, min_altitude, max_altitude); // Simple case test_constant_hfields(20, 2, min_altitude, max_altitude); test_constant_hfields(100, 100, min_altitude, max_altitude); // test_constant_hfields(1000,1000,min_altitude,max_altitude); test_constant_hfields(2, 2, min_altitude, max_altitude); // Simple case test_constant_hfields(20, 2, min_altitude, max_altitude); test_constant_hfields(100, 100, min_altitude, max_altitude); } template void test_negative_security_margin(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny, const Scalar min_altitude, const Scalar max_altitude) { const Scalar x_dim = 1., y_dim = 2.; const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); const Transform3s box_placement( Matrix3s::Identity(), Vec3s(0, 0, (max_altitude + min_altitude) / Scalar(2))); // Test collision const Sphere sphere(1.); static const Transform3s IdTransform; const Box box(Vec3s::Ones()); Transform3s M_sphere, M_box; // No collision case { const Scalar eps_no_collision = +Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(!result.isCollision()); CollisionResult result_check_sphere; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check_sphere); BOOST_CHECK(!result_check_sphere.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(!result_check_box.isCollision()); } // Collision case - positive security_margin { const Scalar eps_no_collision = +Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; request.security_margin = eps_no_collision + Scalar(1e-6); CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(result.isCollision()); CollisionResult result_check_sphere; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check_sphere); BOOST_CHECK(result_check_sphere.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(result_check_box.isCollision()); } // Collision case { const Scalar eps_no_collision = -Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(result.isCollision()); CollisionResult result_check_sphere; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check_sphere); BOOST_CHECK(result_check_sphere.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(result_check_box.isCollision()); } // No collision case - negative security_margin { const Scalar eps_no_collision = -Scalar(0.1) * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; request.security_margin = eps_no_collision - Scalar(1e-4); CollisionResult result; collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(!result.isCollision()); CollisionResult result_check_sphere; collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, request, result_check_sphere); BOOST_CHECK(!result_check_sphere.isCollision()); CollisionResult result_check_box; collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, result_check_box); BOOST_CHECK(!result_check_box.isCollision()); } } BOOST_AUTO_TEST_CASE(negative_security_margin) { const Scalar max_altitude = 1., min_altitude = 0.; // test_negative_security_margin(100, 100, min_altitude, // max_altitude); test_negative_security_margin(100, 100, min_altitude, max_altitude); } BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { const Eigen::DenseIndex nx = 100, ny = 100; typedef AABB BV; const MatrixXs X = RowVector::LinSpaced(nx, -1., 1.).replicate(ny, 1); const MatrixXs Y = VecXs::LinSpaced(ny, 1., -1.).replicate(1, nx); const Scalar dim_square = 0.5; const Eigen::Array hole = (X.array().abs() < dim_square) && (Y.array().abs() < dim_square); const MatrixXs heights = MatrixXs::Ones(ny, nx) - hole.cast().matrix(); const HeightField hfield(2., 2., heights, -10.); Sphere sphere(Scalar(0.48)); const Transform3s sphere_pos(Vec3s(0, 0, Scalar(0.5))); const Transform3s hfield_pos; const CollisionRequest request; CollisionResult result; { collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } sphere.radius = Scalar(0.51); { CollisionResult result; const Sphere sphere2(Scalar(0.51)); collide(&hfield, hfield_pos, &sphere2, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); } } BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { const Eigen::DenseIndex nx = 100, ny = 100; // typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug // mode), as the overlap of OBBRSS is not satisfactory yet. typedef AABB BV; const MatrixXs X = RowVector::LinSpaced(nx, -1., 1.).replicate(ny, 1); const MatrixXs Y = VecXs::LinSpaced(ny, 1., -1.).replicate(1, nx); const Scalar dim_hole = 1; const Eigen::Array hole = (X.array().square() + Y.array().square() <= dim_hole); const MatrixXs heights = MatrixXs::Ones(ny, nx) - hole.cast().matrix(); const HeightField hfield(2., 2., heights, -10.); BOOST_CHECK(hfield.getXGrid()[0] == -1.); BOOST_CHECK(hfield.getXGrid()[nx - 1] == +1.); BOOST_CHECK(hfield.getYGrid()[0] == +1.); BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.); Sphere sphere(Scalar(0.975)); const Transform3s sphere_pos(Vec3s(0, 0, 1)); const Transform3s hfield_pos; const Scalar thresholds[3] = {0, Scalar(0.01), Scalar(-0.005)}; for (int i = 0; i < 3; ++i) { CollisionResult result; CollisionRequest request; request.security_margin = thresholds[i]; collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } // Increase the size of the sphere to force the collision sphere.radius = Scalar(1.01); for (int i = 0; i < 3; ++i) { CollisionResult result; CollisionRequest request; request.security_margin = thresholds[i]; collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); } { CollisionResult result; CollisionRequest request; request.security_margin = Scalar(-0.02); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } } bool isApprox(const Scalar v1, const Scalar v2, const Scalar tol = Scalar(1e-6)) { return std::fabs(v1 - v2) <= tol; } Vec3s computeFaceNormal(const Triangle32& triangle, const std::vector& points) { const Vec3s pointA = points[triangle[0]]; const Vec3s pointB = points[triangle[1]]; const Vec3s pointC = points[triangle[2]]; return (pointB - pointA).cross(pointC - pointA).normalized(); } BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { const Scalar sphere_radius = 1.; Sphere sphere(sphere_radius); MatrixXs altitutes(2, 2); Scalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; HeightField hfield(1., 1., altitutes, 0.); const HeightField::BVS& nodes = hfield.getNodes(); BOOST_CHECK(nodes.size() == 1); const HeightField::Node& node = nodes[0]; typedef HFNodeBase::FaceOrientation FaceOrientation; BOOST_CHECK((node.contact_active_faces & FaceOrientation::BOTTOM) == int(FaceOrientation::BOTTOM)); BOOST_CHECK((node.contact_active_faces & FaceOrientation::TOP) == int(FaceOrientation::TOP)); BOOST_CHECK((node.contact_active_faces & FaceOrientation::NORTH) == int(FaceOrientation::NORTH)); BOOST_CHECK((node.contact_active_faces & FaceOrientation::SOUTH) == int(FaceOrientation::SOUTH)); BOOST_CHECK((node.contact_active_faces & FaceOrientation::EAST) == int(FaceOrientation::EAST)); BOOST_CHECK((node.contact_active_faces & FaceOrientation::WEST) == int(FaceOrientation::WEST)); ConvexTpl convex1, convex2; int convex1_active_faces, convex2_active_faces; details::buildConvexTriangles(node, hfield, convex1, convex1_active_faces, convex2, convex2_active_faces); // Check face normals for convex1 { const std::vector& points = *(convex1.points); // BOTTOM { const Triangle32& triangle = (*(convex1.polygons))[0]; BOOST_CHECK( computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ())); } // TOP { const Triangle32& triangle = (*(convex1.polygons))[1]; BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ())); } // WEST sides { const Triangle32& triangle1 = (*(convex1.polygons))[2]; const Triangle32& triangle2 = (*(convex1.polygons))[3]; BOOST_CHECK( computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitX())); BOOST_CHECK( computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitX())); } // SOUTH-EAST sides { const Vec3s south_east_normal = Vec3s(1., -1., 0).normalized(); const Triangle32& triangle1 = (*(convex1.polygons))[4]; const Triangle32& triangle2 = (*(convex1.polygons))[5]; BOOST_CHECK( computeFaceNormal(triangle1, points).isApprox(south_east_normal)); BOOST_CHECK( computeFaceNormal(triangle2, points).isApprox(south_east_normal)); } // NORTH sides { const Triangle32& triangle1 = (*(convex1.polygons))[6]; const Triangle32& triangle2 = (*(convex1.polygons))[7]; std::cout << "computeFaceNormal(triangle1,points): " << computeFaceNormal(triangle1, points).transpose() << std::endl; BOOST_CHECK( computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitY())); BOOST_CHECK( computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitY())); } } // Check face normals for convex2 { const std::vector& points = *(convex2.points); // BOTTOM { const Triangle32& triangle = (*(convex2.polygons))[0]; BOOST_CHECK( computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ())); } // TOP { const Triangle32& triangle = (*(convex2.polygons))[1]; BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ())); } // SOUTH sides { const Triangle32& triangle1 = (*(convex2.polygons))[2]; const Triangle32& triangle2 = (*(convex2.polygons))[3]; BOOST_CHECK( computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitY())); BOOST_CHECK( computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitY())); } // NORTH-WEST sides { const Vec3s north_west_normal = Vec3s(-1., 1., 0).normalized(); const Triangle32& triangle1 = (*(convex2.polygons))[4]; const Triangle32& triangle2 = (*(convex2.polygons))[5]; BOOST_CHECK( computeFaceNormal(triangle1, points).isApprox(north_west_normal)); BOOST_CHECK( computeFaceNormal(triangle2, points).isApprox(north_west_normal)); } // EAST sides { const Triangle32& triangle1 = (*(convex2.polygons))[6]; const Triangle32& triangle2 = (*(convex2.polygons))[7]; BOOST_CHECK( computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitX())); BOOST_CHECK( computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitX())); } } } BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) { typedef HFNodeBase::FaceOrientation FaceOrientation; const Scalar sphere_radius = 1.; Sphere sphere(sphere_radius); MatrixXs altitutes(3, 3); Scalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; HeightField hfield(1., 1., altitutes, 0.); const HeightField::BVS& nodes = hfield.getNodes(); BOOST_CHECK(nodes.size() == 7); for (const auto& node : nodes) { if (node.isLeaf()) { BOOST_CHECK((node.contact_active_faces & FaceOrientation::BOTTOM) == int(FaceOrientation::BOTTOM)); BOOST_CHECK((node.contact_active_faces & FaceOrientation::TOP) == int(FaceOrientation::TOP)); if (node.x_id == 0) BOOST_CHECK((node.contact_active_faces & FaceOrientation::WEST) == int(FaceOrientation::WEST)); if (node.y_id == 0) BOOST_CHECK((node.contact_active_faces & FaceOrientation::NORTH) == int(FaceOrientation::NORTH)); if (node.x_id == 1) BOOST_CHECK((node.contact_active_faces & FaceOrientation::EAST) == int(FaceOrientation::EAST)); if (node.y_id == 1) BOOST_CHECK((node.contact_active_faces & FaceOrientation::SOUTH) == int(FaceOrientation::SOUTH)); } } } BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { const Scalar sphere_radius = 1.; Sphere sphere(sphere_radius); MatrixXs altitutes(2, 2); Scalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; HeightField hfield(1., 1., altitutes, 0.); const HeightField::BVS& nodes = hfield.getNodes(); BOOST_CHECK(nodes.size() == 1); // Collision from the TOP { const Transform3s sphere_pos(Vec3s(0., 0., 2.)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = -Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); BOOST_CHECK( isApprox(result.distance_lower_bound, -request.security_margin)); } // Same, but with a positive margin. { const Transform3s sphere_pos(Vec3s(0., 0., 2.)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = +Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitZ())); std::cout << "contact.penetration_depth: " << contact.penetration_depth << std::endl; BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } // Collision from the BOTTOM { const Transform3s sphere_pos(Vec3s(0., 0., -1.)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = -Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } { const Transform3s sphere_pos(Vec3s(0., 0., -1.)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = +Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); { const Contact& contact = result.getContact(0); BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitZ())); std::cout << "contact.penetration_depth: " << contact.penetration_depth << std::endl; BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } // Collision from the WEST { const Transform3s sphere_pos( Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = -Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } { const Transform3s sphere_pos( Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = +Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitX())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } // Collision from the EAST { const Transform3s sphere_pos( Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = -Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } { const Transform3s sphere_pos( Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = +Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitX())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } // Collision from the NORTH { const Transform3s sphere_pos( Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = -Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } { const Transform3s sphere_pos( Vec3s(0, hfield.getYGrid()[0] + sphere_radius, Scalar(0.5))); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = +Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitY())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } // Collision from the SOUTH { const Transform3s sphere_pos( Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = -Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } { const Transform3s sphere_pos( Vec3s(0., hfield.getYGrid()[1] - sphere_radius, Scalar(0.5))); const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; request.security_margin = +Scalar(0.005); collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitY())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } } ================================================ FILE: test/math.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #define _USE_MATH_DEFINES #include #define BOOST_TEST_MODULE COAL_MATH #include #include "coal/data_types.h" #include "coal/math/transform.h" #include "coal/internal/intersect.h" #include "coal/internal/tools.h" using namespace coal; BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) { Vec3s v1(1, 2, 3); BOOST_CHECK(v1[0] == Scalar(1)); BOOST_CHECK(v1[1] == Scalar(2)); BOOST_CHECK(v1[2] == Scalar(3)); Vec3s v2 = v1; Vec3s v3(Scalar(3.3), Scalar(4.3), Scalar(5.3)); v1 += v3; BOOST_CHECK(isEqual(v1, v2 + v3)); v1 -= v3; BOOST_CHECK(isEqual(v1, v2)); v1 -= v3; BOOST_CHECK(isEqual(v1, v2 - v3)); v1 += v3; v1.array() *= v3.array(); BOOST_CHECK(isEqual(v1, v2.cwiseProduct(v3))); v1.array() /= v3.array(); BOOST_CHECK(isEqual(v1, v2)); v1.array() /= v3.array(); BOOST_CHECK(isEqual(v1, v2.cwiseQuotient(v3))); v1.array() *= v3.array(); v1 *= 2.0; BOOST_CHECK(isEqual(v1, v2 * 2.0)); v1 /= 2.0; BOOST_CHECK(isEqual(v1, v2)); v1 /= 2.0; BOOST_CHECK(isEqual(v1, v2 / 2.0)); v1 *= 2.0; v1.array() += 2.0; BOOST_CHECK(isEqual(v1, (v2.array() + 2.0).matrix())); v1.array() -= 2.0; BOOST_CHECK(isEqual(v1, v2)); v1.array() -= 2.0; BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix())); v1.array() += 2.0; BOOST_CHECK(isEqual((-Vec3s(1.0, 2.0, 3.0)), Vec3s(-1.0, -2.0, -3.0))); v1 = Vec3s(1.0, 2.0, 3.0); v2 = Vec3s(3.0, 4.0, 5.0); BOOST_CHECK(isEqual((v1.cross(v2)), Vec3s(-2.0, 4.0, -2.0))); BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5); v1 = Vec3s(3.0, 4.0, 5.0); BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5); BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5); BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm())); v1 = Vec3s(1.0, 2.0, 3.0); v2 = Vec3s(3.0, 4.0, 5.0); BOOST_CHECK(isEqual(v1.cross(v2), Vec3s(-2.0, 4.0, -2.0))); BOOST_CHECK(v1.dot(v2) == 26); } Vec3s rotate(Vec3s input, Scalar w, Vec3s vec) { return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input + 2 * w * vec.cross(input); } BOOST_AUTO_TEST_CASE(quaternion) { Quats q1(Quats::Identity()), q2, q3; q2 = fromAxisAngle(Vec3s(0, 0, 1), Scalar(M_PI / 2)); q3 = q2.inverse(); Vec3s v(1, -1, 0); BOOST_CHECK(isEqual(v, q1 * v)); BOOST_CHECK(isEqual(Vec3s(1, 1, 0), q2 * v)); BOOST_CHECK( isEqual(rotate(v, q3.w(), Vec3s(q3.x(), q3.y(), q3.z())), q3 * v)); } BOOST_AUTO_TEST_CASE(transform) { Quats q = fromAxisAngle(Vec3s(0, 0, 1), Scalar(M_PI / 2)); Vec3s T(0, 1, 2); Transform3s tf(q, T); Vec3s v(1, -1, 0); BOOST_CHECK(isEqual(q * v + T, q * v + T)); Vec3s rv(q * v); // typename Transform3s::transform_return_type::type output = // tf * v; // std::cout << rv << std::endl; // std::cout << output.lhs() << std::endl; BOOST_CHECK(isEqual(rv + T, tf.transform(v))); } BOOST_AUTO_TEST_CASE(random_transform) { for (size_t i = 0; i < 100; ++i) { const Transform3s tf = Transform3s::Random(); BOOST_CHECK((tf.inverseTimes(tf)).isIdentity()); } } ================================================ FILE: test/normal_and_nearest_points.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2022-2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #define BOOST_TEST_MODULE COAL_NORMAL_AND_NEAREST_POINTS #include #include "coal/fwd.hh" #include "coal/shape/geometric_shapes.h" #include "coal/collision_data.h" #include "coal/BV/OBBRSS.h" #include "coal/BVH/BVH_model.h" #include "coal/shape/geometric_shape_to_BVH_model.h" #include "utility.h" using namespace coal; typedef Eigen::Vector2d Vec2d; // This test suite is designed to operate on any pair of primitive shapes: // spheres, capsules, boxes, ellipsoids, cones, cylinders, planes, halfspaces, // convex meshes. Do not use this file for BVH, octree etc. It would not make // sense. // This test is designed to check if the normal and the nearest points // are properly handled as defined in DistanceResult::normal. // Regardless of wether or not the two shapes are in intersection, regardless of // whether `collide` or `distance` is called: // --> we denote `dist` the (signed) distance that separates the two shapes // --> we denote `p1` and `p2` their nearest_points (witness points) // --> the `normal` should always point from shape 1 to shape 2, i.e we should // always have: | normal = sign(dist) * (p2 - p1).normalized() // | p2 = p1 + dist * normal // Thus: // --> if o1 and o2 are not in collision, translating o2 by vector // `-(dist + eps) * normal` should bring them in collision (eps > 0). // --> if o1 and o2 are in collision, translating o2 by vector // `-(dist - eps)) * normal` should separate them (eps > 0). // --> finally, if abs(dist) > 0, we should have: // normal = sign(dist) * (p2 - p1).normalized() // p2 = p1 + dist * normal template void test_normal_and_nearest_points( const ShapeType1& o1, const ShapeType2& o2, size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS, Scalar gjk_tolerance = GJK_DEFAULT_TOLERANCE, size_t epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS, Scalar epa_tolerance = EPA_DEFAULT_TOLERANCE) { // Generate random poses for o2 #ifndef NDEBUG // if debug mode std::size_t n = 10; #else size_t n = 1000; #endif // We want to make sure we generate poses that are in collision // so we take a relatively small extent for the random poses Scalar extents[] = {-1.5, -1.5, -1.5, 1.5, 1.5, 1.5}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Transform3s tf1 = Transform3s::Identity(); CollisionRequest colreq; colreq.distance_upper_bound = std::numeric_limits::max(); // For strictly convex shapes, the default tolerance of EPA is way too low. // Because EPA is basically trying to fit a polytope to a strictly convex // surface, it might take it a lot of iterations to converge to a low // tolerance. A solution is to increase the number of iterations and the // tolerance (and/or increase the number of faces and vertices EPA is allowed // to work with). colreq.gjk_max_iterations = gjk_max_iterations; colreq.gjk_tolerance = gjk_tolerance; colreq.epa_max_iterations = epa_max_iterations; colreq.epa_tolerance = epa_tolerance; DistanceRequest distreq; distreq.gjk_max_iterations = gjk_max_iterations; distreq.gjk_tolerance = gjk_tolerance; distreq.epa_max_iterations = epa_max_iterations; distreq.epa_tolerance = epa_tolerance; for (size_t i = 0; i < n; i++) { // Run both `distance` and `collide`. // Since CollisionRequest::distance_lower_bound is set to infinity, // these functions should agree on the results regardless of collision or // not. Transform3s tf2 = transforms[i]; CollisionResult colres; DistanceResult distres; size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres); Scalar dist = distance(&o1, tf1, &o2, tf2, distreq, distres); const Scalar dummy_precision(100 * std::numeric_limits::epsilon()); if (col) { BOOST_CHECK(dist <= 0.); BOOST_CHECK_CLOSE(dist, distres.min_distance, dummy_precision); Contact contact = colres.getContact(0); BOOST_CHECK_CLOSE(dist, contact.penetration_depth, dummy_precision); Vec3s cp1 = contact.nearest_points[0]; EIGEN_VECTOR_IS_APPROX(cp1, distres.nearest_points[0], dummy_precision); Vec3s cp2 = contact.nearest_points[1]; EIGEN_VECTOR_IS_APPROX(cp2, distres.nearest_points[1], dummy_precision); BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), epa_tolerance); EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, epa_tolerance); Vec3s separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, epa_tolerance); if (dist < 0) { EIGEN_VECTOR_IS_APPROX(contact.normal, -(cp2 - cp1).normalized(), epa_tolerance); } // Separate the shapes Transform3s new_tf1 = tf1; Scalar eps = Scalar(1e-2); new_tf1.setTranslation(tf1.getTranslation() + separation_vector - eps * contact.normal); CollisionResult new_colres; DistanceResult new_distres; size_t new_col = collide(&o1, new_tf1, &o2, tf2, colreq, new_colres); Scalar new_dist = distance(&o1, new_tf1, &o2, tf2, distreq, new_distres); BOOST_CHECK(new_dist > 0); BOOST_CHECK(!new_col); BOOST_CHECK(!new_colres.isCollision()); BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist, epa_tolerance); Vec3s new_cp1 = new_distres.nearest_points[0]; Vec3s new_cp2 = new_distres.nearest_points[1]; BOOST_CHECK_CLOSE(new_dist, (new_cp1 - new_cp2).norm(), epa_tolerance); EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal, epa_tolerance); Vec3s new_separation_vector = new_dist * new_distres.normal; EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1, epa_tolerance); if (new_dist > 0) { EIGEN_VECTOR_IS_APPROX(new_distres.normal, (new_cp2 - new_cp1).normalized(), gjk_tolerance); } } else { BOOST_CHECK(dist >= 0.); BOOST_CHECK_CLOSE(distres.min_distance, dist, dummy_precision); BOOST_CHECK_CLOSE(dist, colres.distance_lower_bound, dummy_precision); Vec3s cp1 = distres.nearest_points[0]; Vec3s cp2 = distres.nearest_points[1]; BOOST_CHECK_CLOSE(dist, (cp1 - cp2).norm(), gjk_tolerance); EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, gjk_tolerance); Vec3s separation_vector = dist * distres.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, gjk_tolerance); if (dist > 0) { EIGEN_VECTOR_IS_APPROX(distres.normal, (cp2 - cp1).normalized(), gjk_tolerance); } // Bring the shapes in collision // We actually can't guarantee that the shapes will be in collision. // Suppose you have two disjoing cones, which witness points are the tips // of the cones. // If you translate one of the cones by the separation vector and it // happens to be parallel to the axis of the cone, the two shapes will // still be disjoint. Scalar eps = Scalar(1e-2); Transform3s new_tf1 = tf1; new_tf1.setTranslation(tf1.getTranslation() + separation_vector + eps * distres.normal); CollisionResult new_colres; DistanceResult new_distres; collide(&o1, new_tf1, &o2, tf2, colreq, new_colres); Scalar new_dist = distance(&o1, new_tf1, &o2, tf2, distreq, new_distres); BOOST_CHECK(new_dist < dist); BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist, dummy_precision); // tolerance if (new_colres.isCollision()) { Contact contact = new_colres.getContact(0); Vec3s new_cp1 = contact.nearest_points[0]; EIGEN_VECTOR_IS_APPROX(new_cp1, new_distres.nearest_points[0], dummy_precision); Vec3s new_cp2 = contact.nearest_points[1]; EIGEN_VECTOR_IS_APPROX(new_cp2, new_distres.nearest_points[1], dummy_precision); BOOST_CHECK_CLOSE(contact.penetration_depth, -(new_cp2 - new_cp1).norm(), epa_tolerance); EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal, epa_tolerance); Vec3s new_separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1, epa_tolerance); if (new_dist < 0) { EIGEN_VECTOR_IS_APPROX( contact.normal, -(new_cp2 - new_cp1).normalized(), epa_tolerance); } } } } } template Eigen::Matrix generateRandomVector(Scalar min, Scalar max) { typedef Eigen::Matrix VecType; // Generate a random vector in the [min, max] range VecType v = VecType::Random() * (max - min) * 0.5 + VecType::Ones() * (max + min) * 0.5; return v; } Scalar generateRandomNumber(Scalar min, Scalar max) { Scalar r = static_cast(rand()) / static_cast(RAND_MAX); r = 2 * r - 1; const Scalar half(0.5); return r * (max - min) * half + (max + min) * half; } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_sphere) { for (size_t i = 0; i < 10; ++i) { Vec2s radii = generateRandomVector<2>(Scalar(0.05), 1); shared_ptr o1(new Sphere(radii(0))); shared_ptr o2(new Sphere(radii(1))); test_normal_and_nearest_points(*o1.get(), *o2.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_capsule) { for (size_t i = 0; i < 10; ++i) { Vec2s radii = generateRandomVector<2>(Scalar(0.05), 1); Scalar h = generateRandomNumber(Scalar(0.15), 1); shared_ptr o1(new Sphere(radii(0))); shared_ptr o2(new Capsule(radii(1), h)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_box) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(Scalar(0.05), 1))); shared_ptr o2(new Sphere(generateRandomNumber(Scalar(0.05), 1))); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_mesh) { for (size_t i = 0; i < 10; ++i) { ConvexTpl o1_ = constructPolytopeFromEllipsoid( Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); shared_ptr> o1(new ConvexTpl( o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); ConvexTpl o2_ = constructPolytopeFromEllipsoid( Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); shared_ptr> o2(new ConvexTpl( o2_.points, o2_.num_points, o2_.polygons, o2_.num_polygons)); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; Scalar gjk_tolerance = GJK_DEFAULT_TOLERANCE; size_t epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS; Scalar epa_tolerance = EPA_DEFAULT_TOLERANCE; test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_box) { ConvexTpl o1_ = constructPolytopeFromEllipsoid( Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); shared_ptr> o1(new ConvexTpl( o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); shared_ptr o2(new Box(generateRandomVector<3>(Scalar(0.05), 1))); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_ellipsoid) { for (size_t i = 0; i < 10; ++i) { ConvexTpl o1_ = constructPolytopeFromEllipsoid( Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); shared_ptr> o1(new ConvexTpl( o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); shared_ptr o2( new Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); Scalar gjk_tolerance = Scalar(1e-6); // With EPA's tolerance set at 1e-3, the precision on the normal, contact // points and penetration depth is on the order of the milimeter. However, // EPA (currently) cannot converge to lower tolerances on strictly convex // shapes in a reasonable amount of iterations. Scalar epa_tolerance = Scalar(1e-3); test_normal_and_nearest_points(*o1.get(), *o2.get(), GJK_DEFAULT_MAX_ITERATIONS, gjk_tolerance, EPA_DEFAULT_MAX_ITERATIONS, epa_tolerance); test_normal_and_nearest_points(*o2.get(), *o1.get(), GJK_DEFAULT_MAX_ITERATIONS, gjk_tolerance, EPA_DEFAULT_MAX_ITERATIONS, epa_tolerance); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_ellipsoid) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1( new Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1.0))); shared_ptr o2( new Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1.0))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; Scalar gjk_tolerance = Scalar(1e-6); // With EPA's tolerance set at 1e-3, the precision on the normal, contact // points and penetration depth is on the order of the milimeter. However, // EPA (currently) cannot converge to lower tolerances on strictly convex // shapes in a reasonable amount of iterations. size_t epa_max_iterations = 250; Scalar epa_tolerance = Scalar(1e-3); // For EPA on ellipsoids, we need to increase the number of iterations in // this test. This is simply because this test checks **a lot** of cases and // it can generate some of the worst cases for EPA. We don't want to // increase the tolerance too much because otherwise the test would not // work. test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(Scalar(0.05), 1))); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(Scalar(0.05), 1))); Scalar offset = Scalar(0.1); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) { for (size_t i = 0; i < 10; ++i) { Scalar r = generateRandomNumber(Scalar(0.05), 1); Scalar h = generateRandomNumber(Scalar(0.15), 1); shared_ptr o1(new Capsule(r, h)); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Sphere(generateRandomNumber(Scalar(0.05), 1))); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_plane) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Sphere(generateRandomNumber(Scalar(0.05), 1))); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_halfspace) { for (size_t i = 0; i < 10; ++i) { ConvexTpl o1_ = constructPolytopeFromEllipsoid( Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); shared_ptr> o1(new ConvexTpl( o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_cylinder) { for (size_t i = 0; i < 10; ++i) { Vec2s r = generateRandomVector<2>(Scalar(0.05), 1); Vec2s h = generateRandomVector<2>(Scalar(0.15), 1); shared_ptr o1(new Cone(r(0), h(0))); shared_ptr o2(new Cylinder(r(1), h(1))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; Scalar gjk_tolerance = Scalar(1e-6); size_t epa_max_iterations = 250; Scalar epa_tolerance = Scalar(1e-3); test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); test_normal_and_nearest_points(*o2.get(), *o1.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_ellipsoid) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1( new Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); Vec2s r = generateRandomVector<2>(Scalar(0.05), 1); Vec2s h = generateRandomVector<2>(Scalar(0.15), 1); shared_ptr o2(new Cylinder(r(1), h(1))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; Scalar gjk_tolerance = Scalar(1e-6); size_t epa_max_iterations = 250; Scalar epa_tolerance = Scalar(1e-3); test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); test_normal_and_nearest_points(*o2.get(), *o1.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) { for (size_t i = 0; i < 10; ++i) { Scalar r = generateRandomNumber(Scalar(0.05), 1); Scalar h = generateRandomNumber(Scalar(0.15), 1); shared_ptr o1(new Cone(r, h)); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) { for (size_t i = 0; i < 10; ++i) { Scalar r = generateRandomNumber(Scalar(0.05), 1); Scalar h = generateRandomNumber(Scalar(0.15), 1); shared_ptr o1(new Cylinder(r, h)); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) { for (size_t i = 0; i < 10; ++i) { Scalar r = generateRandomNumber(Scalar(0.05), 1); Scalar h = generateRandomNumber(Scalar(0.15), 1); shared_ptr o1(new Cone(r, h)); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) { for (size_t i = 0; i < 10; ++i) { Scalar r = generateRandomNumber(Scalar(0.05), 1); Scalar h = generateRandomNumber(Scalar(0.15), 1); shared_ptr o1(new Cylinder(r, h)); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_plane) { for (size_t i = 0; i < 10; ++i) { Scalar r = generateRandomNumber(Scalar(0.05), 1); Scalar h = generateRandomNumber(Scalar(0.15), 1); shared_ptr o1(new Capsule(r, h)); const Scalar half(0.5); Scalar offset = generateRandomNumber(-half, half); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_capsule) { for (size_t i = 0; i < 10; ++i) { Vec2s r = generateRandomVector<2>(Scalar(0.05), 1); Vec2s h = generateRandomVector<2>(Scalar(0.15), 1); shared_ptr o1(new Capsule(r(0), h(0))); shared_ptr o2(new Capsule(r(1), h(1))); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) { for (size_t i = 0; i < 10; ++i) { Vec2s r = generateRandomVector<2>(Scalar(0.05), 1); Scalar h = generateRandomNumber(Scalar(0.15), 1); shared_ptr o1(new Sphere(r(0))); shared_ptr o2(new Cylinder(r(1), h)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) { for (size_t i = 0; i < 10; ++i) { Scalar offset = generateRandomNumber(Scalar(0.15), 1); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o1( new Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); shared_ptr o2(new Halfspace(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_plane) { for (size_t i = 0; i < 10; ++i) { Scalar offset = generateRandomNumber(Scalar(0.15), 1); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o1( new Ellipsoid(generateRandomVector<3>(Scalar(0.05), 1))); shared_ptr o2(new Plane(n, offset)); test_normal_and_nearest_points(*o1.get(), *o2.get()); test_normal_and_nearest_points(*o2.get(), *o1.get()); } } void test_normal_and_nearest_points(const BVHModel& o1, const Halfspace& o2) { // Generate random poses for o2 #ifndef NDEBUG // if debug mode std::size_t n = 1; #else size_t n = 10000; #endif Scalar extents[] = {-2., -2., -2., 2., 2., 2.}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Transform3s tf1 = Transform3s::Identity(); transforms[0] = Transform3s::Identity(); CollisionRequest colreq; colreq.distance_upper_bound = std::numeric_limits::max(); colreq.num_max_contacts = 100; CollisionResult colres; DistanceRequest distreq; DistanceResult distres; for (size_t i = 0; i < n; i++) { Transform3s tf2 = transforms[i]; colres.clear(); distres.clear(); size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres); Scalar dist = distance(&o1, tf1, &o2, tf2, distreq, distres); if (col) { BOOST_CHECK(dist <= 0.); BOOST_CHECK_CLOSE(dist, distres.min_distance, 1e-6); for (size_t c = 0; c < colres.numContacts(); c++) { Contact contact = colres.getContact(c); BOOST_CHECK(contact.penetration_depth <= 0); BOOST_CHECK(contact.penetration_depth >= colres.distance_lower_bound); Vec3s cp1 = contact.nearest_points[0]; Vec3s cp2 = contact.nearest_points[1]; BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), 1e-6); EIGEN_VECTOR_IS_APPROX(cp1, cp2 - contact.penetration_depth * contact.normal, Scalar(1e-6)); Vec3s separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, Scalar(1e-6)); if (dist < 0) { EIGEN_VECTOR_IS_APPROX(contact.normal, -(cp2 - cp1).normalized(), Scalar(1e-6)); } } } else { BOOST_CHECK(dist >= 0.); BOOST_CHECK_CLOSE(distres.min_distance, dist, 1e-6); BOOST_CHECK_CLOSE(dist, colres.distance_lower_bound, 1e-6); } } } void test_normal_and_nearest_points(const Halfspace& o1, const BVHModel& o2) { test_normal_and_nearest_points(o2, o1); } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) { Box* box_ptr = new coal::Box(1, 1, 1); coal::CollisionGeometryPtr_t b1(box_ptr); BVHModel o1 = BVHModel(); generateBVHModel(o1, *box_ptr, Transform3s()); o1.buildConvexRepresentation(false); Scalar offset = Scalar(0.1); Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); test_normal_and_nearest_points(o1, *o2.get()); test_normal_and_nearest_points(*o2.get(), o1); } ================================================ FILE: test/obb.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2014-2016, CNRS-LAAS * Copyright (c) 2023, Inria * Author: Florent Lamiraux * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of CNRS nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include "coal/narrowphase/narrowphase.h" #include "../src/BV/OBB.h" #include "coal/internal/shape_shape_func.h" #include "utility.h" using namespace coal; void randomOBBs(Vec3s& a, Vec3s& b, Scalar extentNorm) { // Extent norm is between 0 and extentNorm on each axis // a = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3)); // b = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3)); a = extentNorm * Vec3s::Random().cwiseAbs().normalized(); b = extentNorm * Vec3s::Random().cwiseAbs().normalized(); } void randomTransform(Matrix3s& B, Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar extentNorm) { // TODO Should we scale T to a and b norm ? (void)a; (void)b; (void)extentNorm; Scalar N = a.norm() + b.norm(); // A translation of norm N ensures there is no collision. // Set translation to be between 0 and 2 * N; T = (Vec3s::Random() / sqrt(3)) * 1.5 * N; // T.setZero(); Quats q; q.coeffs().setRandom(); q.normalize(); B = q; } #define NB_METHODS 7 #define MANUAL_PRODUCT 1 #if MANUAL_PRODUCT #define PRODUCT(M33, v3) \ (M33.col(0) * v3[0] + M33.col(1) * v3[1] + M33.col(2) * v3[2]) #else #define PRODUCT(M33, v3) (M33 * v3) #endif typedef std::chrono::high_resolution_clock clock_type; typedef clock_type::duration duration_type; const char* sep = ",\t"; const Scalar eps = Scalar(1.5e-7); const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, ", ", // Coeff separator ",\n", // Row separator "(", // row prefix ",)", // row suffix "( ", // mat prefix ", )" // mat suffix ); namespace obbDisjoint_impls { /// @return true if OBB are disjoint. bool distance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, Scalar& distance) { GJKSolver gjk; Box ba(2 * a), bb(2 * b); Transform3s tfa, tfb(B, T); Vec3s p1, p2, normal; bool compute_penetration = true; distance = gjk.shapeDistance(ba, tfa, bb, tfb, compute_penetration, p1, p2, normal); return (distance > gjk.getDistancePrecision(compute_penetration)); } inline Scalar _computeDistanceForCase1(const Vec3s& T, const Vec3s& a, const Vec3s& b, const Matrix3s& Bf) { Vec3s AABB_corner; /* This seems to be slower AABB_corner.noalias() = T.cwiseAbs () - a; AABB_corner.noalias() -= PRODUCT(Bf,b); /*/ #if MANUAL_PRODUCT AABB_corner.noalias() = T.cwiseAbs() - a; AABB_corner.noalias() -= Bf.col(0) * b[0]; AABB_corner.noalias() -= Bf.col(1) * b[1]; AABB_corner.noalias() -= Bf.col(2) * b[2]; #else AABB_corner.noalias() = T.cwiseAbs() - Bf * b - a; #endif // */ return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm(); } inline Scalar _computeDistanceForCase2(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Matrix3s& Bf) { /* Vec3s AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a); return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm (); /*/ #if MANUAL_PRODUCT Scalar s, t = 0; s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; if (s > 0) t += s * s; s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; if (s > 0) t += s * s; s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2]; if (s > 0) t += s * s; return t; #else Vec3s AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b); return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm(); #endif // */ } int separatingAxisId(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { int id = 0; Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return id; ++id; // | B^T T| - b - Bf^T a squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return id; ++id; int ja = 1, ka = 2, jb = 1, kb = 2; for (int ia = 0; ia < 3; ++ia) { for (int ib = 0; ib < 3; ++ib) { const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return id; } } /* // or Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; return true; } // */ } ++id; jb = kb; kb = ib; } ja = ka; ka = ia; } return id; } // ------------------------ 0 -------------------------------------- bool withRuntimeLoop(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; // | B^T T| - b - Bf^T a squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; int ja = 1, ka = 2, jb = 1, kb = 2; for (int ia = 0; ia < 3; ++ia) { for (int ib = 0; ib < 3; ++ib) { const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } /* // or Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; return true; } // */ } jb = kb; kb = ib; } ja = ka; ka = ia; } return false; } // ------------------------ 1 -------------------------------------- bool withManualLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { Scalar t, s; Scalar diff; // Matrix3s Bf = abs(B); // Bf += reps; Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin Vec3s AABB_corner(T.cwiseAbs() - Bf * b); Vec3s diff3(AABB_corner - a); diff3 = diff3.cwiseMax(Vec3s::Zero()); // for (Vec3s::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); squaredLowerBoundDistance = diff3.squaredNorm(); if (squaredLowerBoundDistance > breakDistance2) return true; AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a; // diff3 = | B^T T| - b - Bf^T a diff3 = AABB_corner - b; diff3 = diff3.cwiseMax(Vec3s::Zero()); squaredLowerBoundDistance = diff3.squaredNorm(); if (squaredLowerBoundDistance > breakDistance2) return true; // A0 x B0 s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); Scalar sinus2; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || // As ||A0|| = ||B0|| = 1, // 2 2 // || A0 x B0 || + (A0 | B0) = 1 if (diff > 0) { sinus2 = 1 - Bf(0, 0) * Bf(0, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A0 x B1 s = T[2] * B(1, 1) - T[1] * B(2, 1); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)); if (diff > 0) { sinus2 = 1 - Bf(0, 1) * Bf(0, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A0 x B2 s = T[2] * B(1, 2) - T[1] * B(2, 2); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)); if (diff > 0) { sinus2 = 1 - Bf(0, 2) * Bf(0, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B0 s = T[0] * B(2, 0) - T[2] * B(0, 0); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)); if (diff > 0) { sinus2 = 1 - Bf(1, 0) * Bf(1, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B1 s = T[0] * B(2, 1) - T[2] * B(0, 1); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)); if (diff > 0) { sinus2 = 1 - Bf(1, 1) * Bf(1, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B2 s = T[0] * B(2, 2) - T[2] * B(0, 2); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)); if (diff > 0) { sinus2 = 1 - Bf(1, 2) * Bf(1, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B0 s = T[1] * B(0, 0) - T[0] * B(1, 0); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)); if (diff > 0) { sinus2 = 1 - Bf(2, 0) * Bf(2, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B1 s = T[1] * B(0, 1) - T[0] * B(1, 1); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)); if (diff > 0) { sinus2 = 1 - Bf(2, 1) * Bf(2, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B2 s = T[1] * B(0, 2) - T[0] * B(1, 2); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)); if (diff > 0) { sinus2 = 1 - Bf(2, 2) * Bf(2, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } return false; } // ------------------------ 2 -------------------------------------- bool withManualLoopUnrolling_2(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; // A0 x B0 Scalar t, s; s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); Scalar sinus2; Scalar diff; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || // As ||A0|| = ||B0|| = 1, // 2 2 // || A0 x B0 || + (A0 | B0) = 1 if (diff > 0) { sinus2 = 1 - Bf(0, 0) * Bf(0, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A0 x B1 s = T[2] * B(1, 1) - T[1] * B(2, 1); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)); if (diff > 0) { sinus2 = 1 - Bf(0, 1) * Bf(0, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A0 x B2 s = T[2] * B(1, 2) - T[1] * B(2, 2); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)); if (diff > 0) { sinus2 = 1 - Bf(0, 2) * Bf(0, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B0 s = T[0] * B(2, 0) - T[2] * B(0, 0); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)); if (diff > 0) { sinus2 = 1 - Bf(1, 0) * Bf(1, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B1 s = T[0] * B(2, 1) - T[2] * B(0, 1); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)); if (diff > 0) { sinus2 = 1 - Bf(1, 1) * Bf(1, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B2 s = T[0] * B(2, 2) - T[2] * B(0, 2); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)); if (diff > 0) { sinus2 = 1 - Bf(1, 2) * Bf(1, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B0 s = T[1] * B(0, 0) - T[0] * B(1, 0); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)); if (diff > 0) { sinus2 = 1 - Bf(2, 0) * Bf(2, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B1 s = T[1] * B(0, 1) - T[0] * B(1, 1); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)); if (diff > 0) { sinus2 = 1 - Bf(2, 1) * Bf(2, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B2 s = T[1] * B(0, 2) - T[0] * B(1, 2); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)); if (diff > 0) { sinus2 = 1 - Bf(2, 2) * Bf(2, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } return false; } // ------------------------ 3 -------------------------------------- template struct loop_case_1 { static inline bool run(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Matrix3s& Bf, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } /* // or Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; return true; } // */ } return false; } }; bool withTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; // Ai x Bj if (loop_case_1<0, 0>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_1<0, 1>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_1<0, 2>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_1<1, 0>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_1<1, 1>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_1<1, 2>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_1<2, 0>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_1<2, 1>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_1<2, 2>::run(B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; return false; } // ------------------------ 4 -------------------------------------- template struct loop_case_2 { static inline bool run(int ia, int ja, int ka, const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Matrix3s& Bf, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } /* // or Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; return true; } // */ } return false; } }; bool withPartialTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); if (squaredLowerBoundDistance > breakDistance2) return true; // Ai x Bj int ja = 1, ka = 2; for (int ia = 0; ia < 3; ++ia) { if (loop_case_2<0>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_2<1>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; if (loop_case_2<2>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; ja = ka; ka = ia; } return false; } // ------------------------ 5 -------------------------------------- bool originalWithLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar& breakDistance2, Scalar& squaredLowerBoundDistance) { Scalar t, s; Scalar diff; Matrix3s Bf(B.cwiseAbs()); squaredLowerBoundDistance = 0; // if any of these tests are one-sided, then the polyhedra are disjoint // A1 x A2 = A0 t = ((T[0] < 0.0) ? -T[0] : T[0]); diff = t - (a[0] + Bf.row(0).dot(b)); if (diff > 0) { squaredLowerBoundDistance = diff * diff; } // A2 x A0 = A1 t = ((T[1] < 0.0) ? -T[1] : T[1]); diff = t - (a[1] + Bf.row(1).dot(b)); if (diff > 0) { squaredLowerBoundDistance += diff * diff; } // A0 x A1 = A2 t = ((T[2] < 0.0) ? -T[2] : T[2]); diff = t - (a[2] + Bf.row(2).dot(b)); if (diff > 0) { squaredLowerBoundDistance += diff * diff; } if (squaredLowerBoundDistance > breakDistance2) return true; squaredLowerBoundDistance = 0; // B1 x B2 = B0 s = B.col(0).dot(T); t = ((s < 0.0) ? -s : s); diff = t - (b[0] + Bf.col(0).dot(a)); if (diff > 0) { squaredLowerBoundDistance += diff * diff; } // B2 x B0 = B1 s = B.col(1).dot(T); t = ((s < 0.0) ? -s : s); diff = t - (b[1] + Bf.col(1).dot(a)); if (diff > 0) { squaredLowerBoundDistance += diff * diff; } // B0 x B1 = B2 s = B.col(2).dot(T); t = ((s < 0.0) ? -s : s); diff = t - (b[2] + Bf.col(2).dot(a)); if (diff > 0) { squaredLowerBoundDistance += diff * diff; } if (squaredLowerBoundDistance > breakDistance2) return true; // A0 x B0 s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); Scalar sinus2; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || // As ||A0|| = ||B0|| = 1, // 2 2 // || A0 x B0 || + (A0 | B0) = 1 if (diff > 0) { sinus2 = 1 - Bf(0, 0) * Bf(0, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A0 x B1 s = T[2] * B(1, 1) - T[1] * B(2, 1); t = ((s < 0.0) ? -s : s); diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)); if (diff > 0) { sinus2 = 1 - Bf(0, 1) * Bf(0, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A0 x B2 s = T[2] * B(1, 2) - T[1] * B(2, 2); t = ((s < 0.0) ? -s : s); diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)); if (diff > 0) { sinus2 = 1 - Bf(0, 2) * Bf(0, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B0 s = T[0] * B(2, 0) - T[2] * B(0, 0); t = ((s < 0.0) ? -s : s); diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)); if (diff > 0) { sinus2 = 1 - Bf(1, 0) * Bf(1, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B1 s = T[0] * B(2, 1) - T[2] * B(0, 1); t = ((s < 0.0) ? -s : s); diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)); if (diff > 0) { sinus2 = 1 - Bf(1, 1) * Bf(1, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A1 x B2 s = T[0] * B(2, 2) - T[2] * B(0, 2); t = ((s < 0.0) ? -s : s); diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)); if (diff > 0) { sinus2 = 1 - Bf(1, 2) * Bf(1, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B0 s = T[1] * B(0, 0) - T[0] * B(1, 0); t = ((s < 0.0) ? -s : s); diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)); if (diff > 0) { sinus2 = 1 - Bf(2, 0) * Bf(2, 0); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B1 s = T[1] * B(0, 1) - T[0] * B(1, 1); t = ((s < 0.0) ? -s : s); diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)); if (diff > 0) { sinus2 = 1 - Bf(2, 1) * Bf(2, 1); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } // A2 x B2 s = T[1] * B(0, 2) - T[0] * B(1, 2); t = ((s < 0.0) ? -s : s); diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)); if (diff > 0) { sinus2 = 1 - Bf(2, 2) * Bf(2, 2); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } } return false; } // ------------------------ 6 -------------------------------------- bool originalWithNoLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const Scalar&, Scalar& squaredLowerBoundDistance) { Scalar t, s; const Scalar reps = Scalar(1e-6); squaredLowerBoundDistance = 0; Matrix3s Bf(B.array().abs() + reps); // Bf += reps; // if any of these tests are one-sided, then the polyhedra are disjoint // A1 x A2 = A0 t = ((T[0] < 0.0) ? -T[0] : T[0]); // if(t > (a[0] + Bf.dotX(b))) if (t > (a[0] + Bf.row(0).dot(b))) return true; // B1 x B2 = B0 // s = B.transposeDotX(T); s = B.col(0).dot(T); t = ((s < 0.0) ? -s : s); // if(t > (b[0] + Bf.transposeDotX(a))) if (t > (b[0] + Bf.col(0).dot(a))) return true; // A2 x A0 = A1 t = ((T[1] < 0.0) ? -T[1] : T[1]); // if(t > (a[1] + Bf.dotY(b))) if (t > (a[1] + Bf.row(1).dot(b))) return true; // A0 x A1 = A2 t = ((T[2] < 0.0) ? -T[2] : T[2]); // if(t > (a[2] + Bf.dotZ(b))) if (t > (a[2] + Bf.row(2).dot(b))) return true; // B2 x B0 = B1 // s = B.transposeDotY(T); s = B.col(1).dot(T); t = ((s < 0.0) ? -s : s); // if(t > (b[1] + Bf.transposeDotY(a))) if (t > (b[1] + Bf.col(1).dot(a))) return true; // B0 x B1 = B2 // s = B.transposeDotZ(T); s = B.col(2).dot(T); t = ((s < 0.0) ? -s : s); // if(t > (b[2] + Bf.transposeDotZ(a))) if (t > (b[2] + Bf.col(2).dot(a))) return true; // A0 x B0 s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); if (t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) return true; // A0 x B1 s = T[2] * B(1, 1) - T[1] * B(2, 1); t = ((s < 0.0) ? -s : s); if (t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) return true; // A0 x B2 s = T[2] * B(1, 2) - T[1] * B(2, 2); t = ((s < 0.0) ? -s : s); if (t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) return true; // A1 x B0 s = T[0] * B(2, 0) - T[2] * B(0, 0); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) return true; // A1 x B1 s = T[0] * B(2, 1) - T[2] * B(0, 1); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) return true; // A1 x B2 s = T[0] * B(2, 2) - T[2] * B(0, 2); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) return true; // A2 x B0 s = T[1] * B(0, 0) - T[0] * B(1, 0); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) return true; // A2 x B1 s = T[1] * B(0, 1) - T[0] * B(1, 1); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) return true; // A2 x B2 s = T[1] * B(0, 2) - T[0] * B(1, 2); t = ((s < 0.0) ? -s : s); if (t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) return true; return false; } } // namespace obbDisjoint_impls struct BenchmarkResult { /// The test ID: /// - 0-10 identifies a separating axes. /// - 11 means no separatins axes could be found. (distance should be <= 0) int ifId; Scalar distance; Scalar squaredLowerBoundDistance; duration_type duration[NB_METHODS]; bool failure; static std::ostream& headers(std::ostream& os) { const std::string unit = " (us)"; os << "separating axis" << sep << "distance lower bound" << sep << "distance" << sep << "failure" << sep << "Runtime Loop" << unit << sep << "Manual Loop Unrolling 1" << unit << sep << "Manual Loop Unrolling 2" << unit << sep << "Template Unrolling" << unit << sep << "Partial Template Unrolling" << unit << sep << "Original (LowerBound)" << unit << sep << "Original (NoLowerBound)" << unit; return os; } std::ostream& print(std::ostream& os) const { os << ifId << sep << std::sqrt(squaredLowerBoundDistance) << sep << distance << sep << failure; for (int i = 0; i < NB_METHODS; ++i) os << sep << static_cast( std::chrono::duration_cast( duration[i]) .count()) * 1e-3; return os; } }; std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) { return br.print(os); } BenchmarkResult benchmark_obb_case(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, const CollisionRequest& request, std::size_t N) { const Scalar breakDistance(request.break_distance + request.security_margin); const Scalar breakDistance2 = breakDistance * breakDistance; BenchmarkResult result; // First determine which axis provide the answer result.ifId = obbDisjoint_impls::separatingAxisId( B, T, a, b, breakDistance2, result.squaredLowerBoundDistance); bool disjoint = obbDisjoint_impls::distance(B, T, a, b, result.distance); assert(0 <= result.ifId && result.ifId <= 11); // Sanity check result.failure = true; bool overlap = (result.ifId == 11); Scalar dist_thr = request.break_distance + request.security_margin; if (!overlap && result.distance <= 0) { std::cerr << "Failure: negative distance for disjoint OBBs."; } else if (!overlap && result.squaredLowerBoundDistance < 0) { std::cerr << "Failure: negative distance lower bound."; } else if (!overlap && eps < std::sqrt(result.squaredLowerBoundDistance) - result.distance) { std::cerr << "Failure: distance is inferior to its lower bound (diff = " << std::sqrt(result.squaredLowerBoundDistance) - result.distance << ")."; } else if (overlap != !disjoint && result.distance >= dist_thr - eps) { std::cerr << "Failure: overlapping test and distance query mismatch."; } else if (overlap && result.distance >= dist_thr - eps) { std::cerr << "Failure: positive distance for overlapping OBBs."; } else { result.failure = false; } if (result.failure) { std::cerr << "\nR = " << Quats(B).coeffs().transpose().format(py_fmt) << "\nT = " << T.transpose().format(py_fmt) << "\na = " << a.transpose().format(py_fmt) << "\nb = " << b.transpose().format(py_fmt) << "\nresult = " << result << '\n' << std::endl; } // Compute time Scalar tmp; clock_type::time_point start, end; // ------------------------ 0 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) obbDisjoint_impls::withRuntimeLoop(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[0] = (end - start) / N; // ------------------------ 1 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) obbDisjoint_impls::withManualLoopUnrolling_1(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[1] = (end - start) / N; // ------------------------ 2 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) obbDisjoint_impls::withManualLoopUnrolling_2(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[2] = (end - start) / N; // ------------------------ 3 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) obbDisjoint_impls::withTemplateLoopUnrolling_1(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[3] = (end - start) / N; // ------------------------ 4 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) obbDisjoint_impls::withPartialTemplateLoopUnrolling_1(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[4] = (end - start) / N; // ------------------------ 5 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) obbDisjoint_impls::originalWithLowerBound(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[5] = (end - start) / N; // ------------------------ 6 -------------------------------------- start = clock_type::now(); // The 2 last arguments are unused. for (std::size_t i = 0; i < N; ++i) obbDisjoint_impls::originalWithNoLowerBound(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[6] = (end - start) / N; return result; } std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) { std::size_t nbFailure = 0; // Create two OBBs axis Vec3s a, b; Matrix3s B; Vec3s T; CollisionRequest request; #ifndef NDEBUG // if debug mode static const size_t nbRandomOBB = 10; static const size_t nbTransformPerOBB = 10; static const size_t nbRunForTimeMeas = 10; #else static const size_t nbRandomOBB = 100; static const size_t nbTransformPerOBB = 100; static const size_t nbRunForTimeMeas = 1000; #endif static const Scalar extentNorm = 1.; if (output != NULL) *output << BenchmarkResult::headers << '\n'; BenchmarkResult result; for (std::size_t iobb = 0; iobb < nbRandomOBB; ++iobb) { randomOBBs(a, b, extentNorm); for (std::size_t itf = 0; itf < nbTransformPerOBB; ++itf) { randomTransform(B, T, a, b, extentNorm); result = benchmark_obb_case(B, T, a, b, request, nbRunForTimeMeas); if (output != NULL) *output << result << '\n'; if (result.failure) nbFailure++; } } return nbFailure; } int main(int argc, char** argv) { std::ostream* output = NULL; if (argc > 1 && strcmp(argv[1], "--generate-output") == 0) { output = &std::cout; } std::cout << "The benchmark real time measurements may be noisy and " "will incur extra overhead." "\nUse the following commands to turn ON:" "\n\tsudo cpufreq-set --governor performance" "\nor OFF:" "\n\tsudo cpufreq-set --governor powersave" "\n"; std::size_t nbFailure = obb_overlap_and_lower_bound_distance(output); if (nbFailure > INT_MAX) return INT_MAX; return (int)nbFailure; } ================================================ FILE: test/octree.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2019, CNRS-LAAS. * Copyright (c) 2023, INRIA. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Florent Lamiraux */ #define BOOST_TEST_MODULE COAL_OCTREE #include #include #include #include "coal/BVH/BVH_model.h" #include "coal/collision.h" #include "coal/distance.h" #include "coal/hfield.h" #include "coal/shape/geometric_shapes.h" #include "coal/shape/geometric_shapes_utility.h" #include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" namespace utf = boost::unit_test::framework; using namespace coal; void makeMesh(const std::vector& vertices, const std::vector& triangles, BVHModel& model) { coal::SplitMethodType split_method(coal::SPLIT_METHOD_MEAN); model.bv_splitter.reset(new BVSplitter(split_method)); model.bv_splitter.reset(new BVSplitter(split_method)); model.beginModel(); model.addSubModel(vertices, triangles); model.endModel(); } coal::OcTree makeOctree(const BVHModel& mesh, const Scalar& resolution) { Vec3s m(mesh.aabb_local.min_); Vec3s M(mesh.aabb_local.max_); coal::Box box(resolution, resolution, resolution); CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); CollisionResult result; Transform3s tfBox; octomap::OcTreePtr_t octree(new octomap::OcTree(resolution)); for (Scalar x = resolution * floor(m[0] / resolution); x <= M[0]; x += resolution) { for (Scalar y = resolution * floor(m[1] / resolution); y <= M[1]; y += resolution) { for (Scalar z = resolution * floor(m[2] / resolution); z <= M[2]; z += resolution) { const Scalar half = Scalar(0.5); Vec3s center(x + half * resolution, y + half * resolution, z + half * resolution); tfBox.setTranslation(center); coal::collide(&box, tfBox, &mesh, Transform3s(), request, result); if (result.isCollision()) { octomap::point3d p((float)center[0], (float)center[1], (float)center[2]); octree->updateNode(p, true); result.clear(); } } } } octree->updateInnerOccupancy(); octree->writeBinary("./env.octree"); return OcTree(octree); } BOOST_AUTO_TEST_CASE(octree_aabb) { // Create a simple octree with 8 nodes arranged in a 2x2x2 cube Scalar resolution = 1.0; octomap::OcTreePtr_t octree_ptr(new octomap::OcTree(resolution)); std::vector points = { octomap::point3d(0.5, 0.5, 0.5), octomap::point3d(1.5, 0.5, 0.5), octomap::point3d(0.5, 1.5, 0.5), octomap::point3d(1.5, 1.5, 0.5), octomap::point3d(0.5, 0.5, 1.5), octomap::point3d(1.5, 0.5, 1.5), octomap::point3d(0.5, 1.5, 1.5), octomap::point3d(1.5, 1.5, 1.5)}; for (const auto& point : points) { octree_ptr->updateNode(point, true); } octree_ptr->updateInnerOccupancy(); // Make sure the octree is not pruned octree_ptr->expand(); // Check initial size BOOST_CHECK_EQUAL(octree_ptr->getNumLeafNodes(), 8); // Create coal::OcTree and compute AABB coal::OcTree octree(octree_ptr); octree.computeLocalAABB(); // The AABB should encompass the 2x2x2 cube from (0,0,0) to (2,2,2) BOOST_CHECK(octree.aabb_local.min_.isApprox(Vec3s(0, 0, 0), 1e-6)); BOOST_CHECK(octree.aabb_local.max_.isApprox(Vec3s(2, 2, 2), 1e-6)); // Create a copy and prune it octomap::OcTreePtr_t octree_pruned_ptr(new octomap::OcTree(*octree_ptr)); octree_pruned_ptr->prune(); // Check pruned size, the 8 leaf nodes should have been merged into 1 BOOST_CHECK_EQUAL(octree_pruned_ptr->getNumLeafNodes(), 1); // Create coal::OcTree and compute AABB coal::OcTree octree_pruned(octree_pruned_ptr); octree_pruned.computeLocalAABB(); // The AABB should encompass the 2x2x2 cube from (0,0,0) to (2,2,2) BOOST_CHECK(octree_pruned.aabb_local.min_.isApprox(Vec3s(0, 0, 0), 1e-6)); BOOST_CHECK(octree_pruned.aabb_local.max_.isApprox(Vec3s(2, 2, 2), 1e-6)); // The AABB should remain the same after pruning BOOST_CHECK(octree.aabb_local.min_.isApprox(octree_pruned.aabb_local.min_)); BOOST_CHECK(octree.aabb_local.max_.isApprox(octree_pruned.aabb_local.max_)); } BOOST_AUTO_TEST_CASE(octree_mesh) { Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); Scalar resolution(10.); std::vector pRob, pEnv; std::vector tRob, tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "rob.obj").string().c_str(), pRob, tRob); loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv); BVHModel robMesh, envMesh; // Build meshes with robot and environment makeMesh(pRob, tRob, robMesh); makeMesh(pEnv, tEnv, envMesh); // Build octomap with environment envMesh.computeLocalAABB(); // Load octree built from envMesh by makeOctree(envMesh, resolution) OcTree envOctree( coal::loadOctreeFile((path / "env.octree").string(), resolution)); std::cout << "Finished loading octree." << std::endl; // Test operator== { BOOST_CHECK(envOctree == envOctree); BOOST_CHECK(envOctree == OcTree(envOctree)); const OcTree envOctree_from_tree(envOctree.getTree()); BOOST_CHECK(envOctree == envOctree_from_tree); } // Test tobytes() { const std::vector bytes = envOctree.tobytes(); BOOST_CHECK(bytes.size() > 0 && bytes.size() <= envOctree.toBoxes().size() * 3 * sizeof(Scalar)); } std::vector transforms; Scalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; #ifndef NDEBUG // if debug mode std::size_t N = 100; #else std::size_t N = 10000; #endif N = coal::getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, N); generateRandomTransforms(extents, transforms, 2 * N); CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); for (std::size_t i = 0; i < N; ++i) { CollisionResult resultMesh; CollisionResult resultOctree; Transform3s tf1(transforms[2 * i]); Transform3s tf2(transforms[2 * i + 1]); ; // Test collision between meshes with random transform for robot. coal::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh); // Test collision between mesh and octree for the same transform. coal::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree); bool resMesh(resultMesh.isCollision()); bool resOctree(resultOctree.isCollision()); BOOST_CHECK(!resMesh || resOctree); if (!resMesh && resOctree) { coal::DistanceRequest dreq; coal::DistanceResult dres; coal::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres); std::cout << "distance mesh mesh: " << dres.min_distance << std::endl; BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution); } } } BOOST_AUTO_TEST_CASE(octree_height_field) { Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); Scalar resolution(10.); std::vector pEnv; std::vector tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv); BVHModel envMesh; // Build meshes with robot and environment makeMesh(pEnv, tEnv, envMesh); // Build octomap with environment envMesh.computeLocalAABB(); // Load octree built from envMesh by makeOctree(envMesh, resolution) OcTree envOctree( coal::loadOctreeFile((path / "env.octree").string(), resolution)); std::cout << "Finished loading octree." << std::endl; // Building hfield const Scalar x_dim = 10, y_dim = 20; const int nx = 100, ny = 100; const Scalar max_altitude = 1., min_altitude = 0.; const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); hfield.computeLocalAABB(); std::vector transforms; Scalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; #ifndef NDEBUG // if debug mode std::size_t N = 1000; #else std::size_t N = 100000; #endif N = coal::getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, N); generateRandomTransforms(extents, transforms, 2 * N); CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, (std::numeric_limits::max)()); for (std::size_t i = 0; i < N; ++i) { CollisionResult resultBox; CollisionResult resultHfield1, resultHfield2; Transform3s tf1(transforms[2 * i]); Transform3s tf2(transforms[2 * i + 1]); Box box; Transform3s box_tf; constructBox(hfield.aabb_local, tf2, box, box_tf); // Test collision between octree and equivalent box. coal::collide(&envOctree, tf1, &box, box_tf, request, resultBox); // Test collision between octree and hfield. coal::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1); coal::collide(&hfield, tf2, &envOctree, tf1, request, resultHfield2); bool resBox(resultBox.isCollision()); bool resHfield(resultHfield1.isCollision()); BOOST_CHECK(resBox == resHfield); BOOST_CHECK(resultHfield1.isCollision() == resultHfield2.isCollision()); if (!resBox && resHfield) { coal::DistanceRequest dreq; coal::DistanceResult dres; coal::distance(&envMesh, tf1, &box, box_tf, dreq, dres); std::cout << "distance mesh box: " << dres.min_distance << std::endl; BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution); } } } ================================================ FILE: test/patch_simplifier.cpp ================================================ // // Copyright (c) 2026 INRIA // #define BOOST_TEST_MODULE COAL_PATCH_SIMPLIFIER #include #include "coal/contact_patch/polygon_convex_hull.h" #include "coal/contact_patch/contact_patch_simplifier.h" using namespace coal; /// @brief Generate a random ContactPatch by creating a random polygon /// and computing its convex hull. ContactPatch generateRandomPatch(std::size_t max_num_points = 20) { // Generate a random cloud of 2D points. std::vector cloud; cloud.reserve(max_num_points); for (std::size_t i = 0; i < max_num_points; ++i) { cloud.emplace_back(Vec2s::Random()); } // Compute the convex hull of the cloud. std::vector cvx_hull; computePolygonConvexHull(cloud, cvx_hull); // Create a ContactPatch and set its points to the convex hull. ContactPatch patch; patch.points() = cvx_hull; return patch; } /// @brief Compute the barycenter of a contact patch. Vec2s computeBarycenter(const ContactPatch& patch) { Vec2s barycenter = Vec2s::Zero(); for (std::size_t i = 0; i < patch.size(); ++i) { barycenter += patch.point(i); } barycenter /= static_cast(patch.size()); return barycenter; } /// @brief Find the index of the point farthest from a reference point. std::size_t findFarthestPoint(const ContactPatch& patch, const Vec2s& reference) { std::size_t farthest_idx = 0; Scalar max_dist_sq = 0; for (std::size_t i = 0; i < patch.size(); ++i) { const Scalar dist_sq = (patch.point(i) - reference).squaredNorm(); if (dist_sq > max_dist_sq) { max_dist_sq = dist_sq; farthest_idx = i; } } return farthest_idx; } BOOST_AUTO_TEST_CASE(patch_simplifier_barycenter) { const std::size_t desired_size = 1; for (std::size_t i = 0; i < 10; ++i) { ContactPatch patch = generateRandomPatch(20); ContactPatch patch_naive = patch; ContactPatchSimplifierNaive naive_simplifier; naive_simplifier.simplify(patch_naive, desired_size); ContactPatch patch_max_area = patch; ContactPatchSimplifierMaxArea max_area_simplifier; max_area_simplifier.simplify(patch_max_area, desired_size); // Check that both simplified patches contain one point. BOOST_CHECK_EQUAL(patch_naive.size(), 1); BOOST_CHECK_EQUAL(patch_max_area.size(), 1); // Check that this point is the barycenter of the points in `patch`. const Vec2s expected_barycenter = computeBarycenter(patch); const Scalar tol = Eigen::NumTraits::dummy_precision(); BOOST_CHECK((patch_naive.point(0) - expected_barycenter).norm() < tol); BOOST_CHECK((patch_max_area.point(0) - expected_barycenter).norm() < tol); } } BOOST_AUTO_TEST_CASE(patch_simplifier_two_points) { const std::size_t desired_size = 2; for (std::size_t i = 0; i < 10; ++i) { ContactPatch patch = generateRandomPatch(20); ContactPatch patch_naive = patch; ContactPatchSimplifierNaive naive_simplifier; naive_simplifier.simplify(patch_naive, desired_size); ContactPatch patch_max_area = patch; ContactPatchSimplifierMaxArea max_area_simplifier; max_area_simplifier.simplify(patch_max_area, desired_size); // Check that both simplified patches contain two points. BOOST_CHECK_EQUAL(patch_naive.size(), 2); BOOST_CHECK_EQUAL(patch_max_area.size(), 2); // Check that these points are the first point of `patch` and the point // farthest from it. const Vec2s first_point = patch.point(0); const std::size_t farthest_idx = findFarthestPoint(patch, first_point); const Vec2s farthest_point = patch.point(farthest_idx); const Scalar tol = Eigen::NumTraits::dummy_precision(); // For naive simplifier BOOST_CHECK((patch_naive.point(0) - first_point).norm() < tol); BOOST_CHECK((patch_naive.point(1) - farthest_point).norm() < tol); // For max area simplifier BOOST_CHECK((patch_max_area.point(0) - first_point).norm() < tol); BOOST_CHECK((patch_max_area.point(1) - farthest_point).norm() < tol); } } BOOST_AUTO_TEST_CASE(patch_simplifier) { const std::vector desired_sizes = {3, 4, 5, 6, 30}; for (const std::size_t desired_size : desired_sizes) { for (std::size_t i = 0; i < 10; ++i) { ContactPatch patch = generateRandomPatch(20); const Scalar original_area = patch.computeArea(); ContactPatch patch_naive = patch; ContactPatchSimplifierNaive naive_simplifier; naive_simplifier.simplify(patch_naive, desired_size); ContactPatch patch_max_area = patch; ContactPatchSimplifierMaxArea max_area_simplifier; max_area_simplifier.simplify(patch_max_area, desired_size); // Check that both simplified patches have the expected size. const std::size_t expected_size = std::min(desired_size, patch.size()); BOOST_CHECK_EQUAL(patch_naive.size(), expected_size); BOOST_CHECK_EQUAL(patch_max_area.size(), expected_size); // Compute areas of simplified patches. const Scalar naive_area = patch_naive.computeArea(); const Scalar max_area_area = patch_max_area.computeArea(); // Check that both simplified patches have the same area (both algorithms // should find the maximum area subset). const Scalar tol = Eigen::NumTraits::dummy_precision(); BOOST_CHECK_CLOSE(naive_area, max_area_area, tol); // Check that this area is smaller than or equal to the original patch's // area. BOOST_CHECK_LE(naive_area, original_area + tol); BOOST_CHECK_LE(max_area_area, original_area + tol); } } } ================================================ FILE: test/pixi_build/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.22) project(test_pixi_build) find_package(coal REQUIRED) ================================================ FILE: test/polygon_convex_hull.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2026, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #define _USE_MATH_DEFINES #include #define BOOST_TEST_MODULE COAL_POLYGON_CONVEX_HULL #include #include "coal/contact_patch/polygon_convex_hull.h" using namespace coal; /// @brief Generate a random cloud of 2D points. std::vector generateRandomCloud(std::size_t n) { std::vector cloud; cloud.reserve(n); for (std::size_t i = 0; i < n; ++i) { cloud.emplace_back(Vec2s::Random()); } return cloud; } /// @brief Computes the convex hull of a cloud of 2D points by computing /// the support point in directions taken on the unit circle. void computePolygonConvexHullNaive(const std::vector& cloud, std::vector& cvx_hull) { std::size_t n = cloud.size() * 1000; // lambda to compute the support point of `cloud` in a given direction. const auto compute_support_point = [&](const Vec2s& dir) { Vec2s best_support = cloud[0]; Scalar best_support_val = best_support.dot(dir); for (const auto& pt : cloud) { const Scalar support_val = pt.dot(dir); if (support_val > best_support_val) { best_support_val = support_val; best_support = pt; } } return best_support; }; // lambda to check if a given point is already in `cvx_hull`. const auto is_in_cvx_hull = [&](const Vec2s& pt) { bool found = false; for (const auto& hull_pt : cvx_hull) { if ((pt - hull_pt).norm() < Eigen::NumTraits::dummy_precision()) { found = true; break; } } return found; }; // Starting from direction (1, 0), we take n directions along the unit circle // and compute the support point of `cloud` in this direction. If the point is // not already in `cvx_hull`, we add it. cvx_hull.clear(); const Scalar angle_step = 2 * M_PI / static_cast(n); for (std::size_t i = 0; i < n; ++i) { const Scalar theta = static_cast(i) * angle_step; const Vec2s dir(std::cos(theta), std::sin(theta)); const Vec2s support_pt = compute_support_point(dir); if (!is_in_cvx_hull(support_pt)) { cvx_hull.push_back(support_pt); } } } BOOST_AUTO_TEST_CASE(test_compute_polygon_convex_hull) { for (std::size_t i = 0; i < 10; ++i) { std::vector cloud = generateRandomCloud(20); std::vector expected_cvx_hull; computePolygonConvexHullNaive(cloud, expected_cvx_hull); std::vector cvx_hull; computePolygonConvexHull(cloud, cvx_hull); std::cout << "cvx_hull.size() = " << cvx_hull.size() << "\n"; std::cout << "expected_cvx_hull.size() = " << expected_cvx_hull.size() << "\n"; BOOST_CHECK(cvx_hull.size() == expected_cvx_hull.size()); if (cvx_hull.size() == expected_cvx_hull.size()) { // Check that the two convex hulls match bool same_cvx_hull = true; for (const Vec2s& pt : cvx_hull) { bool found = false; for (const Vec2s& other_pt : expected_cvx_hull) { if (pt == other_pt) { found = true; break; } } if (!found) { same_cvx_hull = false; break; } } BOOST_CHECK(same_cvx_hull); } } } ================================================ FILE: test/profiling.cpp ================================================ // Copyright (c) 2017, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // // This file is part of Coal. // Coal 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 // 3 of the License, or (at your option) any later version. // // Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // Coal. If not, see . #include #include "coal/fwd.hh" #include "coal/collision.h" #include "coal/BVH/BVH_model.h" #include "coal/collision_utility.h" #include "coal/shape/geometric_shapes.h" #include "coal/collision_func_matrix.h" #include "coal/narrowphase/narrowphase.h" #include "coal/mesh_loader/assimp.h" #include "utility.h" #include "fcl_resources/config.h" using namespace coal; CollisionFunctionMatrix lookupTable; bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) { OBJECT_TYPE object_type1 = o1->getObjectType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); if (object_type1 == OT_GEOM && object_type2 == OT_BVH) return (lookupTable.collision_matrix[node_type2][node_type1] != NULL); else return (lookupTable.collision_matrix[node_type1][node_type2] != NULL); } template CollisionGeometryPtr_t objToGeom(const std::string& filename) { std::vector pt; std::vector tri; loadOBJFile(filename.c_str(), pt, tri); BVHModel* model(new BVHModel()); // model->bv_splitter.reset(new BVSplitter(split_method)); model->beginModel(); model->addSubModel(pt, tri); model->endModel(); return CollisionGeometryPtr_t(model); } template CollisionGeometryPtr_t meshToGeom(const std::string& filename, const Vec3s& scale = Vec3s(1, 1, 1)) { shared_ptr > model(new BVHModel()); loadPolyhedronFromResource(filename, scale, model); return model; } struct Geometry { std::string type; CollisionGeometryPtr_t o; Geometry(const std::string& t, CollisionGeometry* ob) : type(t), o(ob) {} Geometry(const std::string& t, const CollisionGeometryPtr_t& ob) : type(t), o(ob) {} }; struct Results { std::vector rs; Eigen::VectorXd times; // micro-seconds Results(size_t i) : rs(i), times((Eigen::DenseIndex)i) {} }; void collide(const std::vector& tf, const CollisionGeometry* o1, const CollisionGeometry* o2, const CollisionRequest& request, Results& results) { Transform3s Id; BenchTimer timer; for (std::size_t i = 0; i < tf.size(); ++i) { timer.start(); /* int num_contact = */ collide(o1, tf[i], o2, Id, request, results.rs[i]); const auto elapsed_us = timer.getElapsedTimeInMicroSec(); results.times[(Eigen::DenseIndex)i] = elapsed_us; } } const char* sep = ", "; void printResultHeaders() { std::cout << "Type 1" << sep << "Type 2" << sep << "mean time" << sep << "time std dev" << sep << "min time" << sep << "max time" << std::endl; } void printResults(const Geometry& g1, const Geometry& g2, const Results& rs) { double mean = rs.times.mean(); double var = rs.times.cwiseAbs2().mean() - mean * mean; std::cout << g1.type << sep << g2.type << sep << mean << sep << std::sqrt(var) << sep << rs.times.minCoeff() << sep << rs.times.maxCoeff() << std::endl; } #ifndef NDEBUG // if debug mode size_t Ntransform = 1; #else size_t Ntransform = 100; #endif Scalar limit = 20; bool verbose = false; #define OUT(x) \ if (verbose) std::cout << x << std::endl #define CHECK_PARAM_NB(NB, NAME) \ if (iarg + NB >= argc) \ throw std::invalid_argument(#NAME " requires " #NB " numbers") void handleParam(int& iarg, const int& argc, char** argv, CollisionRequest& request) { while (iarg < argc) { std::string a(argv[iarg]); if (a == "-nb_transform") { CHECK_PARAM_NB(1, nb_transform); Ntransform = (size_t)atoi(argv[iarg + 1]); OUT("nb_transform = " << Ntransform); iarg += 2; } else if (a == "-enable_distance_lower_bound") { CHECK_PARAM_NB(1, enable_distance_lower_bound); request.enable_distance_lower_bound = bool(atoi(argv[iarg + 1])); iarg += 2; } else if (a == "-limit") { CHECK_PARAM_NB(1, limit); limit = Scalar(atof(argv[iarg + 1])); iarg += 2; } else if (a == "-verbose") { verbose = true; iarg += 1; } else { break; } } } #define CREATE_SHAPE_2(var, Name) \ CHECK_PARAM_NB(2, Name); \ var.reset( \ new Name(Scalar(atof(argv[iarg + 1])), Scalar(atof(argv[iarg + 2])))); \ iarg += 3; Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) { if (iarg >= argc) throw std::invalid_argument("An argument is required."); std::string a(argv[iarg]); std::string type; CollisionGeometryPtr_t o; if (a == "-box") { CHECK_PARAM_NB(3, Box); o.reset(new Box(Scalar(atof(argv[iarg + 1])), Scalar(atof(argv[iarg + 2])), Scalar(atof(argv[iarg + 3])))); iarg += 4; type = "box"; } else if (a == "-sphere") { CHECK_PARAM_NB(1, Sphere); o.reset(new Sphere(Scalar(atof(argv[iarg + 1])))); iarg += 2; type = "sphere"; } else if (a == "-mesh") { CHECK_PARAM_NB(2, Mesh); OUT("Loading " << argv[iarg + 2] << " as BVHModel<" << argv[iarg + 1] << ">..."); if (strcmp(argv[iarg + 1], "obb") == 0) { o = meshToGeom(argv[iarg + 2]); OUT("Mesh has " << dynamic_pointer_cast >(o)->num_tris << " triangles"); type = "mesh_obb"; } else if (strcmp(argv[iarg + 1], "obbrss") == 0) { o = meshToGeom(argv[iarg + 2]); OUT("Mesh has " << dynamic_pointer_cast >(o)->num_tris << " triangles"); type = "mesh_obbrss"; } else throw std::invalid_argument("BV type must be obb or obbrss"); OUT("done."); iarg += 3; if (iarg < argc && strcmp(argv[iarg], "crop") == 0) { CHECK_PARAM_NB(6, Crop); coal::AABB aabb( Vec3s(Scalar(atof(argv[iarg + 1])), Scalar(atof(argv[iarg + 2])), Scalar(atof(argv[iarg + 3]))), Vec3s(Scalar(atof(argv[iarg + 4])), Scalar(atof(argv[iarg + 5])), Scalar(atof(argv[iarg + 6])))); OUT("Cropping " << aabb.min_.transpose() << " ---- " << aabb.max_.transpose() << " ..."); o->computeLocalAABB(); OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- " << o->aabb_local.max_.transpose() << " ..."); o.reset(extract(o.get(), Transform3s(), aabb)); if (!o) throw std::invalid_argument("Failed to crop."); OUT("Crop has " << dynamic_pointer_cast >(o)->num_tris << " triangles"); iarg += 7; } } else if (a == "-capsule") { CREATE_SHAPE_2(o, Capsule); type = "capsule"; } else if (a == "-cone") { CREATE_SHAPE_2(o, Cone); type = "cone"; } else if (a == "-cylinder") { CREATE_SHAPE_2(o, Cylinder); type = "cylinder"; } else { throw std::invalid_argument(std::string("Unknown argument: ") + a); } return Geometry(type, o); } int main(int argc, char** argv) { std::vector transforms; CollisionRequest request; if (argc > 1) { int iarg = 1; handleParam(iarg, argc, argv, request); Geometry first = makeGeomFromParam(iarg, argc, argv); Geometry second = makeGeomFromParam(iarg, argc, argv); Scalar extents[] = {-limit, -limit, -limit, limit, limit, limit}; generateRandomTransforms(extents, transforms, Ntransform); printResultHeaders(); Results results(Ntransform); collide(transforms, first.o.get(), second.o.get(), request, results); printResults(first, second, results); } else { Scalar extents[] = {-limit, -limit, -limit, limit, limit, limit}; generateRandomTransforms(extents, transforms, Ntransform); boost::filesystem::path path(TEST_RESOURCES_DIR); std::vector geoms; geoms.push_back(Geometry("Box", new Box(1, 2, 3))); geoms.push_back(Geometry("Sphere", new Sphere(2))); geoms.push_back(Geometry("Capsule", new Capsule(2, 1))); geoms.push_back(Geometry("Cone", new Cone(2, 1))); geoms.push_back(Geometry("Cylinder", new Cylinder(2, 1))); // geoms.push_back(Geometry ("Plane" , new Plane // (Vec3s(1,1,1).normalized(), 0) )); // geoms.push_back(Geometry ("Halfspace" , new Halfspace // (Vec3s(1,1,1).normalized(), 0) )); // not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP // (Vec3s(0,1,0), Vec3s(0,0,1), Vec3s(-1,0,0)) )); geoms.push_back(Geometry("rob BVHModel", objToGeom((path / "rob.obj").string()))); // geoms.push_back(Geometry ("rob BVHModel" , objToGeom ((path // / "rob.obj").string()))); geoms.push_back(Geometry ("rob BVHModel" // , objToGeom ((path / "rob.obj").string()))); geoms.push_back(Geometry("rob BVHModel", objToGeom((path / "rob.obj").string()))); geoms.push_back(Geometry("env BVHModel", objToGeom((path / "env.obj").string()))); // geoms.push_back(Geometry ("env BVHModel" , objToGeom ((path // / "env.obj").string()))); geoms.push_back(Geometry ("env BVHModel" // , objToGeom ((path / "env.obj").string()))); geoms.push_back(Geometry("env BVHModel", objToGeom((path / "env.obj").string()))); printResultHeaders(); // collision for (std::size_t i = 0; i < geoms.size(); ++i) { for (std::size_t j = i; j < geoms.size(); ++j) { if (!supportedPair(geoms[i].o.get(), geoms[j].o.get())) continue; Results results(Ntransform); collide(transforms, geoms[i].o.get(), geoms[j].o.get(), request, results); printResults(geoms[i], geoms[j], results); } } } return 0; } ================================================ FILE: test/python_unit/CMakeLists.txt ================================================ set( ${PROJECT_NAME}_PYTHON_TESTS geometric_shapes api collision collision_manager pickling ) if(COAL_PYTHON_NANOBIND) set(PYTHON_BINDINGS_BINARY_DIR "python-nb") else() set(PYTHON_BINDINGS_BINARY_DIR "python") endif() message( STATUS " Python tests will be run with Python executable at ${Python_EXECUTABLE}" ) add_dependencies(build_tests ${PROJECT_NAME}_python) foreach(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) ADD_PYTHON_UNIT_TEST( "${PROJECT_NAME}-py-${TEST}" "test/python_unit/${TEST}.py" ${PYTHON_BINDINGS_BINARY_DIR} ) endforeach() ================================================ FILE: test/python_unit/api.py ================================================ import unittest from test_case import TestCase import coal import numpy as np class TestMainAPI(TestCase): def test_collision(self): capsule = coal.Capsule(1.0, 2.0) M1 = coal.Transform3s() M2 = coal.Transform3s(np.eye(3), np.array([3, 0, 0])) req = coal.CollisionRequest() res = coal.CollisionResult() self.assertTrue(not coal.collide(capsule, M1, capsule, M2, req, res)) def test_distance(self): capsule = coal.Capsule(1.0, 2.0) M1 = coal.Transform3s() M2 = coal.Transform3s(np.eye(3), np.array([3, 0, 0])) req = coal.DistanceRequest() res = coal.DistanceResult() self.assertTrue(coal.distance(capsule, M1, capsule, M2, req, res) > 0) if __name__ == "__main__": unittest.main() ================================================ FILE: test/python_unit/collision.py ================================================ import unittest from test_case import TestCase import coal import numpy as np def tetahedron(): pts = coal.StdVec_Vec3s() pts.append(np.array((0, 0, 0))) pts.append(np.array((0, 1, 0))) pts.append(np.array((1, 0, 0))) pts.append(np.array((0, 0, 1))) tri = coal.StdVec_Triangle() tri.append(coal.Triangle(0, 1, 2)) tri.append(coal.Triangle(0, 1, 3)) tri.append(coal.Triangle(0, 2, 3)) tri.append(coal.Triangle(1, 2, 3)) return coal.Convex(pts, tri) class TestMainAPI(TestCase): def test_convex_halfspace(self): convex = tetahedron() halfspace = coal.Halfspace(np.array((0, 0, 1)), 0) req = coal.CollisionRequest() res = coal.CollisionResult() M1 = coal.Transform3s() M2 = coal.Transform3s(np.eye(3), np.array([0, 0, -0.1])) res.clear() coal.collide(convex, M1, halfspace, M2, req, res) self.assertFalse(coal.collide(convex, M1, halfspace, M2, req, res)) M1 = coal.Transform3s() M2 = coal.Transform3s(np.eye(3), np.array([0, 0, 0.1])) res.clear() self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res)) M1 = coal.Transform3s() M2 = coal.Transform3s(np.eye(3), np.array([0, 0, 2])) res.clear() self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res)) if __name__ == "__main__": unittest.main() ================================================ FILE: test/python_unit/collision_manager.py ================================================ import coal import numpy as np sphere = coal.Sphere(0.5) sphere_obj = coal.CollisionObject(sphere) M_sphere = coal.Transform3s.Identity() M_sphere.setTranslation(np.array([-0.6, 0.0, 0.0])) sphere_obj.setTransform(M_sphere) box = coal.Box(np.array([0.5, 0.5, 0.5])) box_obj = coal.CollisionObject(box) M_box = coal.Transform3s.Identity() M_box.setTranslation(np.array([-0.6, 0.0, 0.0])) box_obj.setTransform(M_box) collision_manager = coal.DynamicAABBTreeCollisionManager() collision_manager.registerObject(sphere_obj) collision_manager.registerObject(box_obj) assert collision_manager.size() == 2 collision_manager.setup() # Perform collision detection callback = coal.CollisionCallBackDefault() collision_manager.collide(sphere_obj, callback) assert callback.data.result.numContacts() == 1 ================================================ FILE: test/python_unit/geometric_shapes.py ================================================ import unittest from test_case import TestCase import coal import numpy as np class TestGeometricShapes(TestCase): def test_capsule(self): capsule = coal.Capsule(1.0, 2.0) self.assertIsInstance(capsule, coal.Capsule) self.assertIsInstance(capsule, coal.ShapeBase) self.assertIsInstance(capsule, coal.CollisionGeometry) self.assertEqual(capsule.getNodeType(), coal.NODE_TYPE.GEOM_CAPSULE) self.assertEqual(capsule.radius, 1.0) self.assertEqual(capsule.halfLength, 1.0) capsule.radius = 3.0 capsule.halfLength = 4.0 self.assertEqual(capsule.radius, 3.0) self.assertEqual(capsule.halfLength, 4.0) com = capsule.computeCOM() self.assertApprox(com, np.zeros(3)) V = capsule.computeVolume() V_cylinder = capsule.radius * capsule.radius * np.pi * 2.0 * capsule.halfLength V_sphere = 4.0 * np.pi / 3 * capsule.radius**3 V_ref = V_cylinder + V_sphere self.assertApprox(V, V_ref) I0 = capsule.computeMomentofInertia() Iz_cylinder = V_cylinder * capsule.radius**2 / 2.0 Iz_sphere = 0.4 * V_sphere * capsule.radius * capsule.radius Iz_ref = Iz_cylinder + Iz_sphere Ix_cylinder = ( V_cylinder * (3 * capsule.radius**2 + 4 * capsule.halfLength**2) / 12.0 ) V_hemi = 0.5 * V_sphere # volume of hemisphere I0x_hemi = 0.5 * Iz_sphere # inertia of hemisphere w.r.t. origin com_hemi = 3.0 * capsule.radius / 8.0 # CoM of hemisphere w.r.t. origin Icx_hemi = ( I0x_hemi - V_hemi * com_hemi * com_hemi ) # inertia of hemisphere w.r.t. CoM Ix_hemi = ( Icx_hemi + V_hemi * (capsule.halfLength + com_hemi) ** 2 ) # inertia of hemisphere w.r.t. tip of cylinder Ix_ref = Ix_cylinder + 2 * Ix_hemi # total inertia of capsule I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref]) self.assertApprox(I0, I0_ref) Ic = capsule.computeMomentofInertiaRelatedToCOM() self.assertApprox(Ic, I0_ref) def test_box1(self): box = coal.Box(np.array([1.0, 2.0, 3.0])) self.assertIsInstance(box, coal.Box) self.assertIsInstance(box, coal.ShapeBase) self.assertIsInstance(box, coal.CollisionGeometry) self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX) self.assertTrue(np.array_equal(box.halfSide, np.array([0.5, 1.0, 1.5]))) box.halfSide = np.array([4.0, 5.0, 6.0]) self.assertTrue(np.array_equal(box.halfSide, np.array([4.0, 5.0, 6.0]))) com = box.computeCOM() self.assertApprox(com, np.zeros(3)) V = box.computeVolume() x = float(2 * box.halfSide[0]) y = float(2 * box.halfSide[1]) z = float(2 * box.halfSide[2]) V_ref = x * y * z self.assertApprox(V, V_ref) I0 = box.computeMomentofInertia() Ix = V_ref * (y * y + z * z) / 12.0 Iy = V_ref * (x * x + z * z) / 12.0 Iz = V_ref * (y * y + x * x) / 12.0 I0_ref = np.diag([Ix, Iy, Iz]) self.assertApprox(I0, I0_ref) Ic = box.computeMomentofInertiaRelatedToCOM() self.assertApprox(Ic, I0_ref) def test_box2(self): box = coal.Box(1.0, 2.0, 3) self.assertIsInstance(box, coal.Box) self.assertIsInstance(box, coal.ShapeBase) self.assertIsInstance(box, coal.CollisionGeometry) self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX) self.assertEqual(box.halfSide[0], 0.5) self.assertEqual(box.halfSide[1], 1.0) self.assertEqual(box.halfSide[2], 1.5) def test_sphere(self): sphere = coal.Sphere(1.0) self.assertIsInstance(sphere, coal.Sphere) self.assertIsInstance(sphere, coal.ShapeBase) self.assertIsInstance(sphere, coal.CollisionGeometry) self.assertEqual(sphere.getNodeType(), coal.NODE_TYPE.GEOM_SPHERE) self.assertEqual(sphere.radius, 1.0) sphere.radius = 2.0 self.assertEqual(sphere.radius, 2.0) com = sphere.computeCOM() self.assertApprox(com, np.zeros(3)) V = sphere.computeVolume() V_ref = 4.0 * np.pi / 3 * sphere.radius**3 self.assertApprox(V, V_ref) I0 = sphere.computeMomentofInertia() I0_ref = 0.4 * V_ref * sphere.radius * sphere.radius * np.identity(3) self.assertApprox(I0, I0_ref) Ic = sphere.computeMomentofInertiaRelatedToCOM() self.assertApprox(Ic, I0_ref) def test_cylinder(self): cylinder = coal.Cylinder(1.0, 2.0) self.assertIsInstance(cylinder, coal.Cylinder) self.assertIsInstance(cylinder, coal.ShapeBase) self.assertIsInstance(cylinder, coal.CollisionGeometry) self.assertEqual(cylinder.getNodeType(), coal.NODE_TYPE.GEOM_CYLINDER) self.assertEqual(cylinder.radius, 1.0) self.assertEqual(cylinder.halfLength, 1.0) cylinder.radius = 3.0 cylinder.halfLength = 4.0 self.assertEqual(cylinder.radius, 3.0) self.assertEqual(cylinder.halfLength, 4.0) com = cylinder.computeCOM() self.assertApprox(com, np.zeros(3)) V = cylinder.computeVolume() V_ref = cylinder.radius * cylinder.radius * np.pi * 2.0 * cylinder.halfLength self.assertApprox(V, V_ref) I0 = cylinder.computeMomentofInertia() Ix_ref = V_ref * (3 * cylinder.radius**2 + 4 * cylinder.halfLength**2) / 12.0 Iz_ref = V_ref * cylinder.radius**2 / 2.0 I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref]) self.assertApprox(I0, I0_ref) Ic = cylinder.computeMomentofInertiaRelatedToCOM() self.assertApprox(Ic, I0_ref) def test_cone(self): cone = coal.Cone(1.0, 2.0) self.assertIsInstance(cone, coal.Cone) self.assertIsInstance(cone, coal.ShapeBase) self.assertIsInstance(cone, coal.CollisionGeometry) self.assertEqual(cone.getNodeType(), coal.NODE_TYPE.GEOM_CONE) self.assertEqual(cone.radius, 1.0) self.assertEqual(cone.halfLength, 1.0) cone.radius = 3.0 cone.halfLength = 4.0 self.assertEqual(cone.radius, 3.0) self.assertEqual(cone.halfLength, 4.0) com = cone.computeCOM() self.assertApprox(com, np.array([0.0, 0.0, -0.5 * cone.halfLength])) V = cone.computeVolume() V_ref = np.pi * cone.radius**2 * 2.0 * cone.halfLength / 3.0 self.assertApprox(V, V_ref) I0 = cone.computeMomentofInertia() Ix_ref = V_ref * (3.0 / 20.0 * cone.radius**2 + 0.4 * cone.halfLength**2) Iz_ref = 0.3 * V_ref * cone.radius**2 I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref]) self.assertApprox(I0, I0_ref) Ic = cone.computeMomentofInertiaRelatedToCOM() Icx_ref = V_ref * 3.0 / 20.0 * (cone.radius**2 + cone.halfLength**2) Ic_ref = np.diag([Icx_ref, Icx_ref, Iz_ref]) self.assertApprox(Ic, Ic_ref) def test_BVH(self): bvh = coal.BVHModelOBBRSS() self.assertEqual(bvh.num_vertices, 0) self.assertEqual(bvh.vertices().shape, (0, 3)) def test_convex(self): verts = coal.StdVec_Vec3s() faces = coal.StdVec_Triangle() verts.extend( [ np.array([0, 0, 0]), np.array([0, 1, 0]), np.array([1, 0, 0]), ] ) faces.append(coal.Triangle(0, 1, 2)) coal.Convex(verts, faces) verts.append(np.array([0, 0, 1])) try: coal.Convex.convexHull(verts, False, None) qhullAvailable = True except Exception as e: self.assertIn( "Library built without qhull. Cannot build object of this type.", str(e) ) qhullAvailable = False if qhullAvailable: coal.Convex.convexHull(verts, False, "") coal.Convex.convexHull(verts, True, "") try: coal.Convex.convexHull(verts[:3], False, None) except Exception as e: self.assertIn( "You shouldn't use this function with less than 4 points.", str(e) ) if __name__ == "__main__": unittest.main() ================================================ FILE: test/python_unit/pickling.py ================================================ import unittest from test_case import TestCase import coal import pickle import numpy as np def tetahedron(): pts = coal.StdVec_Vec3s() pts.append(np.array((0, 0, 0))) pts.append(np.array((0, 1, 0))) pts.append(np.array((1, 0, 0))) pts.append(np.array((0, 0, 1))) tri = coal.StdVec_Triangle() tri.append(coal.Triangle(0, 1, 2)) tri.append(coal.Triangle(0, 1, 3)) tri.append(coal.Triangle(0, 2, 3)) tri.append(coal.Triangle(1, 2, 3)) return coal.Convex(pts, tri) class TestGeometryPickling(TestCase): def pickling(self, obj): with open("save.p", "wb") as f: pickle.dump(obj, f) with open("save.p", "rb") as f: obj2 = pickle.load(f) self.assertTrue(obj == obj2) def test_all_shapes(self): box = coal.Box(1.0, 2.0, 3.0) self.pickling(box) trans = coal.Transform3s(np.array((0, 0, 0))) self.pickling(trans) sphere = coal.Sphere(1.0) self.pickling(sphere) ellipsoid = coal.Ellipsoid(1.0, 2.0, 3.0) self.pickling(ellipsoid) convex = tetahedron() self.pickling(convex) capsule = coal.Capsule(1.0, 2.0) self.pickling(capsule) cylinder = coal.Cylinder(1.0, 2.0) self.pickling(cylinder) plane = coal.Plane(np.array([0.0, 0.0, 1.0]), 2.0) self.pickling(plane) half_space = coal.Halfspace(np.array([0.0, 0.0, 1.0]), 2.0) self.pickling(half_space) if __name__ == "__main__": unittest.main() ================================================ FILE: test/python_unit/test_case.py ================================================ import unittest import numpy as np class TestCase(unittest.TestCase): def assertApprox(self, a, b, epsilon=1e-6): return self.assertTrue(np.allclose(a, b, epsilon), "%s !~= %s" % (a, b)) ================================================ FILE: test/scripts/collision-bench.py ================================================ import csv import sys import matplotlib.pyplot as plt filename = sys.argv[1] with open(filename, "r") as file: reader = csv.reader(file, strict=True) fieldnames = None # fieldnames = reader.fieldnames converters = (str, int, int, int, float, lambda x: float(x) * 1e-3) for row in reader: if fieldnames is None: fieldnames = [n.strip() for n in row] values = [] continue values.append([c(v) for v, c in zip(row, converters)]) request1 = values[: int(len(values) / 2)] request2 = values[int(len(values) / 2) :] Ntransforms = 1 while values[0][0:3] == values[Ntransforms][0:3]: Ntransforms += 1 splitMethods = ["avg", "med", "cen"] type = [ "o", "or", "r", ] BVs = sorted(list(set([v[0] for v in request1[::Ntransforms]]))) xvals = [ BVs.index(v[0]) + len(BVs) * v[2] + 3 * len(BVs) * v[1] for v in request1[::Ntransforms] ] cases = [ v[0] + " " + type[v[1]] + " " + splitMethods[v[2]] for v in request1[::Ntransforms] ] idx_reorder = sorted(list(range(len(xvals))), key=lambda i: xvals[i]) def reorder(v): return [v[i] for i in idx_reorder] xvals_s = reorder(xvals) cases_s = reorder(cases) onlyLB = True # Time plt.figure(0) for i in range(Ntransforms): if not onlyLB: plt.plot( xvals_s, reorder([v[5] for v in request1[i::Ntransforms]]), "-.o", label=str(i), ) plt.plot( xvals_s, reorder([v[5] for v in request2[i::Ntransforms]]), ":+", label=str(i) + "+lb", ) plt.legend() plt.xticks(ticks=xvals_s, labels=cases_s, rotation=90) plt.ylabel("Time (us)") plt.yscale("log") # Time plt.figure(2) for k in range(0, len(request1), Ntransforms): if not onlyLB: plt.plot( [ xvals[int(k / Ntransforms)], ], sum([v[5] for v in request1[k : k + Ntransforms]]) / Ntransforms, ) plt.plot( [ xvals[int(k / Ntransforms)], ], sum([v[5] for v in request2[k : k + Ntransforms]]) / Ntransforms, ) plt.plot( xvals_s, reorder( [ sum([v[5] for v in request2[k : k + Ntransforms]]) / Ntransforms for k in range(0, len(request1), Ntransforms) ] ), ) plt.xticks(ticks=xvals_s, labels=cases_s, rotation=90) plt.ylabel("Time (us)") plt.yscale("log") # Distance lower bound plt.figure(1) for i in range(Ntransforms): if request2[i][3] > 0: continue plt.plot( xvals_s, reorder([v[4] for v in request2[i::Ntransforms]]), ":+", label=str(i) + "+lb", ) plt.legend() plt.ylabel("Distance lower bound") plt.xticks(ticks=xvals_s, labels=cases_s, rotation=90) plt.show() ================================================ FILE: test/scripts/collision.py ================================================ # Load "env.obj" and "rob.obj" in gepetto-gui import os from gepetto.corbaserver import Client path = None devel_hpp_dir = os.getenv("DEVEL_HPP_DIR") if devel_hpp_dir: path = devel_hpp_dir + "/src/hpp-fcl/test/fcl_resources" else: path = os.getenv("PWD") + "/fcl_resources" Red = [1, 0, 0, 0.5] Green = [0, 1, 0, 0.5] Blue = [0, 0, 1, 0.5] c = Client() wid = 0 sceneName = "scene" wid = c.gui.createWindow("test-fcl") c.gui.createScene(sceneName) c.gui.addSceneToWindow(sceneName, wid) c.gui.addMesh("env", path + "/env.obj") c.gui.addMesh("rob", path + "/rob.obj") c.gui.addToGroup("env", sceneName) c.gui.addToGroup("rob", sceneName) q2 = (0, 0, 0, 0, 0, 0, 1) q1 = (-1435.35587657243, 2891.398094594479, 1462.830701842904) + ( 0.6741912139367736, -0.2384437590607974, 0.6418622372743962, -0.2768097707389008, ) c.gui.applyConfiguration("env", q1) c.gui.applyConfiguration("rob", q2) c.gui.refresh() ================================================ FILE: test/scripts/distance_lower_bound.py ================================================ # Load "env.obj" and "rob.obj" in gepetto-gui import os from gepetto.corbaserver import Client path = None devel_hpp_dir = os.getenv("DEVEL_HPP_DIR") if devel_hpp_dir: path = devel_hpp_dir + "/src/hpp-fcl/test/fcl_resources" else: path = os.getenv("PWD") + "/fcl_resources" Red = [1, 0, 0, 0.5] Green = [0, 1, 0, 0.5] Blue = [0, 0, 1, 0.5] c = Client() wid = 0 sceneName = "scene" wid = c.gui.createWindow("test-fcl") c.gui.createScene(sceneName) c.gui.addSceneToWindow(sceneName, wid) c.gui.addMesh("env", path + "/env.obj") c.gui.addToGroup("env", sceneName) c.gui.addBox("box", 500, 200, 150, Blue) c.gui.addToGroup("box", sceneName) q2 = (0, 0, 0, 0, 0, 0, 1) q1 = ( 608.56046341359615, 1624.1152798756957, 2661.5910432301462, 0.8083991299501978, 0.25803832576728564, 0.47026407332553366, 0.24240208429437343, ) c.gui.applyConfiguration("env", q1) c.gui.applyConfiguration("rob", q2) c.gui.refresh() ================================================ FILE: test/scripts/geometric_shapes.py ================================================ # Datas for compare_convex_box from gepetto import Quaternion from gepetto.corbaserver import Client def translate(tr, t, d): return [tr[i] + d * t[i] for i in range(3)] + tr[3:] cl = Client() try: cl.gui.getWindowID("fcl") except Exception: cl.gui.createWindow("fcl") cl.gui.addBox("fcl/b0", 2, 2, 2, [1, 0, 0, 0.5]) cl.gui.addBox("fcl/b1", 2, 2, 2, [0, 1, 0, 0.5]) cl.gui.setWireFrameMode("fcl/b1", "WIREFRAME") cl.gui.addBox("fcl/b1_0", 2, 2, 2, [0, 0, 1, 0.5]) cl.gui.addBox("fcl/b1_1", 2, 2, 2, [0, 0.5, 1, 0.5]) cl.gui.addSphere("fcl/p0", 0.01, [1, 0, 1, 1]) cl.gui.addSphere("fcl/p1", 0.01, [0, 1, 1, 1]) cl.gui.addArrow("fcl/n0", 0.01, 1.0, [1, 0, 1, 1]) cl.gui.addArrow("fcl/n1", 0.01, 1.0, [0, 1, 1, 1]) eps = 0.0 d0 = 1.5183589910964868 + eps n0 = [0.0310588, 0.942603, -0.332467] d1 = 1.7485932899646754 + eps n1 = [0.132426, -0.0219519, -0.99095] qn0 = Quaternion() qn1 = Quaternion() qn0.fromTwoVector([1, 0, 0], n0) qn1.fromTwoVector([1, 0, 0], n1) pb1 = [0.135584, 0.933659, 0.290395, 0.119895, 0.977832, -0.164725, 0.0483272] pb1_0 = translate(pb1, n0, d0) pb1_1 = translate(pb1, n1, -d1) cl.gui.applyConfiguration("fcl/b1", pb1) cl.gui.applyConfiguration("fcl/b1_0", pb1_0) cl.gui.applyConfiguration("fcl/b1_1", pb1_1) cl.gui.applyConfigurations( ["fcl/p0", "fcl/p1"], [ [0.832569, 0.259513, -0.239598, 0, 0, 0, 1], [-0.879579, 0.719545, 0.171906, 0, 0, 0, 1], ], ) cl.gui.applyConfigurations( ["fcl/n0", "fcl/n1"], [ ( 0.832569, 0.259513, -0.239598, ) + qn0.toTuple(), ( -0.879579, 0.719545, 0.171906, ) + qn1.toTuple(), ], ) cl.gui.refresh() ================================================ FILE: test/scripts/gjk.py ================================================ # This script displays two triangles and two spheres in gepetto-gui # It is useful to debug distance computation between two triangles. from gepetto.corbaserver import Client Red = [1, 0, 0, 0.5] Green = [0, 1, 0, 0.5] Blue = [0, 0, 1, 0.5] c = Client() wid = len(c.gui.getWindowList()) - 1 sceneName = "scene/triangles" if sceneName in c.gui.getNodeList(): c.gui.deleteNode(sceneName, True) wid = c.gui.createWindow("triangles") c.gui.createScene(sceneName) c.gui.addSceneToWindow(sceneName, wid) P1 = (-0.6475786872429674, -0.519875255189778, 0.5955961037406681) P2 = (0.069105957031249998, -0.150722900390625, -0.42999999999999999) P3 = (0.063996093749999997, -0.15320971679687501, -0.42999999999999999) Q1 = (-25.655000000000001, -1.2858199462890625, 3.7249809570312502) Q2 = (-10.926, -1.284259033203125, 3.7281499023437501) Q3 = (-10.926, -1.2866180419921875, 3.72335400390625) tf1 = ( -12.824601270753471, -1.6840516940066426, 3.8914453043793844, -0.26862477561450587, -0.46249645019513175, 0.73064726592483387, -0.42437287410898855, ) tf2 = (0, 0, 0, 0, 0, 0, 1) c.gui.addTriangleFace("triangle1", P1, P2, P3, Red) c.gui.addTriangleFace("triangle2", Q1, Q2, Q3, Green) c.gui.addToGroup("triangle1", sceneName) c.gui.addToGroup("triangle2", sceneName) c.gui.applyConfiguration("triangle1", tf1) c.gui.applyConfiguration("triangle2", tf2) c.gui.refresh() ================================================ FILE: test/scripts/obb.py ================================================ import csv import sys from math import sqrt import matplotlib.pyplot as plt import numpy as np filename = sys.argv[1] with open(filename, "r") as file: reader = csv.reader(file, strict=True) fieldnames = None # fieldnames = reader.fieldnames for row in reader: if fieldnames is None: fieldnames = [n.strip() for n in row] values = [[] for _ in fieldnames] continue values[0].append(int(row[0])) for i, v in enumerate(row[1:]): values[i + 1].append(float(v)) # Compute mean and variance for each values, for each separating axis means = [ [ 0.0, ] * 12 for _ in fieldnames[4:] ] stddevs = [ [ 0.0, ] * 12 for _ in fieldnames[4:] ] nb_occurence = [ 0, ] * 12 for i, id in enumerate(values[0]): nb_occurence[id] += 1 for i, id in enumerate(values[0]): for k, n in enumerate(fieldnames[4:]): v = values[k + 4][i] means[k][id] += v / nb_occurence[id] stddevs[k][id] += v * v / nb_occurence[id] for k, n in enumerate(fieldnames[4:]): for id in range(12): # means [k][id] /= nb_occurence[id] # stddevs[k][id] = sqrt (stddevs[k][id]) / nb_occurence[id] - means[k][id]) stddevs[k][id] = sqrt(stddevs[k][id] - means[k][id] * means[k][id]) subplots = False Nrows = 1 Ncols = 3 iplot = 1 time_vs_sep_axis = True nb_occ_sep_axis = False avg_time_vs_impl = True if time_vs_sep_axis: if subplots: plt.subplot(Nrows, Ncols, iplot) else: plt.figure(iplot) plt.title("Time, with std dev, versus separating axis") for k, n in enumerate(fieldnames[4:]): # plt.errorbar( # [ # np.linspace(0, 11, 12) + shift # for shift in np.linspace( # -0.2, # 0.2, # ) # ], # means[k], # stddevs[k], # label=n, # ) plt.errorbar(np.linspace(0, 11, 12), means[k], stddevs[k], label=n) # plt.errorbar( # np.linspace(0, 11, 12), # means[k], # [[0] * len(stddevs[k]), stddevs[k]], # label=n, # ) plt.xlim([-0.5, 11.5]) plt.ylabel("Time (ns)") plt.xlabel("Separating axis") plt.legend(loc="upper left") axx = plt.gca().twinx() axx.hist( values[0], bins=[i - 0.5 for i in range(13)], bottom=-0.5, cumulative=True, rwidth=0.5, fill=False, label="Cumulative occurence", ) axx.set_ylabel("Nb occurence of a separating axis.") plt.legend(loc="lower right") iplot += 1 if nb_occ_sep_axis: if subplots: plt.subplot(Nrows, Ncols, iplot) else: plt.figure(iplot) plt.title("Nb of occurence per separating axis") plt.hist(values[0], bins=[i - 0.5 for i in range(13)]) plt.ylabel("Nb occurence") plt.xlabel("Separating axis") dlb_id = 1 d_id = 2 # plt.title ("Time, with std dev, versus distance") # for k, n in enumerate(fieldnames[4:]): # plt.plot (values[dlb_id], values[k+4], '.', label=n) iplot += 1 if avg_time_vs_impl: if subplots: plt.subplot(Nrows, Ncols, iplot) else: plt.figure(iplot) plt.title("Average time versus the implementation") # plt.boxplot(values[4:], labels=fieldnames[4:], showmeans=True) _mins = np.min(values[4:], axis=1) _maxs = np.max(values[4:], axis=1) _means = np.mean(values[4:], axis=1) _stddev = np.std(values[4:], axis=1) _sorted = sorted( zip(fieldnames[4:], _means, _stddev, _mins, _maxs), key=lambda x: x[1] ) plt.errorbar( [f for f, _, _, _, _ in _sorted], [m for _, m, _, _, _ in _sorted], [s for _, _, s, _, _ in _sorted], fmt="go", linestyle="", label="mean and std deviation", ) plt.plot( [f for f, _, _, _, _ in _sorted], [v for _, _, _, v, _ in _sorted], "b+", ls="", label="min", ) plt.plot( [f for f, _, _, _, _ in _sorted], [u for _, _, _, _, u in _sorted], "r+", ls="", label="max", ) plt.ylabel("Time (ns)") plt.xticks(rotation=20) plt.legend() plt.show() ================================================ FILE: test/scripts/octree.py ================================================ # Load "env.obj" and "rob.obj" in gepetto-gui import csv import os from gepetto.corbaserver import Client pos = list() with open("/home/florent/devel/hpp/src/hpp-fcl/build-rel/test/rob.octree", "r") as f: r = csv.reader(f, delimiter=",") for line in r: pos.append(map(float, line)) path = None devel_hpp_dir = os.getenv("DEVEL_HPP_DIR") if devel_hpp_dir: path = devel_hpp_dir + "/src/hpp-fcl/test/fcl_resources" else: path = os.getenv("PWD") + "/fcl_resources" Red = [1, 0, 0, 0.5] Green = [0, 1, 0, 0.5] Blue = [0, 0, 1, 0.5] c = Client() wid = 0 sceneName = "scene" wid = c.gui.createWindow("test-fcl") c.gui.createScene(sceneName) c.gui.addSceneToWindow(sceneName, wid) c.gui.addMesh("env", path + "/env.obj") c.gui.addMesh("rob", path + "/rob.obj") c.gui.addToGroup("env", sceneName) c.gui.addToGroup("rob", sceneName) # closest points c.gui.addSphere("p1", 10, Red) c.gui.addToGroup("p1", sceneName) q1 = (-1373.283643275499, -396.2224237620831, 259.5808934420347) + ( 0.5956794918568784, -0.2147188057951074, 0.257436335996247, 0.7299234962157893, ) q2 = (0, 0, 0, 0, 0, 0, 1) c.gui.applyConfiguration("rob", q1) c.gui.applyConfiguration("env", q2) p1 = (-1990.2983245164919, -105.42114741459312, 359.74684390031132, 0, 0, 0, 1) c.gui.applyConfiguration("p1", p1) c.gui.refresh() ================================================ FILE: test/security_margin.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2022, INRIA. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #define BOOST_TEST_MODULE COAL_SECURITY_MARGIN #include #include #include #include "coal/distance.h" #include "coal/math/transform.h" #include "coal/collision.h" #include "coal/collision_object.h" #include "coal/shape/geometric_shapes.h" #include "coal/shape/geometric_shapes_utility.h" #include "coal/shape/geometric_shape_to_BVH_model.h" #include "utility.h" using namespace coal; using coal::CollisionGeometryPtr_t; using coal::CollisionRequest; using coal::CollisionResult; using coal::Transform3s; using coal::Vec3s; #define MATH_SQUARED(x) (x * x) BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3s tf1; const Transform3s tf2_collision(Vec3s(0, 1, 1)); coal::Box s1(1, 1, 1); coal::Box s2(1, 1, 1); const Scalar tol = Scalar(1e-8); AABB bv1, bv2; computeBV(s1, Transform3s(), bv1); computeBV(s2, Transform3s(), bv2); // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); Scalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); BOOST_CHECK_CLOSE(sqrDistLowerBound, 0, tol); } // No security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(0.01); Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); Scalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); BOOST_CHECK_CLOSE(sqrDistLowerBound, MATH_SQUARED(distance), tol); } // Security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(0.01); collisionRequest.security_margin = distance; Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); Scalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); BOOST_CHECK_SMALL(sqrDistLowerBound, tol); } // Negative security margin - collion because the two boxes are in contact { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(-0.01); collisionRequest.security_margin = distance; const Transform3s tf2( Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, distance))); AABB bv2_transformed; computeBV(s2, tf2, bv2_transformed); Scalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); BOOST_CHECK_SMALL(sqrDistLowerBound, tol); } // Negative security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(-0.01); collisionRequest.security_margin = distance; AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); Scalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); BOOST_CHECK_CLOSE( sqrDistLowerBound, MATH_SQUARED((std::sqrt(2) * collisionRequest.security_margin)), tol); } } BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3s tf1; const Transform3s tf2_collision(Vec3s(0, 0, 0)); coal::Box s1(1, 1, 1); coal::Box s2(1, 1, 1); AABB bv1, bv2; computeBV(s1, Transform3s(), bv1); computeBV(s2, Transform3s(), bv2); // The two AABB are collocated { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = -2.; collisionRequest.security_margin = distance; AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); Scalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); } const AABB bv3; BOOST_CHECK(!bv1.overlap(bv3)); } BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionGeometryPtr_t s1(new coal::Sphere(1)); CollisionGeometryPtr_t s2(new coal::Sphere(2)); const Transform3s tf1; const Transform3s tf2_collision(Vec3s(0, 0, 3)); // NOTE: when comparing a result to zero, **do not use BOOST_CHECK_CLOSE**! // Zero is not close to any other number, so the test will always fail. // Instead, use BOOST_CHECK_SMALL. // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; collide(s1.get(), tf1, s2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, Scalar(1e-8)); BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, Scalar(1e-8)); } // No security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const Scalar distance = Scalar(0.01); Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, Scalar(1e-8)); } // Positive security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const Scalar distance = Scalar(0.01); collisionRequest.security_margin = distance; Transform3s tf2( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, Scalar(1e-8)); BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance, 1e-8); } // Negative security margin - collion because the two spheres are in contact { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const Scalar distance = Scalar(-0.01); collisionRequest.security_margin = distance; Transform3s tf2( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, Scalar(1e-8)); BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance, Scalar(1e-8)); } // Negative security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; collisionRequest.security_margin = Scalar(-0.01); collide(s1.get(), tf1, s2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, -collisionRequest.security_margin, Scalar(1e-8)); } } BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionGeometryPtr_t c1(new coal::Capsule(0.5, 1.)); CollisionGeometryPtr_t c2(new coal::Capsule(0.5, 1.)); const Transform3s tf1; const Transform3s tf2_collision(Vec3s(0, 1., 0)); // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; collide(c1.get(), tf1, c2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, Scalar(1e-8)); BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, Scalar(1e-8)); } // No security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const Scalar distance = Scalar(0.01); Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, Scalar(1e-8)); } // Positive security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const Scalar distance = Scalar(0.01); collisionRequest.security_margin = distance; Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, Scalar(1e-8)); BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance, Scalar(1e-8)); } // Negative security margin - collion because the two capsules are in contact { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const Scalar distance = Scalar(-0.01); collisionRequest.security_margin = distance; Transform3s tf2( Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, Scalar(1e-8)); BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance, Scalar(1e-8)); } // Negative security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; collisionRequest.security_margin = Scalar(-0.01); collide(c1.get(), tf1, c2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, Scalar(0.01), Scalar(1e-8)); } } BOOST_AUTO_TEST_CASE(box_box) { CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3s tf1; const Transform3s tf2_collision(Vec3s(0, 1, 1)); const Scalar tol = Scalar(1e-3); // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, Scalar(1e-8)); } // No security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(0.01); const Transform3s tf2_no_collision( (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval()); CollisionResult collisionResult; collide(b1.get(), tf1, b2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, tol); } // Positive security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(0.01); collisionRequest.security_margin = distance; CollisionResult collisionResult; collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, -collisionRequest.security_margin, tol); BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, Scalar(1e-8)); } // Negative security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); collisionRequest.security_margin = Scalar(-0.01); CollisionResult collisionResult; collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, -collisionRequest.security_margin, tol); } // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(-0.01); collisionRequest.security_margin = distance; CollisionResult collisionResult; const Transform3s tf2((tf2_collision.getTranslation() + Vec3s(0, collisionRequest.security_margin, collisionRequest.security_margin)) .eval()); collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance, tol); } } template void test_shape_shape(const ShapeType1& shape1, const Transform3s& tf1, const ShapeType2& shape2, const Transform3s& tf2_collision, const Scalar tol) { // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, Scalar(1e-8)); } // No security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(0.01); const Transform3s tf2_no_collision( (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval()); CollisionResult collisionResult; collide(&shape1, tf1, &shape2, tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, tol); } // Positive security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(0.01); collisionRequest.security_margin = distance; CollisionResult collisionResult; collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, -collisionRequest.security_margin, tol); BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, Scalar(1e-8)); } // Negative security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); collisionRequest.security_margin = Scalar(-0.01); CollisionResult collisionResult; collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE( collisionResult.distance_lower_bound, (collisionRequest.security_margin * tf2_collision.getTranslation()) .norm(), tol); } // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); const Scalar distance = Scalar(-0.01); collisionRequest.security_margin = distance; CollisionResult collisionResult; const Transform3s tf2((tf2_collision.getTranslation() + Vec3s(0, collisionRequest.security_margin, collisionRequest.security_margin)) .eval()); collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance, tol); } } BOOST_AUTO_TEST_CASE(sphere_box) { Box* box_ptr = new coal::Box(1, 1, 1); CollisionGeometryPtr_t b1(box_ptr); BVHModel box_bvh_model = BVHModel(); generateBVHModel(box_bvh_model, *box_ptr, Transform3s()); box_bvh_model.buildConvexRepresentation(false); ConvexBase32& box_convex = *box_bvh_model.convex.get(); CollisionGeometryPtr_t s2(new coal::Sphere(0.5)); const Transform3s tf1; const Transform3s tf2_collision(Vec3s(0, 0, 1)); const Scalar tol = Scalar(1e-6); test_shape_shape(*b1.get(), tf1, *s2.get(), tf2_collision, tol); test_shape_shape(box_convex, tf1, *s2.get(), tf2_collision, tol); } ================================================ FILE: test/serialization.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2021-2023 INRIA. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #define BOOST_TEST_MODULE COAL_SERIALIZATION #include #include #include "coal/fwd.hh" COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #include "coal/collision.h" #include "coal/contact_patch.h" #include "coal/distance.h" #include "coal/BV/OBBRSS.h" #include "coal/BVH/BVH_model.h" #include "coal/serialization/collision_data.h" #include "coal/serialization/contact_patch.h" #include "coal/serialization/AABB.h" #include "coal/serialization/BVH_model.h" #include "coal/serialization/hfield.h" #include "coal/serialization/transform.h" #include "coal/serialization/geometric_shapes.h" #include "coal/serialization/convex.h" #include "coal/serialization/archive.h" #include "coal/serialization/memory.h" #ifdef COAL_HAS_OCTOMAP #include "coal/serialization/octree.h" #endif #include "utility.h" #include "fcl_resources/config.h" #include #include namespace utf = boost::unit_test::framework; using namespace coal; template bool check(const T& value, const T& other) { return value == other; } template bool check_ptr(const T* value, const T* other) { return *value == *other; } enum SerializationMode { TXT = 1, XML = 2, BIN = 4, STREAM = 8 }; template void test_serialization(const T* value, T& other_value, const int mode = TXT | XML | BIN | STREAM) { test_serialization(*value, other_value, mode); } template ::value> struct test_pointer_serialization_impl { static void run(const T&, T&, const int) {} }; template struct test_pointer_serialization_impl { static void run(const T& value, T& other_value, const int mode) { const CollisionGeometry* ptr = &value; CollisionGeometry* other_ptr = &other_value; const boost::filesystem::path tmp_path(boost::archive::tmpdir()); const boost::filesystem::path txt_path("file.txt"); const boost::filesystem::path txt_ptr_path("ptr_file.txt"); const boost::filesystem::path xml_path("file.xml"); const boost::filesystem::path bin_path("file.bin"); const boost::filesystem::path txt_filename(tmp_path / txt_path); const boost::filesystem::path xml_filename(tmp_path / xml_path); const boost::filesystem::path bin_filename(tmp_path / bin_path); // TXT if (mode & 0x1) { { std::ofstream ofs(txt_filename.c_str()); boost::archive::text_oarchive oa(ofs); oa << ptr; } BOOST_CHECK(check(*reinterpret_cast(ptr), *reinterpret_cast(ptr))); { std::ifstream ifs(txt_filename.c_str()); boost::archive::text_iarchive ia(ifs); ia >> other_ptr; } BOOST_CHECK( check(*reinterpret_cast(ptr), *reinterpret_cast(other_ptr))); } } }; template void test_pointer_serialization(const T& value, T& other_value, const int mode = TXT | XML | BIN | STREAM) { test_pointer_serialization_impl::run(value, other_value, mode); } template ::value, int>::type = 0> void test_serialization(const T& value, T& other_value, Hook post_load_hook, const int mode = TXT | XML | BIN | STREAM) { const boost::filesystem::path tmp_path(boost::archive::tmpdir()); const boost::filesystem::path txt_path("file.txt"); const boost::filesystem::path txt_ptr_path("ptr_file.txt"); const boost::filesystem::path xml_path("file.xml"); const boost::filesystem::path bin_path("file.bin"); const boost::filesystem::path txt_filename(tmp_path / txt_path); const boost::filesystem::path xml_filename(tmp_path / xml_path); const boost::filesystem::path bin_filename(tmp_path / bin_path); // TXT if (mode & 0x1) { // -- TXT { const std::string filename = txt_filename.string(); coal::serialization::saveToText(value, filename); BOOST_CHECK(check(value, value)); coal::serialization::loadFromText(other_value, filename); post_load_hook(other_value); BOOST_CHECK(check(value, other_value)); } // -- String stream (TXT format) { std::stringstream ss_out; coal::serialization::saveToStringStream(value, ss_out); BOOST_CHECK(check(value, value)); std::istringstream ss_in(ss_out.str()); coal::serialization::loadFromStringStream(other_value, ss_in); post_load_hook(other_value); BOOST_CHECK(check(value, other_value)); } // -- String { const std::string str_out = coal::serialization::saveToString(value); BOOST_CHECK(check(value, value)); const std::string str_in(str_out); coal::serialization::loadFromString(other_value, str_in); post_load_hook(other_value); BOOST_CHECK(check(value, other_value)); } } // XML if (mode & 0x2) { { const std::string filename = xml_filename.string(); const std::string xml_tag = "value"; coal::serialization::saveToXML(value, filename, xml_tag); BOOST_CHECK(check(value, value)); coal::serialization::loadFromXML(other_value, filename, xml_tag); post_load_hook(other_value); BOOST_CHECK(check(value, other_value)); } } // BIN if (mode & 0x4) { { const std::string filename = bin_filename.string(); coal::serialization::saveToBinary(value, filename); BOOST_CHECK(check(value, value)); coal::serialization::loadFromBinary(other_value, filename); post_load_hook(other_value); BOOST_CHECK(check(value, other_value)); } } // Stream Buffer if (mode & 0x8) { { boost::asio::streambuf buffer; coal::serialization::saveToBuffer(value, buffer); BOOST_CHECK(check(value, value)); coal::serialization::loadFromBuffer(other_value, buffer); post_load_hook(other_value); BOOST_CHECK(check(value, other_value)); } } // Test std::shared_ptr { const boost::filesystem::path txt_ptr_filename(tmp_path / txt_ptr_path); std::shared_ptr ptr = std::make_shared(value); const std::string filename = txt_ptr_filename.string(); coal::serialization::saveToText(ptr, filename); BOOST_CHECK(check_ptr(ptr.get(), ptr.get())); std::shared_ptr other_ptr = nullptr; coal::serialization::loadFromText(other_ptr, filename); post_load_hook(*other_ptr); BOOST_CHECK(check_ptr(ptr.get(), other_ptr.get())); } test_pointer_serialization(value, other_value); } template void test_serialization(const T& value, T& other_value, const int mode = TXT | XML | BIN | STREAM) { test_serialization(value, other_value, [](T&) {}, mode); } template ::value, int>::type = 0> void test_serialization(const T& value, Hook post_load_hook, const int mode = TXT | XML | BIN | STREAM) { T other_value; test_serialization(value, other_value, post_load_hook, mode); } template void test_serialization(const T& value, const int mode = TXT | XML | BIN | STREAM) { T other_value; test_serialization(value, other_value, mode); } BOOST_AUTO_TEST_CASE(test_aabb) { AABB aabb(-Vec3s::Ones(), Vec3s::Ones()); test_serialization(aabb); } BOOST_AUTO_TEST_CASE(test_collision_data) { Sphere sphere(1.); Box box(Vec3s::Ones()); // testing serialization with valid pointers // we need to add a post load hook to remap the contact's internal pointers to // the shapes Contact contact0(&sphere, &box, 1, 2, Vec3s::Ones(), Vec3s::Zero(), -10.); test_serialization(contact0, [&](Contact& c) { c.resolveReferences({&sphere, &box}); }); // testing serialization with NULL pointers Contact contact1(NULL, NULL, 1, 2, Vec3s::Ones(), Vec3s::Zero(), -10.); test_serialization(contact1); Contact contact2; test_serialization(contact2); CollisionRequest collision_request(CONTACT, 10); test_serialization(collision_request); CollisionResult collision_result; collision_result.addContact(contact0); collision_result.addContact(contact1); collision_result.addContact(contact2); collision_result.distance_lower_bound = Scalar(0.1); collision_result.normal.setOnes(); collision_result.nearest_points[0].setRandom(); collision_result.nearest_points[1].setRandom(); test_serialization(collision_result, [&](CollisionResult& cr) { cr.resolveReferences({{&sphere, &box}, {NULL, NULL}, {NULL, NULL}}); }); DistanceRequest distance_request(true, 1., 2.); test_serialization(distance_request); DistanceResult distance_result; distance_result.min_distance = 1.234; distance_result.normal.setOnes(); distance_result.nearest_points[0].setRandom(); distance_result.nearest_points[1].setRandom(); distance_result.o1 = &sphere; distance_result.o2 = &box; test_serialization(distance_result, [&](DistanceResult& dr) { dr.resolveReferences({&sphere, &box}); }); { // Serializing contact patches. const Halfspace hspace(0, 0, 1, 0); const Scalar radius = 0.25; const Scalar height = 1.; const Cylinder cylinder(radius, height); const Transform3s tf1; Transform3s tf2; // set translation to have a collision const Scalar offset = Scalar(0.001); tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); if (col_res.isCollision()) { ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); // Serialize patch request, result and the patch itself test_serialization(patch_req); test_serialization(patch_res); if (patch_res.numContactPatches() > 0) { test_serialization(patch_res.getContactPatch(0)); } } } } template void checkEqualStdVector(const std::vector& v1, const std::vector& v2) { BOOST_CHECK(v1.size() == v2.size()); if (v1.size() == v2.size()) { for (size_t i = 0; i < v1.size(); i++) { BOOST_CHECK(v1[i] == v2[i]); } } } BOOST_AUTO_TEST_CASE(test_BVHModel) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); BVHModel m1, m2; m1.beginModel(); m1.addSubModel(p1, t1); m1.endModel(); BOOST_CHECK(m1.num_vertices == p1.size()); BOOST_CHECK(m1.num_tris == t1.size()); checkEqualStdVector(*m1.vertices, p1); checkEqualStdVector(*m1.tri_indices, t1); BOOST_CHECK(m1 == m1); m2.beginModel(); m2.addSubModel(p2, t2); m2.endModel(); BOOST_CHECK(m2.num_vertices == p2.size()); BOOST_CHECK(m2.num_tris == t2.size()); checkEqualStdVector(*m2.vertices, p2); checkEqualStdVector(*m2.tri_indices, t2); BOOST_CHECK(m2 == m2); BOOST_CHECK(m1 != m2); // Test BVHModel { BVHModel m1_copy; test_serialization(m1, m1_copy); } { BVHModel m1_copy; test_serialization(m1, m1_copy, STREAM); } } #ifdef COAL_HAS_QHULL BOOST_AUTO_TEST_CASE(test_Convex) { std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel m1; m1.beginModel(); m1.addSubModel(p1, t1); m1.endModel(); m1.buildConvexHull(true); ConvexTpl& convex = static_cast&>(*m1.convex.get()); // Test Convex { ConvexTpl convex_copy; test_serialization(convex, convex_copy); } // Test std::shared_ptr { const boost::filesystem::path tmp_dir(boost::archive::tmpdir()); // TODO(louis): understand why serializing a shared_ptr // in TXT format fails only on MacOS + -O0. // const boost::filesystem::path txt_filename = tmp_dir / "file.txt"; // const boost::filesystem::path bin_filename = tmp_dir / "file.bin"; const boost::filesystem::path xml_filename = tmp_dir / "file.xml"; ConvexTpl convex_copy; std::shared_ptr ptr = std::make_shared>(convex); BOOST_CHECK(ptr.get()); const std::string filename = xml_filename.string(); const std::string tag_name = "CollisionGeometry"; coal::serialization::saveToXML(ptr, filename, tag_name); BOOST_CHECK( check(*reinterpret_cast*>(ptr.get()), convex)); std::shared_ptr other_ptr = nullptr; BOOST_CHECK(!other_ptr.get()); coal::serialization::loadFromXML(other_ptr, filename, tag_name); BOOST_CHECK(check( convex, *reinterpret_cast*>(other_ptr.get()))); } } #endif BOOST_AUTO_TEST_CASE(test_HeightField) { const Scalar min_altitude = -1.; const Scalar x_dim = 1., y_dim = 2.; const Eigen::DenseIndex nx = 100, ny = 200; const MatrixXs heights = MatrixXs::Random(ny, nx); HeightField hfield(x_dim, y_dim, heights, min_altitude); // Test HeightField { HeightField hfield_copy; test_serialization(hfield, hfield_copy); } { HeightField hfield_copy; test_serialization(hfield, hfield_copy, STREAM); } } BOOST_AUTO_TEST_CASE(test_transform) { Transform3s T; T.setQuatRotation(Quaternion3f::UnitRandom()); T.setTranslation(Vec3s::Random()); Transform3s T_copy; test_serialization(T, T_copy); } BOOST_AUTO_TEST_CASE(test_shapes) { { TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(), Vec3s::UnitZ()); triangle.setSweptSphereRadius(1.); triangle.computeLocalAABB(); TriangleP triangle_copy(Vec3s::Random(), Vec3s::Random(), Vec3s::Random()); test_serialization(triangle, triangle_copy); } { Box box(Vec3s::UnitX()), box_copy(Vec3s::Random()); box.setSweptSphereRadius(1.); box.computeLocalAABB(); test_serialization(box, box_copy); } { Sphere sphere(1.), sphere_copy(2.); sphere.setSweptSphereRadius(1.); sphere.computeLocalAABB(); test_serialization(sphere, sphere_copy); } { Ellipsoid ellipsoid(1., 2., 3.), ellipsoid_copy(0., 0., 0.); ellipsoid.setSweptSphereRadius(1.); ellipsoid.computeLocalAABB(); test_serialization(ellipsoid, ellipsoid_copy); } { Capsule capsule(1., 2.), capsule_copy(10., 10.); capsule.setSweptSphereRadius(1.); capsule.computeLocalAABB(); test_serialization(capsule, capsule_copy); } { Cone cone(1., 2.), cone_copy(10., 10.); cone.setSweptSphereRadius(1.); cone.computeLocalAABB(); test_serialization(cone, cone_copy); } { Cylinder cylinder(1., 2.), cylinder_copy(10., 10.); cylinder.setSweptSphereRadius(1.); cylinder.computeLocalAABB(); test_serialization(cylinder, cylinder_copy); } { Halfspace hs(Vec3s::Random(), 1.), hs_copy(Vec3s::Zero(), 0.); hs.setSweptSphereRadius(1.); hs.computeLocalAABB(); test_serialization(hs, hs_copy); } { Plane plane(Vec3s::Random(), 1.), plane_copy(Vec3s::Zero(), 0.); plane.setSweptSphereRadius(1.); plane.computeLocalAABB(); test_serialization(plane, plane_copy); } #ifdef COAL_HAS_QHULL { const size_t num_points = 500; std::shared_ptr> points = std::make_shared>(); points->reserve(num_points); for (size_t i = 0; i < num_points; i++) { points->emplace_back(Vec3s::Random()); } using Convex = ConvexTpl; std::unique_ptr convex = std::unique_ptr(static_cast(ConvexBase32::convexHull( points, static_cast(points->size()), true))); convex->setSweptSphereRadius(1.); convex->computeLocalAABB(); Convex convex_copy; test_serialization(*convex, convex_copy); } #endif } #ifdef COAL_HAS_OCTOMAP BOOST_AUTO_TEST_CASE(test_octree) { const Scalar resolution = Scalar(1e-2); const MatrixX3s points = MatrixX3s::Random(1000, 3); OcTreePtr_t octree_ptr = makeOctree(points, resolution); const OcTree& octree = *octree_ptr.get(); const boost::filesystem::path tmp_dir(boost::archive::tmpdir()); const boost::filesystem::path txt_filename = tmp_dir / "file.txt"; const boost::filesystem::path bin_filename = tmp_dir / "file.bin"; { std::ofstream ofs(bin_filename.c_str(), std::ios::binary); boost::archive::binary_oarchive oa(ofs); oa << octree; } OcTree octree_value(1.); { std::ifstream ifs(bin_filename.c_str(), std::fstream::binary | std::fstream::in); boost::archive::binary_iarchive ia(ifs); ia >> octree_value; } BOOST_CHECK(octree.getTree() == octree.getTree()); BOOST_CHECK(octree_value.getTree() == octree_value.getTree()); // BOOST_CHECK(octree.getTree() == octree_value.getTree()); BOOST_CHECK(octree.getResolution() == octree_value.getResolution()); BOOST_CHECK(octree.getTree()->size() == octree_value.getTree()->size()); BOOST_CHECK(octree.toBoxes().size() == octree_value.toBoxes().size()); BOOST_CHECK(octree == octree_value); OcTree octree_copy(1.); test_serialization(octree, octree_copy); } #endif BOOST_AUTO_TEST_CASE(test_memory_footprint) { Sphere sphere(1.); BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere)); std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel m1; m1.beginModel(); m1.addSubModel(p1, t1); m1.endModel(); std::cout << "computeMemoryFootprint(m1): " << computeMemoryFootprint(m1) << std::endl; BOOST_CHECK(sizeof(BVHModel) < computeMemoryFootprint(m1)); BOOST_CHECK(static_cast(m1.memUsage(false)) == computeMemoryFootprint(m1)); } COAL_COMPILER_DIAGNOSTIC_POP ================================================ FILE: test/shape_inflation.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2022, INRIA. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #define BOOST_TEST_MODULE COAL_SECURITY_MARGIN #include #include #include #include "coal/distance.h" #include "coal/math/transform.h" #include "coal/collision.h" #include "coal/collision_object.h" #include "coal/shape/geometric_shapes.h" #include "coal/shape/geometric_shapes_utility.h" #include "utility.h" using namespace coal; using coal::CollisionGeometryPtr_t; using coal::CollisionObject; using coal::CollisionRequest; using coal::CollisionResult; using coal::DistanceRequest; using coal::DistanceResult; using coal::Transform3s; using coal::Vec3s; #define MATH_SQUARED(x) (x * x) template bool isApprox(const Shape& s1, const Shape& s2, const Scalar tol); bool isApprox(const Scalar& v1, const Scalar& v2, const Scalar tol) { typedef Eigen::Matrix Matrix; Matrix m1; m1 << v1; Matrix m2; m2 << v2; return m1.isApprox(m2, tol); } bool isApprox(const Box& s1, const Box& s2, const Scalar tol) { return s1.halfSide.isApprox(s2.halfSide, tol); } bool isApprox(const Sphere& s1, const Sphere& s2, const Scalar tol) { return isApprox(s1.radius, s2.radius, tol); } bool isApprox(const Ellipsoid& s1, const Ellipsoid& s2, const Scalar tol) { return s1.radii.isApprox(s2.radii, tol); } bool isApprox(const Capsule& s1, const Capsule& s2, const Scalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } bool isApprox(const Cylinder& s1, const Cylinder& s2, const Scalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } bool isApprox(const Cone& s1, const Cone& s2, const Scalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } bool isApprox(const TriangleP& s1, const TriangleP& s2, const Scalar tol) { return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) && s1.c.isApprox(s2.c, tol); } bool isApprox(const Halfspace& s1, const Halfspace& s2, const Scalar tol) { return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol); } template void test(const Shape& original_shape, const Scalar inflation, const Scalar tol = Scalar(1e-8)) { // Zero inflation { const Scalar inflation = 0.; const auto& inflation_result = original_shape.inflated(inflation); const Transform3s& shift = inflation_result.second; const Shape& inflated_shape = inflation_result.first; BOOST_CHECK(isApprox(original_shape, inflated_shape, tol)); BOOST_CHECK(shift.isIdentity(tol)); } // Positive inflation { const auto& inflation_result = original_shape.inflated(inflation); const Shape& inflated_shape = inflation_result.first; const Transform3s& inflation_shift = inflation_result.second; BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); const auto& deflation_result = inflated_shape.inflated(-inflation); const Shape& deflated_shape = deflation_result.first; const Transform3s& deflation_shift = deflation_result.second; BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); } // Negative inflation { const auto& inflation_result = original_shape.inflated(-inflation); const Shape& inflated_shape = inflation_result.first; const Transform3s& inflation_shift = inflation_result.second; BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); const auto& deflation_result = inflated_shape.inflated(+inflation); const Shape& deflated_shape = deflation_result.first; const Transform3s& deflation_shift = deflation_result.second; BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); } } template void test_throw(const Shape& shape, const Scalar inflation) { BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument); } template void test_no_throw(const Shape& shape, const Scalar inflation) { BOOST_REQUIRE_NO_THROW(shape.inflated(inflation)); } BOOST_AUTO_TEST_CASE(test_inflate) { const coal::Sphere sphere(1); test(sphere, Scalar(0.01), Scalar(1e-8)); test_throw(sphere, Scalar(-1.1)); test_no_throw(sphere, 1.); const coal::Box box(1, 1, 1); test(box, Scalar(0.01), Scalar(1e-8)); test_throw(box, Scalar(-0.6)); const coal::Ellipsoid ellipsoid(1, 2, 3); test(ellipsoid, Scalar(0.01), Scalar(1e-8)); test_throw(ellipsoid, Scalar(-1.1)); const coal::Capsule capsule(1, 2); test(capsule, Scalar(0.01), Scalar(1e-8)); test_throw(capsule, Scalar(-1.1)); const coal::Cylinder cylinder(1, 2); test(cylinder, Scalar(0.01), Scalar(1e-8)); test_throw(cylinder, Scalar(-1.1)); const coal::Cone cone(1, 4); test(cone, Scalar(0.01), Scalar(1e-8)); test_throw(cone, Scalar(-1.1)); const coal::Halfspace halfspace(Vec3s::UnitZ(), 0); test(halfspace, Scalar(0.01), Scalar(1e-8)); // const coal::TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(), // Vec3s::UnitZ()); // test(triangle, 0.01, 1e-8); } ================================================ FILE: test/shape_mesh_consistency.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE COAL_SHAPE_MESH_CONSISTENCY #include #include "coal/narrowphase/narrowphase.h" #include "coal/shape/geometric_shape_to_BVH_model.h" #include "coal/distance.h" #include "coal/collision.h" #include "utility.h" using namespace coal; #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) Scalar extents[6] = {0, 0, 0, 10, 10, 10}; BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { Sphere s1(20); Sphere s2(20); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3s pose; pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3s(40.1, 0, 0)); res.clear(), res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3s()); generateBVHModel(s2_rss, s2, Transform3s()); DistanceRequest request; DistanceResult res, res1; Transform3s pose; pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } pose.setTranslation(Vec3s(15.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3s pose; pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } pose.setTranslation( Vec3s(15, 0, 0)); // libccd cannot use small value here :( res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_conecone) { Cone s1(5, 10); Cone s2(5, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3s pose; pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation( Vec3s(15, 0, 0)); // libccd cannot use small value here :( res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { Sphere s1(20); Sphere s2(20); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3s pose; pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3s(40.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); } } BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { Box s1(20, 40, 50); Box s2(10, 10, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3s()); generateBVHModel(s2_rss, s2, Transform3s()); DistanceRequest request; DistanceResult res, res1; Transform3s pose; pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } pose.setTranslation(Vec3s(15.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { Cylinder s1(5, 10); Cylinder s2(5, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3s pose; pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; else BOOST_CHECK( fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; else BOOST_CHECK( fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; else BOOST_CHECK( fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3s(10.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { Cone s1(5, 10); Cone s2(5, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3s pose; pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3s(10.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3s(), &s2, pose, request, res); distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { Transform3s t; generateRandomTransform(extents, t); Transform3s pose1(t); Transform3s pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { Sphere s1(20); Sphere s2(10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(40, 0, 0)); pose_aabb.setTranslation(Vec3s(40, 0, 0)); pose_obb.setTranslation(Vec3s(40, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(30, 0, 0)); pose_aabb.setTranslation(Vec3s(30, 0, 0)); pose_obb.setTranslation(Vec3s(30, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(29.9, 0, 0)); pose_aabb.setTranslation( Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(-29.9, 0, 0)); pose_aabb.setTranslation( Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(-30, 0, 0)); pose_aabb.setTranslation(Vec3s(-30, 0, 0)); pose_obb.setTranslation(Vec3s(-30, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s()); generateBVHModel(s2_aabb, s2, Transform3s()); generateBVHModel(s1_obb, s1, Transform3s()); generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(15.01, 0, 0)); pose_aabb.setTranslation(Vec3s(15.01, 0, 0)); pose_obb.setTranslation(Vec3s(15.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(14.99, 0, 0)); pose_aabb.setTranslation(Vec3s(14.99, 0, 0)); pose_obb.setTranslation(Vec3s(14.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { Sphere s1(20); Box s2(5, 5, 5); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3s()); generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(22.4, 0, 0)); pose_aabb.setTranslation(Vec3s(22.4, 0, 0)); pose_obb.setTranslation(Vec3s(22.4, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(22.51, 0, 0)); pose_aabb.setTranslation(Vec3s(22.51, 0, 0)); pose_obb.setTranslation(Vec3s(22.51, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; pose.setTranslation(Vec3s(9.99, 0, 0)); pose_aabb.setTranslation(Vec3s(9.99, 0, 0)); pose_obb.setTranslation(Vec3s(9.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(10.01, 0, 0)); pose_aabb.setTranslation(Vec3s(10.01, 0, 0)); pose_obb.setTranslation(Vec3s(10.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { Cone s1(5, 10); Cone s2(5, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; pose.setTranslation(Vec3s(9.9, 0, 0)); pose_aabb.setTranslation(Vec3s(9.9, 0, 0)); pose_obb.setTranslation(Vec3s(9.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(10.1, 0, 0)); pose_aabb.setTranslation(Vec3s(10.1, 0, 0)); pose_obb.setTranslation(Vec3s(10.1, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(0, 0, 9.9)); pose_aabb.setTranslation(Vec3s(0, 0, 9.9)); pose_obb.setTranslation(Vec3s(0, 0, 9.9)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(0, 0, 10.1)); pose_aabb.setTranslation(Vec3s(0, 0, 10.1)); pose_obb.setTranslation(Vec3s(0, 0, 10.1)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { Sphere s1(20); Sphere s2(10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(40, 0, 0)); pose_aabb.setTranslation(Vec3s(40, 0, 0)); pose_obb.setTranslation(Vec3s(40, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(30, 0, 0)); pose_aabb.setTranslation(Vec3s(30, 0, 0)); pose_obb.setTranslation(Vec3s(30, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(29.9, 0, 0)); pose_aabb.setTranslation( Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(-29.9, 0, 0)); pose_aabb.setTranslation( Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(-30, 0, 0)); pose_aabb.setTranslation(Vec3s(-30, 0, 0)); pose_obb.setTranslation(Vec3s(-30, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { Box s1(20, 40, 50); Box s2(10, 10, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s()); generateBVHModel(s2_aabb, s2, Transform3s()); generateBVHModel(s1_obb, s1, Transform3s()); generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(15.01, 0, 0)); pose_aabb.setTranslation(Vec3s(15.01, 0, 0)); pose_obb.setTranslation(Vec3s(15.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(14.99, 0, 0)); pose_aabb.setTranslation(Vec3s(14.99, 0, 0)); pose_obb.setTranslation(Vec3s(14.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { Sphere s1(20); Box s2(5, 5, 5); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3s()); generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(22.4, 0, 0)); pose_aabb.setTranslation(Vec3s(22.4, 0, 0)); pose_obb.setTranslation(Vec3s(22.4, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(22.51, 0, 0)); pose_aabb.setTranslation(Vec3s(22.51, 0, 0)); pose_obb.setTranslation(Vec3s(22.51, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { Cylinder s1(5, 10); Cylinder s2(5, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; pose.setTranslation(Vec3s(9.99, 0, 0)); pose_aabb.setTranslation(Vec3s(9.99, 0, 0)); pose_obb.setTranslation(Vec3s(9.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(10.01, 0, 0)); pose_aabb.setTranslation(Vec3s(10.01, 0, 0)); pose_obb.setTranslation(Vec3s(10.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { Cone s1(5, 10); Cone s2(5, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3s pose, pose_aabb, pose_obb; pose.setTranslation(Vec3s(9.9, 0, 0)); pose_aabb.setTranslation(Vec3s(9.9, 0, 0)); pose_obb.setTranslation(Vec3s(9.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(10.1, 0, 0)); pose_aabb.setTranslation(Vec3s(10.1, 0, 0)); pose_obb.setTranslation(Vec3s(10.1, 0, 0)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(0, 0, 9.9)); pose_aabb.setTranslation(Vec3s(0, 0, 9.9)); pose_obb.setTranslation(Vec3s(0, 0, 9.9)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(0, 0, 10.1)); pose_aabb.setTranslation(Vec3s(0, 0, 10.1)); pose_obb.setTranslation(Vec3s(0, 0, 10.1)); result.clear(); res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } ================================================ FILE: test/simple.cpp ================================================ #define BOOST_TEST_MODULE COAL_SIMPLE #include #include "coal/internal/intersect.h" #include "coal/collision.h" #include "coal/BVH/BVH_model.h" #include "fcl_resources/config.h" #include using namespace coal; static Scalar epsilon = Scalar(1e-6); static bool approx(Scalar x, Scalar y) { return std::abs(x - y) < epsilon; } BOOST_AUTO_TEST_CASE(projection_test_line) { Vec3s v1(0, 0, 0); Vec3s v2(2, 0, 0); Vec3s p(1, 0, 0); Project::ProjectResult res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); p = Vec3s(-1, 0, 0); res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 1)); BOOST_CHECK(approx(res.parameterization[0], 1)); BOOST_CHECK(approx(res.parameterization[1], 0)); p = Vec3s(3, 0, 0); res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 1)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 1)); } BOOST_AUTO_TEST_CASE(projection_test_triangle) { Vec3s v1(0, 0, 1); Vec3s v2(0, 1, 0); Vec3s v3(1, 0, 0); Vec3s p(1, 1, 1); Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 7); BOOST_CHECK(approx(res.sqr_distance, Scalar(4 / 3.0))); BOOST_CHECK(approx(res.parameterization[0], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[1], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[2], Scalar(1 / 3.0))); p = Vec3s(0, 0, 1.5); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 1)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); p = Vec3s(1.5, 0, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 4); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 1)); p = Vec3s(0, 1.5, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 1)); BOOST_CHECK(approx(res.parameterization[2], 0)); p = Vec3s(1, 1, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 6); BOOST_CHECK(approx(res.sqr_distance, 0.5)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); p = Vec3s(1, 0, 1); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 5); BOOST_CHECK(approx(res.sqr_distance, 0.5)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); p = Vec3s(0, 1, 1); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0.5)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0)); } BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { Vec3s v1(0, 0, 1); Vec3s v2(0, 1, 0); Vec3s v3(1, 0, 0); Vec3s v4(1, 1, 1); Vec3s p(0.5, 0.5, 0.5); Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 15); BOOST_CHECK(approx(res.sqr_distance, 0)); BOOST_CHECK(approx(res.parameterization[0], 0.25)); BOOST_CHECK(approx(res.parameterization[1], 0.25)); BOOST_CHECK(approx(res.parameterization[2], 0.25)); BOOST_CHECK(approx(res.parameterization[3], 0.25)); p = Vec3s(0, 0, 0); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 7); BOOST_CHECK(approx(res.sqr_distance, Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[0], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[1], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[2], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3s(0, 1, 1); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 11); BOOST_CHECK(approx(res.sqr_distance, Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[0], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[1], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], Scalar(1 / 3.0))); p = Vec3s(1, 1, 0); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 14); BOOST_CHECK(approx(res.sqr_distance, Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[2], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[3], Scalar(1 / 3.0))); p = Vec3s(1, 0, 1); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 13); BOOST_CHECK(approx(res.sqr_distance, Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[0], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], Scalar(1 / 3.0))); BOOST_CHECK(approx(res.parameterization[3], Scalar(1 / 3.0))); p = Vec3s(1.5, 1.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 8); BOOST_CHECK(approx(res.sqr_distance, 0.75)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 1)); p = Vec3s(1.5, -0.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 4); BOOST_CHECK(approx(res.sqr_distance, 0.75)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 1)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3s(-0.5, -0.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 0.75)); BOOST_CHECK(approx(res.parameterization[0], 1)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3s(-0.5, 1.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 0.75)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 1)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3s(0.5, -0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 5); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3s(0.5, 1.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 10); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); p = Vec3s(1.5, 0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 12); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); p = Vec3s(-0.5, 0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3s(0.5, 0.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 9); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); p = Vec3s(0.5, 0.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 6); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0)); } ================================================ FILE: test/swept_sphere_radius.cpp ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Louis Montaut */ #define BOOST_TEST_MODULE COAL_SWEPT_SPHERE_RADIUS #include #include "coal/narrowphase/narrowphase.h" #include "coal/collision_utility.h" #include "coal/serialization/geometric_shapes.h" #include "coal/serialization/convex.h" #include "coal/serialization/transform.h" #include "coal/serialization/archive.h" #include "utility.h" using namespace coal; NODE_TYPE node1_type; NODE_TYPE node2_type; int line; #define SET_LINE \ node1_type = shape1.getNodeType(); \ node2_type = shape2.getNodeType(); \ line = __LINE__ #define COAL_CHECK(cond) \ BOOST_CHECK_MESSAGE( \ cond, "from line " << line << ", for collision pair: " \ << get_node_type_name(node1_type) << " - " \ << get_node_type_name(node2_type) \ << " with ssr1 = " << shape1.getSweptSphereRadius() \ << ", ssr2 = " << shape2.getSweptSphereRadius() \ << ": " #cond) #define COAL_CHECK_VECTOR_CLOSE(v1, v2, tol) \ EIGEN_VECTOR_IS_APPROX(v1, v2, tol); \ COAL_CHECK(((v1) - (v2)).isZero(tol)) #define COAL_CHECK_REAL_CLOSE(v1, v2, tol) \ Scalar_IS_APPROX(v1, v2, tol); \ COAL_CHECK(std::abs((v1) - (v2)) < tol) #define COAL_CHECK_CONDITION(cond) \ BOOST_CHECK(cond); \ COAL_CHECK(cond) // Preambule: swept sphere radius allows to virually convolve geometric shapes // by a sphere with positive radius (Minkowski sum). // Sweeping a shape by a sphere corresponds to doing a Minkowski addition of the // shape with a sphere of radius r. Essentially, this rounds the shape's corners // and edges, which can be useful to smooth collision detection algorithms. // // A nice mathematical property of GJK and EPA is that it is not // necessary to take into account the swept sphere radius in the iterations of // the algorithms. This is because the GJK and EPA algorithms are based on the // Minkowski difference of the two objects. // With spheres of radii r1 and r2 swept around the shapes s1 and s2 of a // collision pair, the Minkowski difference is simply the Minkowski difference // of s1 and s2, summed with a sphere of radius r1 + r2. // This means that running GJK and EPA on the swept-sphere shapes is equivalent // to running GJK and EPA on the original shapes, and then augmenting the // distance by r1 + r2. // This does not modify the normal returned by GJK and EPA. // So we can also easily recover the witness points of the swept sphere shapes. // // This suite of test is designed to verify that property and generally test for // swept-sphere radius support in Coal. // Notes: // - not all collision pairs use GJK/EPA, so this test makes sure that // swept-sphere radius is taken into account even for specialized collision // algorithms. // - when manually taking swept-sphere radius into account in GJK/EPA, we // strongly deteriorate the convergence properties of the algorithms. This // is because certain parts of the shapes become locally strictly convex, // which GJK/EPA are not designed to handle. This becomes particularly the // bigger the swept-sphere radius. So don't be surprised if the tests fail // for large radii. struct SweptSphereGJKSolver : public GJKSolver { template Scalar shapeDistance( const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2, bool compute_penetration, Vec3s& p1, Vec3s& p2, Vec3s& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const { if (use_swept_sphere_radius_in_gjk_epa_iterations) { Scalar distance; this->runGJKAndEPA( s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal); return distance; } // Default behavior of coal's GJKSolver Scalar distance; this->runGJKAndEPA( s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal); return distance; } }; template void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { SweptSphereGJKSolver solver; // The swept sphere radius is detrimental to the convergence of GJK // and EPA. This gets worse as the radius of the swept sphere increases. // So we need to increase the number of iterations to get a good result. const Scalar tol = Scalar(1e-6); solver.gjk_tolerance = tol; solver.epa_tolerance = tol; solver.epa_max_iterations = 1000; const bool compute_penetration = true; Scalar extents[] = {-2, -2, -2, 2, 2, 2}; std::size_t n = 10; std::vector tf1s; std::vector tf2s; generateRandomTransforms(extents, tf1s, n); generateRandomTransforms(extents, tf2s, n); const std::array swept_sphere_radius = {0, Scalar(0.1), 1, 10}; for (const Scalar& ssr1 : swept_sphere_radius) { shape1.setSweptSphereRadius(ssr1); for (const Scalar& ssr2 : swept_sphere_radius) { shape2.setSweptSphereRadius(ssr2); for (std::size_t i = 0; i < n; ++i) { Transform3s tf1 = tf1s[i]; Transform3s tf2 = tf2s[i]; SET_LINE; std::array distance; std::array p1; std::array p2; std::array normal; // Default coal behavior - Don't take swept sphere radius into account // during GJK/EPA iterations. Correct the solution afterwards. distance[0] = solver.shapeDistance(shape1, tf1, shape2, tf2, compute_penetration, p1[0], p2[0], normal[0], false); // Take swept sphere radius into account during GJK/EPA iterations distance[1] = solver.shapeDistance(shape1, tf1, shape2, tf2, compute_penetration, p1[1], p2[1], normal[1], true); // Precision is dependent on the swept-sphere radius. // The issue of precision does not come from the default behavior of // coal, but from the result in which we manually take the swept // sphere radius into account in GJK/EPA iterations. const Scalar precision = 3 * sqrt(tol) + (1 / Scalar(100)) * std::max(shape1.getSweptSphereRadius(), shape2.getSweptSphereRadius()); // Check that the distance is the same COAL_CHECK_REAL_CLOSE(distance[0], distance[1], precision); // Check that the normal is the same COAL_CHECK_CONDITION(normal[0].dot(normal[1]) > 0); COAL_CHECK_CONDITION(std::abs(1 - normal[0].dot(normal[1])) < precision); // Check that the witness points are the same COAL_CHECK_VECTOR_CLOSE(p1[0], p1[1], precision); COAL_CHECK_VECTOR_CLOSE(p2[0], p2[1], precision); } } } } static const Scalar min_shape_size = Scalar(0.1); static const Scalar max_shape_size = Scalar(0.5); BOOST_AUTO_TEST_CASE(ssr_mesh_mesh) { ConvexTpl shape1 = makeRandomConvex(min_shape_size, max_shape_size); ConvexTpl shape2 = makeRandomConvex(min_shape_size, max_shape_size); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_mesh_ellipsoid) { ConvexTpl shape1 = makeRandomConvex(min_shape_size, max_shape_size); Ellipsoid shape2 = makeRandomEllipsoid(min_shape_size, max_shape_size); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_box_box) { Box shape1 = makeRandomBox(min_shape_size, max_shape_size); Box shape2 = makeRandomBox(min_shape_size, max_shape_size); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_ellipsoid_ellipsoid) { Ellipsoid shape1 = makeRandomEllipsoid(min_shape_size, max_shape_size); Ellipsoid shape2 = makeRandomEllipsoid(min_shape_size, max_shape_size); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_ellipsoid_box) { Ellipsoid shape1 = makeRandomEllipsoid(min_shape_size, max_shape_size); Box shape2 = makeRandomBox(min_shape_size, max_shape_size); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_cone_cone) { Cone shape1 = makeRandomCone({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); Cone shape2 = makeRandomCone({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_cone_ellipsoid) { Cone shape1 = makeRandomCone({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); Ellipsoid shape2 = makeRandomEllipsoid(min_shape_size, max_shape_size); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_capsule_capsule) { Capsule shape1 = makeRandomCapsule({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); Capsule shape2 = makeRandomCapsule({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_capsule_cone) { Capsule shape1 = makeRandomCapsule({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); Cone shape2 = makeRandomCone({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); test_gjksolver_swept_sphere_radius(shape1, shape2); } BOOST_AUTO_TEST_CASE(ssr_cylinder_cylinder) { Cylinder shape1 = makeRandomCylinder({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); Cylinder shape2 = makeRandomCylinder({min_shape_size / 2, min_shape_size}, {max_shape_size, max_shape_size}); test_gjksolver_swept_sphere_radius(shape1, shape2); } template void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { std::cout << "Testing collision between " << std::string(get_node_type_name(shape1.getNodeType())) << " and " << std::string(get_node_type_name(shape2.getNodeType())) << '\n'; Scalar extents[] = {-2, -2, -2, 2, 2, 2}; std::size_t n = 1; std::vector tf1s; std::vector tf2s; generateRandomTransforms(extents, tf1s, n); generateRandomTransforms(extents, tf2s, n); const std::array swept_sphere_radius = {0, Scalar(0.1), 1, 10}; for (const Scalar& ssr1 : swept_sphere_radius) { shape1.setSweptSphereRadius(ssr1); for (const Scalar& ssr2 : swept_sphere_radius) { shape2.setSweptSphereRadius(ssr2); for (std::size_t i = 0; i < n; ++i) { Transform3s tf1 = tf1s[i]; Transform3s tf2 = tf2s[i]; SET_LINE; CollisionRequest request; request.enable_contact = true; // We make sure we get witness points by setting the security margin to // infinity. That way shape1 and shape2 will always be considered in // collision. request.security_margin = std::numeric_limits::max(); const Scalar tol = Scalar(1e-6); request.gjk_tolerance = tol; request.epa_tolerance = tol; std::array result; // Without swept sphere radius const Scalar ssr1 = shape1.getSweptSphereRadius(); const Scalar ssr2 = shape2.getSweptSphereRadius(); shape1.setSweptSphereRadius(0.); shape2.setSweptSphereRadius(0.); coal::collide(&shape1, tf1, &shape2, tf2, request, result[0]); // With swept sphere radius shape1.setSweptSphereRadius(ssr1); shape2.setSweptSphereRadius(ssr2); coal::collide(&shape1, tf1, &shape2, tf2, request, result[1]); BOOST_CHECK(result[0].isCollision()); BOOST_CHECK(result[1].isCollision()); if (result[0].isCollision() && result[1].isCollision()) { std::array contact; contact[0] = result[0].getContact(0); contact[1] = result[1].getContact(0); // Precision is dependent on the swept sphere radii. // The issue of precision does not come from the default behavior of // coal, but from the result in which we manually take the swept // sphere radius into account in GJK/EPA iterations. const Scalar precision = 3 * sqrt(tol) + (1 / Scalar(100)) * std::max(ssr1, ssr2); const Scalar ssr = ssr1 + ssr2; // Check that the distance is the same COAL_CHECK_REAL_CLOSE(contact[0].penetration_depth - ssr, contact[1].penetration_depth, precision); // Check that the normal is the same COAL_CHECK_CONDITION((contact[0].normal).dot(contact[1].normal) > 0); COAL_CHECK_CONDITION( std::abs(1 - (contact[0].normal).dot(contact[1].normal)) < precision); // Check that the witness points are the same COAL_CHECK_VECTOR_CLOSE( contact[0].nearest_points[0] + ssr1 * contact[0].normal, contact[1].nearest_points[0], precision); COAL_CHECK_VECTOR_CLOSE( contact[0].nearest_points[1] - ssr2 * contact[0].normal, contact[1].nearest_points[1], precision); } } } } } const std::vector tested_geometries = { GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE}; BOOST_AUTO_TEST_CASE(ssr_geom_geom) { // Each possible geom pair is tested twice for (const NODE_TYPE& shape_type1 : tested_geometries) { for (const NODE_TYPE& shape_type2 : tested_geometries) { if (shape_type1 == GEOM_PLANE || shape_type1 == GEOM_HALFSPACE) { if (shape_type2 == GEOM_PLANE || shape_type2 == GEOM_HALFSPACE) { // TODO(louis): check plane-plane plane-halfspace etc. collisions continue; } } std::shared_ptr shape1 = makeRandomGeometry(shape_type1); std::shared_ptr shape2 = makeRandomGeometry(shape_type2); test_collide_swept_sphere_radius(*shape1, *shape2); } } } ================================================ FILE: test/utility.cpp ================================================ #include "utility.h" #include "coal/BV/BV.h" #include "coal/BVH/BVH_model.h" #include "coal/shape/geometric_shape_to_BVH_model.h" #include "coal/collision_utility.h" #include "coal/fwd.hh" #include "coal/collision.h" #include "coal/distance.h" #include #include #include #include namespace coal { void BenchTimer::start() { start_time_ = clock::now(); } double BenchTimer::getElapsedTime() { return getElapsedTimeInSec(); } double BenchTimer::getElapsedTimeInSec() { return std::chrono::duration(clock::now() - start_time_).count(); } double BenchTimer::getElapsedTimeInMilliSec() { return std::chrono::duration(clock::now() - start_time_) .count(); } double BenchTimer::getElapsedTimeInMicroSec() { return std::chrono::duration(clock::now() - start_time_) .count(); } const Eigen::IOFormat vfmt = Eigen::IOFormat( Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", ""); const Eigen::IOFormat pyfmt = Eigen::IOFormat( Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); const Vec3s UnitX = Vec3s(1, 0, 0); const Vec3s UnitY = Vec3s(0, 1, 0); const Vec3s UnitZ = Vec3s(0, 0, 1); Scalar rand_interval(Scalar rmin, Scalar rmax) { Scalar t = Scalar(rand()) / ((Scalar)RAND_MAX + 1); return (t * (rmax - rmin) + rmin); } void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles) { FILE* file = fopen(filename, "rb"); if (!file) { std::cerr << "file not exist" << std::endl; return; } bool has_normal = false; bool has_texture = false; char line_buffer[2000]; while (fgets(line_buffer, 2000, file)) { char* first_token = strtok(line_buffer, "\r\n\t "); if (!first_token || first_token[0] == '#' || first_token[0] == 0) continue; switch (first_token[0]) { case 'v': { if (first_token[1] == 'n') { strtok(NULL, "\t "); strtok(NULL, "\t "); strtok(NULL, "\t "); has_normal = true; } else if (first_token[1] == 't') { strtok(NULL, "\t "); strtok(NULL, "\t "); has_texture = true; } else { Scalar x = (Scalar)atof(strtok(NULL, "\t ")); Scalar y = (Scalar)atof(strtok(NULL, "\t ")); Scalar z = (Scalar)atof(strtok(NULL, "\t ")); Vec3s p(x, y, z); points.push_back(p); } } break; case 'f': { Triangle32 tri; char* data[30]; int n = 0; while ((data[n] = strtok(NULL, "\t \r\n")) != NULL) { if (strlen(data[n])) n++; } for (int t = 0; t < (n - 2); ++t) { if ((!has_texture) && (!has_normal)) { tri[0] = (Triangle32::IndexType)(atoi(data[0]) - 1); tri[1] = (Triangle32::IndexType)(atoi(data[1]) - 1); tri[2] = (Triangle32::IndexType)(atoi(data[2]) - 1); } else { const char* v1; for (Triangle32::IndexType i = 0; i < 3; i++) { // vertex ID if (i == 0) v1 = data[0]; else v1 = data[(Triangle32::IndexType)t + i]; tri[i] = (Triangle32::IndexType)(atoi(v1) - 1); } } triangles.push_back(tri); } } } } } void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles) { std::ofstream os(filename); if (!os) { std::cerr << "file not exist" << std::endl; return; } for (std::size_t i = 0; i < points.size(); ++i) { os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2] << std::endl; } for (std::size_t i = 0; i < triangles.size(); ++i) { os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " << triangles[i][2] + 1 << std::endl; } os.close(); } #ifdef COAL_HAS_OCTOMAP OcTree loadOctreeFile(const std::string& filename, const Scalar& resolution) { octomap::OcTreePtr_t octree(new octomap::OcTree(filename)); if (octree->getResolution() != resolution) { std::ostringstream oss; oss << "Resolution of the OcTree is " << octree->getResolution() << " and not " << resolution; throw std::invalid_argument(oss.str()); } return coal::OcTree(octree); } #endif void eulerToMatrix(Scalar a, Scalar b, Scalar c, Matrix3s& R) { Scalar c1 = cos(a); Scalar c2 = cos(b); Scalar c3 = cos(c); Scalar s1 = sin(a); Scalar s2 = sin(b); Scalar s3 = sin(c); R << c1 * c2, -c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, -c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3; } void generateRandomTransform(Scalar extents[6], Transform3s& transform) { Scalar x = rand_interval(extents[0], extents[3]); Scalar y = rand_interval(extents[1], extents[4]); Scalar z = rand_interval(extents[2], extents[5]); const Scalar pi = Scalar(3.1415926); Scalar a = rand_interval(0, 2 * pi); Scalar b = rand_interval(0, 2 * pi); Scalar c = rand_interval(0, 2 * pi); Matrix3s R; eulerToMatrix(a, b, c, R); Vec3s T(x, y, z); transform.setTransform(R, T); } void generateRandomTransforms(Scalar extents[6], std::vector& transforms, std::size_t n) { transforms.resize(n); for (std::size_t i = 0; i < n; ++i) { Scalar x = rand_interval(extents[0], extents[3]); Scalar y = rand_interval(extents[1], extents[4]); Scalar z = rand_interval(extents[2], extents[5]); const Scalar pi = Scalar(3.1415926); Scalar a = rand_interval(0, 2 * pi); Scalar b = rand_interval(0, 2 * pi); Scalar c = rand_interval(0, 2 * pi); { Matrix3s R; eulerToMatrix(a, b, c, R); Vec3s T(x, y, z); transforms[i].setTransform(R, T); } } } void generateRandomTransforms(Scalar extents[6], Scalar delta_trans[3], Scalar delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n) { transforms.resize(n); transforms2.resize(n); for (std::size_t i = 0; i < n; ++i) { Scalar x = rand_interval(extents[0], extents[3]); Scalar y = rand_interval(extents[1], extents[4]); Scalar z = rand_interval(extents[2], extents[5]); const Scalar pi = Scalar(3.1415926); Scalar a = rand_interval(0, 2 * pi); Scalar b = rand_interval(0, 2 * pi); Scalar c = rand_interval(0, 2 * pi); { Matrix3s R; eulerToMatrix(a, b, c, R); Vec3s T(x, y, z); transforms[i].setTransform(R, T); } Scalar deltax = rand_interval(-delta_trans[0], delta_trans[0]); Scalar deltay = rand_interval(-delta_trans[1], delta_trans[1]); Scalar deltaz = rand_interval(-delta_trans[2], delta_trans[2]); Scalar deltaa = rand_interval(-delta_rot, delta_rot); Scalar deltab = rand_interval(-delta_rot, delta_rot); Scalar deltac = rand_interval(-delta_rot, delta_rot); { Matrix3s R; eulerToMatrix(a + deltaa, b + deltab, c + deltac, R); Vec3s T(x + deltax, y + deltay, z + deltaz); transforms2[i].setTransform(R, T); } } } bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_) { CollisionData* cdata = static_cast(cdata_); const CollisionRequest& request = cdata->request; CollisionResult& result = cdata->result; if (cdata->done) return true; collide(o1, o2, request, result); if ((result.isCollision()) && (result.numContacts() >= request.num_max_contacts)) cdata->done = true; return cdata->done; } bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, Scalar& dist) { DistanceData* cdata = static_cast(cdata_); const DistanceRequest& request = cdata->request; DistanceResult& result = cdata->result; if (cdata->done) { dist = result.min_distance; return true; } distance(o1, o2, request, result); dist = result.min_distance; if (dist <= 0) return true; // in collision or in touch return cdata->done; } std::string getNodeTypeName(NODE_TYPE node_type) { if (node_type == BV_UNKNOWN) return std::string("BV_UNKNOWN"); else if (node_type == BV_AABB) return std::string("BV_AABB"); else if (node_type == BV_OBB) return std::string("BV_OBB"); else if (node_type == BV_RSS) return std::string("BV_RSS"); else if (node_type == BV_kIOS) return std::string("BV_kIOS"); else if (node_type == BV_OBBRSS) return std::string("BV_OBBRSS"); else if (node_type == BV_KDOP16) return std::string("BV_KDOP16"); else if (node_type == BV_KDOP18) return std::string("BV_KDOP18"); else if (node_type == BV_KDOP24) return std::string("BV_KDOP24"); else if (node_type == GEOM_BOX) return std::string("GEOM_BOX"); else if (node_type == GEOM_SPHERE) return std::string("GEOM_SPHERE"); else if (node_type == GEOM_CAPSULE) return std::string("GEOM_CAPSULE"); else if (node_type == GEOM_CONE) return std::string("GEOM_CONE"); else if (node_type == GEOM_CYLINDER) return std::string("GEOM_CYLINDER"); else if (node_type == GEOM_CONVEX) return std::string("GEOM_CONVEX"); else if (node_type == GEOM_PLANE) return std::string("GEOM_PLANE"); else if (node_type == GEOM_HALFSPACE) return std::string("GEOM_HALFSPACE"); else if (node_type == GEOM_TRIANGLE) return std::string("GEOM_TRIANGLE"); else if (node_type == GEOM_OCTREE) return std::string("GEOM_OCTREE"); else return std::string("invalid"); } Quats makeQuat(Scalar w, Scalar x, Scalar y, Scalar z) { Quats q; q.w() = w; q.x() = x; q.y() = y; q.z() = z; return q; } std::size_t getNbRun(const int& argc, char const* const* argv, std::size_t defaultValue) { for (int i = 0; i < argc; ++i) if (strcmp(argv[i], "--nb-run") == 0) if (i + 1 != argc) return (std::size_t)strtol(argv[i + 1], NULL, 10); return defaultValue; } void generateEnvironments(std::vector& env, Scalar env_scale, std::size_t n) { Scalar extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; std::vector transforms(n); generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < n; ++i) { Box* box = new Box(5, 10, 20); env.push_back( new CollisionObject(shared_ptr(box), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); } generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < n; ++i) { Sphere* sphere = new Sphere(30); env.push_back(new CollisionObject(shared_ptr(sphere), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); } generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < n; ++i) { Cylinder* cylinder = new Cylinder(10, 40); env.push_back(new CollisionObject(shared_ptr(cylinder), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); } } void generateEnvironmentsMesh(std::vector& env, Scalar env_scale, std::size_t n) { Scalar extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Box box(5, 10, 20); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3s::Identity()); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); } generateRandomTransforms(extents, transforms, n); Sphere sphere(30); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3s::Identity(), 16, 16); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); } generateRandomTransforms(extents, transforms, n); Cylinder cylinder(10, 40); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3s::Identity(), 16, 16); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); } } ConvexTpl buildBox(Scalar l, Scalar w, Scalar d) { std::shared_ptr> pts(new std::vector( {Vec3s(l, w, d), Vec3s(l, w, -d), Vec3s(l, -w, d), Vec3s(l, -w, -d), Vec3s(-l, w, d), Vec3s(-l, w, -d), Vec3s(-l, -w, d), Vec3s(-l, -w, -d)})); std::shared_ptr> polygons( new std::vector(6)); (*polygons)[0].set(0, 2, 3, 1); // x+ side (*polygons)[1].set(2, 6, 7, 3); // y- side (*polygons)[2].set(4, 5, 7, 6); // x- side (*polygons)[3].set(0, 1, 5, 4); // y+ side (*polygons)[4].set(1, 3, 7, 5); // z- side (*polygons)[5].set(0, 2, 6, 4); // z+ side return ConvexTpl(pts, // points 8, // num points polygons, 6 // number of polygons ); } /// Takes a point and projects it onto the surface of the unit sphere void toSphere(Vec3s& point) { assert(point.norm() > 1e-8); point /= point.norm(); } /// Takes a point, projects it on the unit sphere and applies an ellipsoid /// transformation to it. A point x belongs to the surface of an ellipsoid if /// x^T * A * x = 1, where A = diag(1/r**2) with r being the radii of the /// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y * /// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) = /// diag(r). void toEllipsoid(Vec3s& point, const Ellipsoid& ellipsoid) { toSphere(point); point[0] *= ellipsoid.radii[0]; point[1] *= ellipsoid.radii[1]; point[2] *= ellipsoid.radii[2]; } ConvexTpl constructPolytopeFromEllipsoid( const Ellipsoid& ellipsoid) { Scalar PHI = (1 + std::sqrt(Scalar(5))) / 2; // vertices std::shared_ptr> pts(new std::vector({ Vec3s(-1, PHI, 0), Vec3s(1, PHI, 0), Vec3s(-1, -PHI, 0), Vec3s(1, -PHI, 0), Vec3s(0, -1, PHI), Vec3s(0, 1, PHI), Vec3s(0, -1, -PHI), Vec3s(0, 1, -PHI), Vec3s(PHI, 0, -1), Vec3s(PHI, 0, 1), Vec3s(-PHI, 0, -1), Vec3s(-PHI, 0, 1), })); std::vector& pts_ = *pts; for (size_t i = 0; i < 12; ++i) { toEllipsoid(pts_[i], ellipsoid); } // faces std::shared_ptr> tris( new std::vector(20)); (*tris)[0].set(0, 11, 5); (*tris)[1].set(0, 5, 1); (*tris)[2].set(0, 1, 7); (*tris)[3].set(0, 7, 10); (*tris)[4].set(0, 10, 11); (*tris)[5].set(1, 5, 9); (*tris)[6].set(5, 11, 4); (*tris)[7].set(11, 10, 2); (*tris)[8].set(10, 7, 6); (*tris)[9].set(7, 1, 8); (*tris)[10].set(3, 9, 4); (*tris)[11].set(3, 4, 2); (*tris)[12].set(3, 2, 6); (*tris)[13].set(3, 6, 8); (*tris)[14].set(3, 8, 9); (*tris)[15].set(4, 9, 5); (*tris)[16].set(2, 4, 11); (*tris)[17].set(6, 2, 10); (*tris)[18].set(8, 6, 7); (*tris)[19].set(9, 8, 1); return ConvexTpl(pts, // points 12, // num_points tris, // triangles 20 // number of triangles ); } Box makeRandomBox(Scalar min_size, Scalar max_size) { return Box(Vec3s(rand_interval(min_size, max_size), rand_interval(min_size, max_size), rand_interval(min_size, max_size))); } Sphere makeRandomSphere(Scalar min_size, Scalar max_size) { return Sphere(rand_interval(min_size, max_size)); } Ellipsoid makeRandomEllipsoid(Scalar min_size, Scalar max_size) { return Ellipsoid(Vec3s(rand_interval(min_size, max_size), rand_interval(min_size, max_size), rand_interval(min_size, max_size))); } Capsule makeRandomCapsule(std::array min_size, std::array max_size) { return Capsule(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } Cone makeRandomCone(std::array min_size, std::array max_size) { return Cone(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } Cylinder makeRandomCylinder(std::array min_size, std::array max_size) { return Cylinder(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } ConvexTpl makeRandomConvex(Scalar min_size, Scalar max_size) { Ellipsoid ellipsoid = makeRandomEllipsoid(min_size, max_size); return constructPolytopeFromEllipsoid(ellipsoid); } Plane makeRandomPlane(Scalar min_size, Scalar max_size) { return Plane(Vec3s::Random().normalized(), rand_interval(min_size, max_size)); } Halfspace makeRandomHalfspace(Scalar min_size, Scalar max_size) { return Halfspace(Vec3s::Random().normalized(), rand_interval(min_size, max_size)); } std::shared_ptr makeRandomGeometry(NODE_TYPE node_type) { switch (node_type) { case GEOM_TRIANGLE: COAL_THROW_PRETTY(std::string(COAL_PRETTY_FUNCTION) + " for " + std::string(get_node_type_name(node_type)) + " is not yet implemented.", std::invalid_argument); break; case GEOM_BOX: return std::make_shared(makeRandomBox(Scalar(0.1), Scalar(1))); break; case GEOM_SPHERE: return std::make_shared(makeRandomSphere(Scalar(0.1), Scalar(1))); break; case GEOM_ELLIPSOID: return std::make_shared( makeRandomEllipsoid(Scalar(0.1), Scalar(1))); break; case GEOM_CAPSULE: return std::make_shared(makeRandomCapsule( {Scalar(0.1), Scalar(0.2)}, {Scalar(0.8), Scalar(1)})); break; case GEOM_CONE: return std::make_shared(makeRandomCone({Scalar(0.1), Scalar(0.2)}, {Scalar(0.8), Scalar(1.0)})); break; case GEOM_CYLINDER: return std::make_shared(makeRandomCylinder( {Scalar(0.1), Scalar(0.2)}, {Scalar(0.8), Scalar(1.0)})); break; case GEOM_CONVEX: return std::make_shared>( makeRandomConvex(Scalar(0.1), Scalar(1))); break; case GEOM_PLANE: return std::make_shared(makeRandomPlane(Scalar(0.1), Scalar(1))); break; case GEOM_HALFSPACE: return std::make_shared( makeRandomHalfspace(Scalar(0.1), Scalar(1))); break; default: COAL_THROW_PRETTY(std::string(get_node_type_name(node_type)) + " is not a primitive shape.", std::invalid_argument); break; } } } // namespace coal ================================================ FILE: test/utility.h ================================================ /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Jia Pan */ #ifndef TEST_COAL_UTILITY_H #define TEST_COAL_UTILITY_H #include "coal/math/transform.h" #include "coal/collision_data.h" #include "coal/collision_object.h" #include "coal/broadphase/default_broadphase_callbacks.h" #include "coal/shape/convex.h" #include #ifdef COAL_HAS_OCTOMAP #include "coal/octree.h" #endif #define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \ "check " #Va ".isApprox(" #Vb ") failed at precision " \ << precision << " [\n" \ << (Va).transpose() << "\n!=\n" \ << (Vb).transpose() << "\n]") #define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \ "check " #Va ".isApprox(" #Vb ") failed at precision " \ << precision << " [\n" \ << (Va) << "\n!=\n" \ << (Vb) << "\n]") #define Scalar_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE(std::abs((Va) - (Vb)) < precision, \ "check " #Va ".isApprox(" #Vb ") failed at precision " \ << precision << " [\n" \ << Va << "\n!=\n" \ << Vb << "\n]") namespace octomap { #ifdef COAL_HAS_OCTOMAP typedef coal::shared_ptr OcTreePtr_t; #endif } // namespace octomap namespace coal { class BenchTimer { using clock = std::chrono::steady_clock; using time_point = clock::time_point; public: void start(); ///< start timer double getElapsedTime(); ///< get elapsed time in milli-second double getElapsedTimeInSec(); ///< get elapsed time in second (same as ///< getElapsedTime) double getElapsedTimeInMilliSec(); ///< get elapsed time in milli-second double getElapsedTimeInMicroSec(); ///< get elapsed time in micro-second private: time_point start_time_ = clock::now(); }; struct TStruct { std::vector records; double overall_time; TStruct() { overall_time = 0; } void push_back(double t) { records.push_back(t); overall_time += t; } }; extern const Eigen::IOFormat vfmt; extern const Eigen::IOFormat pyfmt; typedef Eigen::AngleAxis AngleAxis; extern const Vec3s UnitX; extern const Vec3s UnitY; extern const Vec3s UnitZ; /// @brief Load an obj mesh file void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles); void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles); #ifdef COAL_HAS_OCTOMAP coal::OcTree loadOctreeFile(const std::string& filename, const Scalar& resolution); #endif /// @brief Generate one random transform whose translation is constrained by /// extents and rotation without constraints. The translation is (x, y, z), and /// extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= /// z <= extents[5] void generateRandomTransform(Scalar extents[6], Transform3s& transform); /// @brief Generate n random transforms whose translations are constrained by /// extents. void generateRandomTransforms(Scalar extents[6], std::vector& transforms, std::size_t n); /// @brief Generate n random transforms whose translations are constrained by /// extents. Also generate another transforms2 which have additional random /// translation & rotation to the transforms generated. void generateRandomTransforms(Scalar extents[6], Scalar delta_trans[3], Scalar delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n); /// @ brief Structure for minimum distance between two meshes and the /// corresponding nearest point pair struct DistanceRes { Scalar distance; Vec3s p1; Vec3s p2; }; /// @brief Default collision callback for two objects o1 and o2 in broad phase. /// return value means whether the broad phase can stop now. bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata); /// @brief Default distance callback for two objects o1 and o2 in broad phase. /// return value means whether the broad phase can stop now. also return dist, /// i.e. the bmin distance till now bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, Scalar& dist); std::string getNodeTypeName(NODE_TYPE node_type); Quats makeQuat(Scalar w, Scalar x, Scalar y, Scalar z); /// Get the argument --nb-run from argv std::size_t getNbRun(const int& argc, char const* const* argv, std::size_t defaultValue); void generateEnvironments(std::vector& env, Scalar env_scale, std::size_t n); void generateEnvironmentsMesh(std::vector& env, Scalar env_scale, std::size_t n); /// @brief Constructs a box with halfsides (l, w, d), centered around 0. /// The box is 2*l wide on the x-axis, 2*w wide on the y-axis and 2*d wide on /// the z-axis. ConvexTpl buildBox(Scalar l, Scalar w, Scalar d); /// @brief We give an ellipsoid as input. The output is a 20 faces polytope /// which vertices belong to the original ellipsoid surface. The procedure is /// simple: we construct a icosahedron, see /// https://sinestesia.co/blog/tutorials/python-icospheres/ . We then apply an /// ellipsoid tranformation to each vertex of the icosahedron. ConvexTpl constructPolytopeFromEllipsoid( const Ellipsoid& ellipsoid); Box makeRandomBox(Scalar min_size, Scalar max_size); Sphere makeRandomSphere(Scalar min_size, Scalar max_size); Ellipsoid makeRandomEllipsoid(Scalar min_size, Scalar max_size); Capsule makeRandomCapsule(std::array min_size, std::array max_size); Cone makeRandomCone(std::array min_size, std::array max_size); Cylinder makeRandomCylinder(std::array min_size, std::array max_size); ConvexTpl makeRandomConvex(Scalar min_size, Scalar max_size); Plane makeRandomPlane(Scalar min_size, Scalar max_size); Halfspace makeRandomHalfspace(Scalar min_size, Scalar max_size); std::shared_ptr makeRandomGeometry(NODE_TYPE node_type); } // namespace coal #endif