diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs
index 5463ef857beed42f718ba1800eccc0d85bf9805c..2785904a0cb1fecac206637a1e7991ae0dafb616 100644
--- a/.git-blame-ignore-revs
+++ b/.git-blame-ignore-revs
@@ -1,5 +1,9 @@
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)
diff --git a/.github/workflows/conda/environment_macos_linux.yml b/.github/workflows/conda/environment_macos_linux.yml
new file mode 100644
index 0000000000000000000000000000000000000000..4a4655fbd32b392a40da531f39635545eb50a344
--- /dev/null
+++ b/.github/workflows/conda/environment_macos_linux.yml
@@ -0,0 +1,22 @@
+name: fcl
+channels:
+ - conda-forge
+ - nodefaults
+dependencies:
+ - eigen
+ - octomap
+ - assimp
+ - numpy
+ - boost
+ - eigenpy
+ - python
+ - doxygen
+ - lxml
+ - pylatexenc
+ - qhull
+ - cmake
+ - ccache
+ - cxx-compiler
+ - llvm-openmp
+ - pkg-config
+ - ninja
diff --git a/.github/workflows/conda/conda-env.yml b/.github/workflows/conda/environment_windows.yml
similarity index 80%
rename from .github/workflows/conda/conda-env.yml
rename to .github/workflows/conda/environment_windows.yml
index 54d08f51413b0cbf873f73e674554e1fdc371e91..8c6fccaede6be219700d402eb53df1d3cb08adea 100644
--- a/.github/workflows/conda/conda-env.yml
+++ b/.github/workflows/conda/environment_windows.yml
@@ -14,3 +14,7 @@ dependencies:
- lxml
- pylatexenc
- qhull
+ - cmake
+ - ccache
+ - pkg-config
+ - ninja
diff --git a/.github/workflows/macos-linux-conda.yml b/.github/workflows/macos-linux-conda.yml
index 9a83ac9a3470f2ce633803f522d2bdeb5f606891..cad761ce84754c96233de2d68b9b6c32f4d28c3a 100644
--- a/.github/workflows/macos-linux-conda.yml
+++ b/.github/workflows/macos-linux-conda.yml
@@ -4,63 +4,75 @@ on: [push,pull_request]
jobs:
hpp-fcl-conda:
- name: CI on ${{ matrix.os }} with Conda
+ name: CI on ${{ matrix.os }} with Conda Python ${{ matrix.python-version }} - ${{ 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
strategy:
fail-fast: false
matrix:
os: ["ubuntu-latest", "macos-latest"]
python-version: ["3.8", "3.12"]
+ cxx_options: ['', '-mavx2']
+ build_type: [Release, Debug]
+ exclude:
+ - build_type: Debug
+ cxx_options: -mavx2
+ os: macos-latest
+ - build_type: Release
+ cxx_options: -mavx2
+ os: macos-latest
steps:
- - uses: actions/checkout@v3
+ - uses: actions/checkout@v4
with:
submodules: recursive
- - uses: conda-incubator/setup-miniconda@v2
+ - uses: actions/cache@v3
with:
- activate-environment: hpp-fcl
+ path: .ccache
+ key: ccache-macos-linux-conda-${{ matrix.os }}-${{ matrix.build_type }}-${{ matrix.cxx_options }}-${{ matrix.python-version }}-${{ github.sha }}
+ restore-keys: ccache-macos-linux-conda-${{ matrix.os }}-${{ matrix.build_type }}-${{ matrix.cxx_options }}-${{ matrix.python-version }}-
+
+ - uses: conda-incubator/setup-miniconda@v3
+ with:
+ activate-environment: fcl
auto-update-conda: true
- environment-file: .github/workflows/conda/conda-env.yml
+ environment-file: .github/workflows/conda/environment_macos_linux.yml
python-version: ${{ matrix.python-version }}
-
- - name: Install compilers on OSX
- if: contains(matrix.os, 'macos')
- shell: bash -l {0}
- run: |
- conda activate hpp-fcl
- conda install compilers=1.4.2 -c conda-forge
-
- - name: Install cmake and update conda
- shell: bash -l {0}
- run: |
- conda activate hpp-fcl
- conda install cmake -c main
- conda install llvm-openmp -c conda-forge
+ auto-activate-base: false
- name: Build hpp-fcl
- shell: bash -l {0}
+ shell: bash -el {0}
run: |
- conda activate hpp-fcl
conda list
echo $CONDA_PREFIX
mkdir build
cd build
- cmake .. -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX -DCMAKE_BUILD_TYPE=Release -DPYTHON_EXECUTABLE=$(which python3) -DGENERATE_PYTHON_STUBS=ON -DHPP_FCL_HAS_QHULL=ON
- make -j2
- make build_tests
- export CTEST_OUTPUT_ON_FAILURE=1
- make test
- make install
+ cmake .. \
+ -G "Ninja" \
+ -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \
+ -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX \
+ -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+ -DCMAKE_CXX_FLAGS=${{ matrix.cxx_options }} \
+ -DPYTHON_EXECUTABLE=$(which python3) \
+ -DGENERATE_PYTHON_STUBS=ON \
+ -DHPP_FCL_HAS_QHULL=ON
+ cmake --build . -j2
+ ctest --output-on-failure
+ cmake --install .
- name: Uninstall hpp-fcl
- shell: bash -l {0}
+ shell: bash -el {0}
run: |
cd build
- make uninstall
+ cmake --build . --target uninstall
check:
if: always()
diff --git a/.github/workflows/windows-conda-clang.yml b/.github/workflows/windows-conda-clang.yml
index 22c27a61f4a82111a633a553c23462c38fbde577..33fc913ca4889d6ff0379910054ed584a11a3e0c 100644
--- a/.github/workflows/windows-conda-clang.yml
+++ b/.github/workflows/windows-conda-clang.yml
@@ -4,6 +4,12 @@ on: [push,pull_request]
jobs:
build:
runs-on: ${{ matrix.os }}
+ env:
+ CCACHE_BASEDIR: "${GITHUB_WORKSPACE}"
+ CCACHE_DIR: "${GITHUB_WORKSPACE}/.ccache"
+ CCACHE_COMPRESS: true
+ CCACHE_COMPRESSLEVEL: 6
+
strategy:
fail-fast: false
matrix:
@@ -15,42 +21,40 @@ jobs:
compiler: clang-cl
steps:
- - uses: actions/checkout@v3
+ - uses: actions/checkout@v4
with:
submodules: recursive
- - uses: conda-incubator/setup-miniconda@v2
- env:
- ACTIONS_ALLOW_UNSECURE_COMMANDS: 'true'
+
+ - uses: actions/cache@v3
+ with:
+ path: .ccache
+ key: ccache-windows-conda-clang-${{ matrix.os }}-${{ matrix.compiler }}-${{ github.sha }}
+ restore-keys: ccache-windows-conda-clang-${{ matrix.os }}-${{ matrix.compiler }}-
+
+ - uses: conda-incubator/setup-miniconda@v3
with:
activate-environment: fcl
- environment-file: .github/workflows/conda/conda-env.yml
+ auto-update-conda: true
+ environment-file: .github/workflows/conda/environment_windows.yml
python-version: "3.10"
- - name: Install cmake and update conda
- run: |
- conda install cmake -c main
+ auto-activate-base: false
- name: Build FCL
shell: cmd /C CALL {0}
- env:
- ACTIONS_ALLOW_UNSECURE_COMMANDS: 'true'
run: |
- :: unset extra Boost envs
- set Boost_ROOT=
- set BOOST_ROOT_1_69_0=
- set BOOST_ROOT_1_72_0=
- set PATH=%PATH:C:\hostedtoolcache\windows\Boost\1.72.0;=%
-
call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64
+ :: Tell Ninja to use clang-cl
+ set CC=clang-cl
+ set CXX=clang-cl
:: Create build
mkdir build
pushd build
- :: Configure
- set PKG_CONFIG_PATH=%CONDA_PREFIX%\Library\share\pkgconfig:%CONDA_PREFIX%\Library\share\pkgconfig
cmake ^
- -G "Visual Studio 16 2019" -T "ClangCl" -DCMAKE_GENERATOR_PLATFORM=x64 ^
+ -G "Ninja" ^
-DCMAKE_INSTALL_PREFIX=%CONDA_PREFIX%\Library ^
+ -DCMAKE_CXX_COMPILER_LAUNCHER=ccache ^
-DCMAKE_BUILD_TYPE=Release ^
-DGENERATE_PYTHON_STUBS=ON ^
-DPYTHON_SITELIB=%CONDA_PREFIX%\Lib\site-packages ^
@@ -65,7 +69,7 @@ jobs:
if errorlevel 1 exit 1
:: Testing
- ctest --output-on-failure -C Release -V
+ ctest --output-on-failure -C Release
if errorlevel 1 exit 1
:: Test Python import
diff --git a/.github/workflows/windows-conda-v142.yml b/.github/workflows/windows-conda-v142.yml
index 6b183034fab908930b690fe2a64a0ea14cda128f..e09444ba3bcec0831fc33f9626957bd95c90a1cc 100644
--- a/.github/workflows/windows-conda-v142.yml
+++ b/.github/workflows/windows-conda-v142.yml
@@ -4,6 +4,12 @@ on: [push,pull_request]
jobs:
build:
runs-on: ${{ matrix.os }}
+ env:
+ CCACHE_BASEDIR: "${GITHUB_WORKSPACE}"
+ CCACHE_DIR: "${GITHUB_WORKSPACE}/.ccache"
+ CCACHE_COMPRESS: true
+ CCACHE_COMPRESSLEVEL: 6
+
strategy:
fail-fast: false
matrix:
@@ -14,31 +20,27 @@ jobs:
os: windows-2019
steps:
- - uses: actions/checkout@v3
+ - uses: actions/checkout@v4
with:
submodules: recursive
- - uses: conda-incubator/setup-miniconda@v2
- env:
- ACTIONS_ALLOW_UNSECURE_COMMANDS: 'true'
+
+ - uses: actions/cache@v3
+ with:
+ path: .ccache
+ key: ccache-windows-conda-vs-${{ matrix.os }}-${{ matrix.compiler }}-${{ github.sha }}
+ restore-keys: ccache-windows-conda-vs-${{ matrix.os }}-${{ matrix.compiler }}-
+
+ - uses: conda-incubator/setup-miniconda@v3
with:
activate-environment: fcl
- environment-file: .github/workflows/conda/conda-env.yml
+ auto-update-conda: true
+ environment-file: .github/workflows/conda/environment_windows.yml
python-version: "3.10"
- - name: Install cmake and update conda
- run: |
- conda install cmake -c main
+ auto-activate-base: false
- name: Build FCL
shell: cmd /C CALL {0}
- env:
- ACTIONS_ALLOW_UNSECURE_COMMANDS: 'true'
run: |
- :: unset extra Boost envs
- set Boost_ROOT=
- set BOOST_ROOT_1_69_0=
- set BOOST_ROOT_1_72_0=
- set PATH=%PATH:C:\hostedtoolcache\windows\Boost\1.72.0;=%
-
call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64
:: Create build
@@ -46,10 +48,10 @@ jobs:
pushd build
:: Configure
- set PKG_CONFIG_PATH=%CONDA_PREFIX%\Library\share\pkgconfig:%CONDA_PREFIX%\Library\share\pkgconfig
cmake ^
- -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 ^
+ -G "Ninja" ^
-DCMAKE_INSTALL_PREFIX=%CONDA_PREFIX%\Library ^
+ -DCMAKE_CXX_COMPILER_LAUNCHER=ccache ^
-DCMAKE_BUILD_TYPE=Release ^
-DGENERATE_PYTHON_STUBS=ON ^
-DPYTHON_SITELIB=%CONDA_PREFIX%\Lib\site-packages ^
@@ -64,7 +66,7 @@ jobs:
if errorlevel 1 exit 1
:: Testing
- ctest --output-on-failure -C Release -V
+ ctest --output-on-failure -C Release
if errorlevel 1 exit 1
:: Test Python import
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1d8957a80edf4031b519b56e6cdeb3b35f5a2dd2..e489ce9c349f1229344e27c425bafffcb0ab3e54 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -107,7 +107,7 @@ CMAKE_DEPENDENT_OPTION(GENERATE_PYTHON_STUBS "Generate the Python stubs associat
ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.0")
if(BUILD_PYTHON_INTERFACE)
- FIND_PACKAGE(eigenpy 2.7.10 REQUIRED)
+ FIND_PACKAGE(eigenpy 2.9.2 REQUIRED)
endif()
# Required dependencies
@@ -243,7 +243,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/BVH/BVH_utility.h
include/hpp/fcl/collision_object.h
include/hpp/fcl/collision_utility.h
- include/hpp/fcl/octree.h
include/hpp/fcl/hfield.h
include/hpp/fcl/fwd.hh
include/hpp/fcl/mesh_loader/assimp.h
@@ -257,7 +256,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/internal/traversal_node_bvh_shape.h
include/hpp/fcl/internal/traversal_node_bvhs.h
include/hpp/fcl/internal/traversal_node_hfield_shape.h
- include/hpp/fcl/internal/traversal_node_octree.h
include/hpp/fcl/internal/traversal_node_setup.h
include/hpp/fcl/internal/traversal_node_shapes.h
include/hpp/fcl/internal/traversal_recurse.h
@@ -282,6 +280,14 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/timings.h
)
+IF(HPP_FCL_HAS_OCTOMAP)
+ LIST(APPEND ${PROJECT_NAME}_HEADERS
+ include/hpp/fcl/octree.h
+ include/hpp/fcl/serialization/octree.h
+ include/hpp/fcl/internal/traversal_node_octree.h
+ )
+ENDIF(HPP_FCL_HAS_OCTOMAP)
+
add_subdirectory(doc)
add_subdirectory(src)
if (BUILD_PYTHON_INTERFACE)
diff --git a/LICENSE b/LICENSE
index 529a238fa178840a0341d29281d0630b9d558838..c0a96bba948f1a4bddadb778c6a636bfcd204e25 100644
--- a/LICENSE
+++ b/LICENSE
@@ -2,6 +2,8 @@ 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-2023, INRIA
All rights reserved.
Redistribution and use in source and binary forms, with or without
diff --git a/README.md b/README.md
index f1bde0d7fc551d20fa2a370c98bc0c05d77d5b96..00f7e3694e1781fdd6baf0f6e761a48456c5ce74 100644
--- a/README.md
+++ b/README.md
@@ -50,6 +50,149 @@ On the other hand, why do we care about dedicated collision detection solvers li
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.
+## C++ example
+Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install hpp-fcl`.
+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 HPP-FCL in C++:
+```cpp
+#include "hpp/fcl/math/transform.h"
+#include "hpp/fcl/mesh_loader/loader.h"
+#include "hpp/fcl/BVH/BVH_model.h"
+#include "hpp/fcl/collision.h"
+#include "hpp/fcl/collision_data.h"
+#include <iostream>
+#include <memory>
+
+// 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<hpp::fcl::ConvexBase> loadConvexMesh(const std::string& file_name) {
+ hpp::fcl::NODE_TYPE bv_type = hpp::fcl::BV_AABB;
+ hpp::fcl::MeshLoader loader(bv_type);
+ hpp::fcl::BVHModelPtr_t bvh = loader.load(file_name);
+ bvh->buildConvexHull(true, "Qt");
+ return bvh->convex;
+}
+
+int main() {
+ // Create the hppfcl shapes.
+ // Hppfcl 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<hpp::fcl::Ellipsoid> shape1 = std::make_shared<hpp::fcl::Ellipsoid>(0.7, 1.0, 0.8);
+ std::shared_ptr<hpp::fcl::ConvexBase> shape2 = loadConvexMesh("../path/to/mesh/file.obj");
+
+ // Define the shapes' placement in 3D space
+ hpp::fcl::Transform3f T1;
+ T1.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom());
+ T1.setTranslation(hpp::fcl::Vec3f::Random());
+ hpp::fcl::Transform3f T2 = hpp::fcl::Transform3f::Identity();
+ T2.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom());
+ T2.setTranslation(hpp::fcl::Vec3f::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.
+ hpp::fcl::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.)
+ hpp::fcl::CollisionResult col_res;
+
+ // Collision call
+ hpp::fcl::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()) {
+ hpp::fcl::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 HPP-FCL's python bindings:
+```python
+import numpy as np
+import hppfcl
+# 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 hpp-fcl.
+import pinocchio as pin
+
+def loadConvexMesh(file_name: str):
+ loader = hppfcl.MeshLoader()
+ bvh: hppfcl.BVHModelBase = loader.load(file_name)
+ bvh.buildConvexHull(True, "Qt")
+ return bvh.convex
+
+if __name__ == "__main__":
+ # Create hppfcl shapes
+ shape1 = hppfcl.Ellipsoid(0.7, 1.0, 0.8)
+ shape2 = loadConvexMesh("../path/to/mesh/file.obj")
+
+ # Define the shapes' placement in 3D space
+ T1 = hppfcl.Transform3f()
+ T1.setTranslation(pin.SE3.Random().translation)
+ T1.setRotation(pin.SE3.Random().rotation)
+ T2 = hppfcl.Transform3f();
+ # Using np arrays also works
+ T1.setTranslation(np.random.rand(3))
+ T2.setRotation(pin.SE3.Random().rotation)
+
+ # Define collision requests and results
+ col_req = hppfcl.CollisionRequest()
+ col_res = hppfcl.CollisionResult()
+
+ # Collision call
+ hppfcl.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: hppfcl.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 **HPP-FCL** 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 extend, [Eureka Robotics](https://eurekarobotics.com/).
diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h
index 8ba57ea26823555a0942c9c88050019ca19aea96..300b24ecc87ac55f48a4b4c335f51e4948e1a0a8 100644
--- a/include/hpp/fcl/BV/BV_node.h
+++ b/include/hpp/fcl/BV/BV_node.h
@@ -39,9 +39,7 @@
#define HPP_FCL_BV_NODE_H
#include <hpp/fcl/data_types.h>
-
#include <hpp/fcl/BV/BV.h>
-#include <iostream>
namespace hpp {
namespace fcl {
diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h
index 9d765a7126b97a647bf4ec0a2d967eef38ffd151..abbf3b6917aae8f074d5d9ca3fe865d96a351bda 100644
--- a/include/hpp/fcl/BV/OBB.h
+++ b/include/hpp/fcl/BV/OBB.h
@@ -50,6 +50,8 @@ struct CollisionRequest;
/// @brief Oriented bounding box class
struct HPP_FCL_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,
diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h
index e5fb8a7f48106aa4d0b206aa810e1d9b720e4fc5..fc45d318ec670da3c23ded58defdade992a642c1 100644
--- a/include/hpp/fcl/BV/OBBRSS.h
+++ b/include/hpp/fcl/BV/OBBRSS.h
@@ -52,6 +52,8 @@ struct CollisionRequest;
/// @brief Class merging the OBB and RSS, can handle collision and distance
/// simultaneously
struct HPP_FCL_DLLAPI OBBRSS {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
/// @brief OBB member, for rotation
OBB obb;
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index 6e624d4ca128f130d440d460f51ac66321879168..c11e09d86225220ebde633ba5f9a96013a5a1709 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -51,6 +51,8 @@ struct CollisionRequest;
/// @brief A class for rectangle sphere-swept bounding volume
struct HPP_FCL_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,
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 92d82ba4f904fc0dedf9987669cd6301859769de..0ce92f66d322647c2d4a122407c988d138601eb7 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -95,6 +95,8 @@ class HPP_FCL_DLLAPI KDOP {
Eigen::Array<FCL_REAL, N, 1> dist_;
public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
/// @brief Creating kDOP containing nothing
KDOP();
@@ -165,11 +167,6 @@ class HPP_FCL_DLLAPI KDOP {
//// @brief Check whether one point is inside the KDOP
bool inside(const Vec3f& p) const;
-
- public:
- /// \cond
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- /// \endcond
};
template <short N>
diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h
index 35dc5f211808ed8dafb41ab5909b398bec0be005..f776f0a15dcc8bc5666cd024fcfd3c338b17c592 100644
--- a/include/hpp/fcl/BV/kIOS.h
+++ b/include/hpp/fcl/BV/kIOS.h
@@ -53,6 +53,8 @@ struct CollisionRequest;
class HPP_FCL_DLLAPI kIOS {
/// @brief One sphere in kIOS
struct HPP_FCL_DLLAPI kIOS_Sphere {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
Vec3f o;
FCL_REAL r;
@@ -92,6 +94,8 @@ class HPP_FCL_DLLAPI kIOS {
}
public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
/// @brief Equality operator
bool operator==(const kIOS& other) const {
bool res = obb == other.obb && num_spheres == other.num_spheres;
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index a57fc7b845071605c260222102c38c8426f7b760..61a01a9dedf0e54d7f790376499b287e9ee88ac3 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -44,6 +44,8 @@
#include <hpp/fcl/BVH/BVH_internal.h>
#include <hpp/fcl/BV/BV_node.h>
#include <vector>
+#include <memory>
+#include <iostream>
namespace hpp {
namespace fcl {
@@ -63,13 +65,13 @@ class BVSplitter;
class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
public:
/// @brief Geometry point data
- Vec3f* vertices;
+ std::shared_ptr<std::vector<Vec3f>> vertices;
/// @brief Geometry triangle index data, will be NULL for point clouds
- Triangle* tri_indices;
+ std::shared_ptr<std::vector<Triangle>> tri_indices;
/// @brief Geometry point data in previous frame
- Vec3f* prev_vertices;
+ std::shared_ptr<std::vector<Vec3f>> prev_vertices;
/// @brief Number of triangles
unsigned int num_tris;
@@ -100,11 +102,7 @@ class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
BVHModelBase(const BVHModelBase& other);
/// @brief deconstruction, delete mesh data related.
- virtual ~BVHModelBase() {
- delete[] vertices;
- delete[] tri_indices;
- delete[] prev_vertices;
- }
+ virtual ~BVHModelBase() {}
/// @brief Get the object type: it is a BVH
OBJECT_TYPE getObjectType() const { return OT_BVH; }
@@ -203,13 +201,28 @@ class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
Vec3f computeCOM() const {
FCL_REAL vol = 0;
Vec3f 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<Vec3f>& 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<Triangle>& tri_indices_ = *tri_indices;
+
for (unsigned int i = 0; i < num_tris; ++i) {
- const Triangle& tri = tri_indices[i];
+ const Triangle& tri = tri_indices_[i];
FCL_REAL d_six_vol =
- (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
+ (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;
+ com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) *
+ d_six_vol;
}
return com / (vol * 4);
@@ -217,10 +230,24 @@ class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
FCL_REAL computeVolume() const {
FCL_REAL vol = 0;
+ if (!(vertices.get())) {
+ std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
+ "vertices."
+ << std::endl;
+ return vol;
+ }
+ const std::vector<Vec3f>& 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<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_tris; ++i) {
- const Triangle& tri = tri_indices[i];
+ const Triangle& tri = tri_indices_[i];
FCL_REAL d_six_vol =
- (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
+ (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
vol += d_six_vol;
}
@@ -234,11 +261,25 @@ class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0,
1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0;
+ if (!(vertices.get())) {
+ std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
+ "not contain vertices."
+ << std::endl;
+ return C;
+ }
+ const std::vector<Vec3f>& vertices_ = *vertices;
+ if (!(vertices.get())) {
+ std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
+ "not contain vertices."
+ << std::endl;
+ return C;
+ }
+ const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_tris; ++i) {
- const Triangle& tri = tri_indices[i];
- const Vec3f& v1 = vertices[tri[0]];
- const Vec3f& v2 = vertices[tri[1]];
- const Vec3f& v3 = vertices[tri[2]];
+ const Triangle& tri = tri_indices_[i];
+ const Vec3f& v1 = vertices_[tri[0]];
+ const Vec3f& v2 = vertices_[tri[1]];
+ const Vec3f& v3 = vertices_[tri[2]];
Matrix3f A;
A << v1.transpose(), v2.transpose(), v3.transpose();
C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
@@ -274,11 +315,14 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
typedef BVHModelBase Base;
public:
+ using bv_node_vector_t =
+ std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;
+
/// @brief Split rule to split one BV node into two children
- shared_ptr<BVSplitter<BV> > bv_splitter;
+ shared_ptr<BVSplitter<BV>> bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
- shared_ptr<BVFitter<BV> > bv_fitter;
+ shared_ptr<BVFitter<BV>> bv_fitter;
/// @brief Default constructor to build an empty BVH
BVHModel();
@@ -293,10 +337,7 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
/// @brief deconstruction, delete mesh data related.
- ~BVHModel() {
- delete[] bvs;
- delete[] primitive_indices;
- }
+ ~BVHModel() {}
/// @brief We provide getBV() and getNumBVs() because BVH may be compressed
/// (in future), so we must provide some flexibility here
@@ -304,13 +345,13 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
/// @brief Access the bv giving the its index
const BVNode<BV>& getBV(unsigned int i) const {
assert(i < num_bvs);
- return bvs[i];
+ return (*bvs)[i];
}
/// @brief Access the bv giving the its index
BVNode<BV>& getBV(unsigned int i) {
assert(i < num_bvs);
- return bvs[i];
+ return (*bvs)[i];
}
/// @brief Get the number of bv in the BVH
@@ -336,10 +377,10 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
bool allocateBVs();
unsigned int num_bvs_allocated;
- unsigned int* primitive_indices;
+ std::shared_ptr<std::vector<unsigned int>> primitive_indices;
/// @brief Bounding volume hierarchy
- BVNode<BV>* bvs;
+ std::shared_ptr<bv_node_vector_t> bvs;
/// @brief Number of BV nodes in bounding volume hierarchy
unsigned int num_bvs;
@@ -370,15 +411,19 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
/// OBBRSS), special implementation is provided.
void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
const Vec3f& parent_c) {
- if (!bvs[bv_id].isLeaf()) {
- makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes,
- bvs[bv_id].getCenter());
-
- makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes,
- bvs[bv_id].getCenter());
+ bv_node_vector_t& bvs_ = *bvs;
+ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
+ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
+ parent_axes,
+ bvs_[static_cast<size_t>(bv_id)].getCenter());
+
+ makeParentRelativeRecurse(
+ bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
+ bvs_[static_cast<size_t>(bv_id)].getCenter());
}
- bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
+ bvs_[static_cast<size_t>(bv_id)].bv =
+ translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c);
}
private:
@@ -435,8 +480,14 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
if (num_bvs != other.num_bvs) return false;
- for (unsigned int k = 0; k < num_bvs; ++k) {
- if (bvs[k] != other.bvs[k]) return false;
+ if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
+ return false;
+ if (bvs.get() && other.bvs.get()) {
+ const bv_node_vector_t& bvs_ = *bvs;
+ const bv_node_vector_t& other_bvs_ = *(other.bvs);
+ for (unsigned int k = 0; k < num_bvs; ++k) {
+ if (bvs_[k] != other_bvs_[k]) return false;
+ }
}
return true;
@@ -475,13 +526,13 @@ template <>
NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
template <>
-NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
+NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const;
template <>
-NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
+NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const;
template <>
-NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
+NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const;
} // namespace fcl
diff --git a/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/hpp/fcl/broadphase/broadphase_SaP.h
index 0751401d1c80715170fab74be162f8f55438bfe1..a71f78ad44864940617085bc54d122b82fae569b 100644
--- a/include/hpp/fcl/broadphase/broadphase_SaP.h
+++ b/include/hpp/fcl/broadphase/broadphase_SaP.h
@@ -152,9 +152,9 @@ class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager {
/// @brief set the value of the end point
Vec3f& getVal();
- FCL_REAL getVal(size_t i) const;
+ FCL_REAL getVal(int i) const;
- FCL_REAL& getVal(size_t i);
+ FCL_REAL& getVal(int i);
};
/// @brief A pair of objects that are not culling away and should further
diff --git a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h
index bf240fded3c0ca2b33d515dbd9522b454f93134e..4dcdfc42cfaebb2a73167509c2b6cdd96bea19b7 100644
--- a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h
+++ b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h
@@ -2,7 +2,7 @@
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Toyota Research Institute
- * Copyright (c) 2022, INRIA
+ * Copyright (c) 2022-2023, INRIA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -238,9 +238,12 @@ struct HPP_FCL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase {
/// @brief Reset the callback
void init();
- /// @brief Check wether a collision pair exists
+ /// @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:
diff --git a/include/hpp/fcl/broadphase/detail/morton.h b/include/hpp/fcl/broadphase/detail/morton.h
index fb480d387a55e2472a3771015bec0b347a3b8862..cf9968af49aedd32c405add78c08a66b1ed0846c 100644
--- a/include/hpp/fcl/broadphase/detail/morton.h
+++ b/include/hpp/fcl/broadphase/detail/morton.h
@@ -40,6 +40,7 @@
#define HPP_FCL_MORTON_H
#include "hpp/fcl/BV/AABB.h"
+#include <cstdint>
#include <bitset>
namespace hpp {
diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h
index bf4357365b31a595821f17ea269776c53fc5ef47..9516b4cb6dbb92649f3a84f61a3fb7b2fa3a8bae 100644
--- a/include/hpp/fcl/collision_data.h
+++ b/include/hpp/fcl/collision_data.h
@@ -39,6 +39,7 @@
#define HPP_FCL_COLLISION_DATA_H
#include <vector>
+#include <array>
#include <set>
#include <limits>
@@ -70,9 +71,30 @@ struct HPP_FCL_DLLAPI Contact {
/// if object 2 is octree, it is the id of the cell
int b2;
- /// @brief contact normal, pointing from o1 to o2
+ /// @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().
Vec3f 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<Vec3f, 2> nearest_points;
+
/// @brief contact position, in world space
Vec3f pos;
@@ -83,11 +105,19 @@ struct HPP_FCL_DLLAPI Contact {
static const int NONE = -1;
/// @brief Default constructor
- Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {}
+ Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
+ penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
+ nearest_points[0] = nearest_points[1] = normal = pos =
+ Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ }
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
int b2_)
- : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {}
+ : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
+ penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
+ nearest_points[0] = nearest_points[1] = normal = pos =
+ Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ }
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_)
@@ -97,7 +127,24 @@ struct HPP_FCL_DLLAPI Contact {
b2(b2_),
normal(normal_),
pos(pos_),
- penetration_depth(depth_) {}
+ penetration_depth(depth_) {
+ nearest_points[0] = pos - 0.5 * depth_ * normal_;
+ nearest_points[1] = pos + 0.5 * depth_ * normal_;
+ }
+
+ Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
+ int b2_, const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_,
+ FCL_REAL depth_)
+ : o1(o1_),
+ o2(o2_),
+ b1(b1_),
+ b2(b2_),
+ normal(normal_),
+ penetration_depth(depth_) {
+ nearest_points[0] = p1;
+ nearest_points[1] = p2;
+ pos = (p1 + p2) / 2;
+ }
bool operator<(const Contact& other) const {
if (b1 == other.b1) return b2 < other.b2;
@@ -107,17 +154,21 @@ struct HPP_FCL_DLLAPI Contact {
bool operator==(const Contact& other) const {
return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 &&
b2 == other.b2 && normal == other.normal && pos == other.pos &&
+ nearest_points[0] == other.nearest_points[0] &&
+ nearest_points[1] == other.nearest_points[1] &&
penetration_depth == other.penetration_depth;
}
bool operator!=(const Contact& other) const { return !(*this == other); }
+
+ FCL_REAL getDistanceToCollision(const CollisionRequest& request) const;
};
struct QueryResult;
/// @brief base class for all query requests
struct HPP_FCL_DLLAPI QueryRequest {
- // @briefInitial guess to use for the GJK algorithm
+ // @brief Initial guess to use for the GJK algorithm
GJKInitialGuess gjk_initial_guess;
/// @brief whether enable gjk initial guess
@@ -146,6 +197,18 @@ struct HPP_FCL_DLLAPI QueryRequest {
/// @brief the support function initial guess set by user
support_func_guess_t cached_support_func_guess;
+ /// @brief max number of faces for EPA
+ size_t epa_max_face_num;
+
+ /// @brief max number of vertices for EPA
+ size_t epa_max_vertex_num;
+
+ /// @brief max number of iterations for EPA
+ size_t epa_max_iterations;
+
+ /// @brief tolerance for EPA
+ FCL_REAL epa_tolerance;
+
/// @brief enable timings when performing collision/distance request
bool enable_timings;
@@ -165,6 +228,10 @@ struct HPP_FCL_DLLAPI QueryRequest {
gjk_max_iterations(128),
cached_gjk_guess(1, 0, 0),
cached_support_func_guess(support_func_guess_t::Zero()),
+ epa_max_face_num(128),
+ epa_max_vertex_num(64),
+ epa_max_iterations(255),
+ epa_tolerance(1e-6),
enable_timings(false),
collision_distance_threshold(
Eigen::NumTraits<FCL_REAL>::dummy_precision()) {}
@@ -184,9 +251,20 @@ struct HPP_FCL_DLLAPI QueryRequest {
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
return gjk_initial_guess == other.gjk_initial_guess &&
enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
+ gjk_variant == other.gjk_variant &&
+ gjk_convergence_criterion == other.gjk_convergence_criterion &&
+ gjk_convergence_criterion_type ==
+ other.gjk_convergence_criterion_type &&
+ gjk_tolerance == other.gjk_tolerance &&
+ gjk_max_iterations == other.gjk_max_iterations &&
cached_gjk_guess == other.cached_gjk_guess &&
cached_support_func_guess == other.cached_support_func_guess &&
- enable_timings == other.enable_timings;
+ epa_max_face_num == other.epa_max_face_num &&
+ epa_max_vertex_num == other.epa_max_vertex_num &&
+ epa_max_iterations == other.epa_max_iterations &&
+ epa_tolerance == other.epa_tolerance &&
+ enable_timings == other.enable_timings &&
+ collision_distance_threshold == other.collision_distance_threshold;
HPP_FCL_COMPILER_DIAGNOSTIC_POP
}
};
@@ -233,11 +311,13 @@ enum CollisionRequestFlag {
/// @brief request to the collision algorithm
struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest {
- /// @brief The maximum number of contacts will return
+ /// @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
+ /// contact position) will return.
+ /// @note Only effective if the collision pair involves an Octree.
+ /// Otherwise, it is always true.
bool enable_contact;
/// Whether a lower bound on distance is returned when objects are disjoint
@@ -298,6 +378,11 @@ struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest {
}
};
+inline FCL_REAL Contact::getDistanceToCollision(
+ const CollisionRequest& request) const {
+ return penetration_depth - request.security_margin;
+}
+
/// @brief collision result
struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
private:
@@ -307,18 +392,30 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
public:
/// Lower bound on distance between objects if they are disjoint.
/// See \ref hpp_fcl_collision_and_distance_lower_bound_computation
- /// @note computed only on request (or if it does not add any computational
- /// overhead).
+ /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is
+ /// set to infinity, distance_lower_bound is the actual distance between the
+ /// shapes.
FCL_REAL distance_lower_bound;
- /// @brief nearest points
- /// available only when distance_lower_bound is inferior to
+ /// @brief normal associated to nearest_points.
+ /// Same as `CollisionResult::nearest_points` but for the normal.
+ Vec3f 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.
- Vec3f nearest_points[2];
+ std::array<Vec3f, 2> nearest_points;
public:
CollisionResult()
- : distance_lower_bound((std::numeric_limits<FCL_REAL>::max)()) {}
+ : distance_lower_bound((std::numeric_limits<FCL_REAL>::max)()) {
+ nearest_points[0] = nearest_points[1] = normal =
+ Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ }
/// @brief Update the lower bound only if the distance is inferior.
inline void updateDistanceLowerBound(const FCL_REAL& distance_lower_bound_) {
@@ -332,7 +429,10 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
/// @brief whether two CollisionResult are the same or not
inline bool operator==(const CollisionResult& other) const {
return contacts == other.contacts &&
- distance_lower_bound == other.distance_lower_bound;
+ distance_lower_bound == other.distance_lower_bound &&
+ nearest_points[0] == other.nearest_points[0] &&
+ nearest_points[1] == other.nearest_points[1] &&
+ normal == other.normal;
}
/// @brief return binary collision result
@@ -344,8 +444,9 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
/// @brief get the i-th contact calculated
const Contact& getContact(size_t i) const {
if (contacts.size() == 0)
- throw std::invalid_argument(
- "The number of contacts is zero. No Contact can be returned.");
+ HPP_FCL_THROW_PRETTY(
+ "The number of contacts is zero. No Contact can be returned.",
+ std::invalid_argument);
if (i < contacts.size())
return contacts[i];
@@ -356,8 +457,9 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
/// @brief set the i-th contact calculated
void setContact(size_t i, const Contact& c) {
if (contacts.size() == 0)
- throw std::invalid_argument(
- "The number of contacts is zero. No Contact can be returned.");
+ HPP_FCL_THROW_PRETTY(
+ "The number of contacts is zero. No Contact can be returned.",
+ std::invalid_argument);
if (i < contacts.size())
contacts[i] = c;
@@ -377,8 +479,9 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
void clear() {
distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
contacts.clear();
- distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
timings.clear();
+ nearest_points[0] = nearest_points[1] = normal =
+ Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
}
/// @brief reposition Contact objects when fcl inverts them
@@ -419,16 +522,17 @@ struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest {
/// @brief distance result
struct HPP_FCL_DLLAPI DistanceResult : QueryResult {
public:
- /// @brief minimum distance between two objects. if two objects are in
+ /// @brief minimum distance between two objects. If two objects are in
/// collision, min_distance <= 0.
FCL_REAL min_distance;
- /// @brief nearest points
- Vec3f nearest_points[2];
-
- /// In case both objects are in collision, store the normal
+ /// @brief normal.
Vec3f normal;
+ /// @brief nearest points.
+ /// See CollisionResult::nearest_points.
+ std::array<Vec3f, 2> nearest_points;
+
/// @brief collision object 1
const CollisionGeometry* o1;
@@ -537,7 +641,7 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult {
namespace internal {
inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
CollisionResult& res,
- const FCL_REAL& sqrDistLowerBound) {
+ const FCL_REAL sqrDistLowerBound) {
// BV cannot find negative distance.
if (res.distance_lower_bound <= 0) return;
FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin;
@@ -547,11 +651,13 @@ inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
CollisionResult& res,
const FCL_REAL& distance,
- const Vec3f& p0, const Vec3f& p1) {
+ const Vec3f& p0, const Vec3f& p1,
+ const Vec3f& 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
diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h
index 7403f92306a611607cea540d2821e6004c50475c..fae6514644394076b22f8c9f782996578c8a15ea 100644
--- a/include/hpp/fcl/collision_object.h
+++ b/include/hpp/fcl/collision_object.h
@@ -260,10 +260,18 @@ class HPP_FCL_DLLAPI CollisionObject {
if (t.getRotation().isIdentity()) {
aabb = translate(cgeom->aabb_local, t.getTranslation());
} else {
- Vec3f center(t.transform(cgeom->aabb_center));
- Vec3f delta(Vec3f::Constant(cgeom->aabb_radius));
- aabb.min_ = center - delta;
- aabb.max_ = center + delta;
+ aabb.min_ = aabb.max_ = t.getTranslation();
+
+ Vec3f 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();
+ }
}
}
diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h
index 952a5cd127549bceb992be6f447bef715baba520..8474fec27cbe51504d8b000c0e14bae32510da44 100644
--- a/include/hpp/fcl/data_types.h
+++ b/include/hpp/fcl/data_types.h
@@ -3,6 +3,7 @@
*
* 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
@@ -64,10 +65,12 @@ namespace hpp {
namespace fcl {
typedef double FCL_REAL;
typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
+typedef Eigen::Matrix<FCL_REAL, 6, 1> Vec6f;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> VecXf;
typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3> Matrixx3f;
-typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3> Matrixx3i;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3, Eigen::RowMajor> Matrixx3f;
+typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor>
+ Matrixx3i;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
typedef Eigen::Vector2i support_func_guess_t;
@@ -80,7 +83,7 @@ typedef Eigen::Vector2i support_func_guess_t;
enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess };
/// @brief Variant to use for the GJK algorithm
-enum GJKVariant { DefaultGJK, NesterovAcceleration };
+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
diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh
index 0585ce3b43dc5d6f2b8456aed3998da3dadc0755..f40179365f3978da567c9daec898951a4550b790 100644
--- a/include/hpp/fcl/fwd.hh
+++ b/include/hpp/fcl/fwd.hh
@@ -54,6 +54,12 @@
#define HPP_FCL_UNUSED_VARIABLE(var) (void)(var)
+#ifdef NDEBUG
+#define HPP_FCL_ONLY_USED_FOR_DEBUG(var) HPP_FCL_UNUSED_VARIABLE(var)
+#else
+#define HPP_FCL_ONLY_USED_FOR_DEBUG(var)
+#endif
+
#define HPP_FCL_THROW_PRETTY(message, exception) \
{ \
std::stringstream ss; \
diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h
index adb8f3d013e62068e5d9b6002d9c3ee73523637a..6c066d0dc1cd19b8769ee4b7891f43e947421a82 100644
--- a/include/hpp/fcl/hfield.h
+++ b/include/hpp/fcl/hfield.h
@@ -1,7 +1,7 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2021, INRIA
+ * Copyright (c) 2021-2024, INRIA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -38,6 +38,7 @@
#define HPP_FCL_HEIGHT_FIELD_H
#include <hpp/fcl/fwd.hh>
+#include <hpp/fcl/data_types.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BVH/BVH_internal.h>
@@ -51,6 +52,8 @@ namespace fcl {
/// @{
struct HPP_FCL_DLLAPI HFNodeBase {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
/// @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)
@@ -103,6 +106,8 @@ struct HPP_FCL_DLLAPI HFNodeBase {
template <typename BV>
struct HPP_FCL_DLLAPI HFNode : public HFNodeBase {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
typedef HFNodeBase Base;
/// @brief bounding volume storing the geometry
@@ -135,16 +140,11 @@ struct HPP_FCL_DLLAPI HFNode : public HFNodeBase {
Vec3f getCenter() const { return bv.center(); }
/// @brief Access to the orientation of the BV
- const Matrix3f& getOrientation() const {
- static const Matrix3f id3 = Matrix3f::Identity();
- return id3;
+ hpp::fcl::Matrix3f::IdentityReturnType getOrientation() const {
+ return Matrix3f::Identity();
}
virtual ~HFNode() {}
-
- /// \cond
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- /// \endcond
};
namespace details {
@@ -181,10 +181,12 @@ struct UpdateBoundingVolume<AABB> {
template <typename BV>
class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {
public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
typedef CollisionGeometry Base;
typedef HFNode<BV> Node;
- typedef std::vector<Node> BVS;
+ typedef std::vector<Node, Eigen::aligned_allocator<Node> > BVS;
/// @brief Constructing an empty HeightField
HeightField()
@@ -465,9 +467,6 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {
y_grid == other.y_grid && bvs == other.bvs &&
num_bvs == other.num_bvs;
}
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/// @brief Specialization of getNodeType() for HeightField with different BV
diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h
index 181302ea8c32ea47cd6a4787eb9d74bf1af93e3c..89f4f104061bd2ba3fa8c15663976738b846791c 100644
--- a/include/hpp/fcl/internal/shape_shape_func.h
+++ b/include/hpp/fcl/internal/shape_shape_func.h
@@ -66,6 +66,7 @@ struct ShapeShapeCollider {
if (request.isSatisfied(result)) return result.numContacts();
DistanceResult distanceResult;
+ // (louis) enable_contact not used yet but may be in the future
DistanceRequest distanceRequest(request.enable_contact);
FCL_REAL distance = ShapeShapeDistance<T_SH1, T_SH2>(
o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
@@ -73,20 +74,19 @@ struct ShapeShapeCollider {
size_t num_contacts = 0;
const Vec3f& p1 = distanceResult.nearest_points[0];
const Vec3f& p2 = distanceResult.nearest_points[1];
+ const Vec3f& normal = distanceResult.normal;
FCL_REAL distToCollision = distance - request.security_margin;
internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
- p1, p2);
+ p1, p2, normal);
if (distToCollision <= request.collision_distance_threshold &&
result.numContacts() < request.num_max_contacts) {
if (result.numContacts() < request.num_max_contacts) {
const Vec3f& p1 = distanceResult.nearest_points[0];
const Vec3f& p2 = distanceResult.nearest_points[1];
- Contact contact(
- o1, o2, distanceResult.b1, distanceResult.b2, (p1 + p2) / 2,
- (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()),
- -distance);
+ Contact contact(o1, o2, distanceResult.b1, distanceResult.b2, p1, p2,
+ normal, distance);
result.addContact(contact);
}
@@ -136,6 +136,11 @@ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace);
SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane);
SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, 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);
+// TODO
+// SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, TriangleP);
SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace);
SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace);
diff --git a/include/hpp/fcl/internal/tools.h b/include/hpp/fcl/internal/tools.h
index 52a2ecad726c960f9c2be5d07ab1b521dbd04a5c..346141a500ae534a2379059bb04ed09d9e6ac91d 100644
--- a/include/hpp/fcl/internal/tools.h
+++ b/include/hpp/fcl/internal/tools.h
@@ -108,8 +108,7 @@ void eigen(const Eigen::MatrixBase<Derived>& m,
int n = 3;
int j, iq, ip, i;
Scalar tresh, theta, tau, t, sm, s, h, g, c;
- int nrot;
- HPP_FCL_UNUSED_VARIABLE(nrot);
+
Scalar b[3];
Scalar z[3];
Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
@@ -120,8 +119,6 @@ void eigen(const Eigen::MatrixBase<Derived>& m,
z[ip] = 0;
}
- nrot = 0;
-
for (i = 0; i < 50; ++i) {
sm = 0;
for (ip = 0; ip < n; ++ip)
@@ -189,7 +186,6 @@ void eigen(const Eigen::MatrixBase<Derived>& m,
v[j][ip] = g - s * (h + g * tau);
v[j][iq] = h + s * (g - h * tau);
}
- nrot++;
}
}
}
diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h
index f1ced715e6c41d15321a792d21561056288a1901..d7521d88e6bb897c72546bc321420fa4d692ced4 100644
--- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h
@@ -193,38 +193,30 @@ class MeshShapeCollisionTraversalNode
Vec3f normal;
Vec3f c1, c2; // closest point
- bool collision;
if (RTIsIdentity) {
static const Transform3f Id;
- collision = nsolver->shapeTriangleInteraction(
- *(this->model2), this->tf2, p1, p2, p3, Id, distance, c2, c1, normal);
+ nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
+ Id, distance, c2, c1, normal);
} else {
- collision = nsolver->shapeTriangleInteraction(*(this->model2), this->tf2,
- p1, p2, p3, this->tf1,
- distance, c2, c1, normal);
+ nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
+ this->tf1, distance, c2, c1, normal);
}
+ // TODO @louis: change things here as in hfields & ShapeShapeDistance
FCL_REAL distToCollision = distance - this->request.security_margin;
- if (collision) {
+ if (distToCollision <= this->request.collision_distance_threshold) {
sqrDistLowerBound = 0;
if (this->request.num_max_contacts > this->result->numContacts()) {
this->result->addContact(Contact(this->model1, this->model2,
- primitive_id, Contact::NONE, c1,
- -normal, -distance));
+ primitive_id, Contact::NONE, c1, c2,
+ -normal, distance));
assert(this->result->isCollision());
}
- } else if (distToCollision <= this->request.collision_distance_threshold) {
- sqrDistLowerBound = 0;
- if (this->request.num_max_contacts > this->result->numContacts()) {
- this->result->addContact(
- Contact(this->model1, this->model2, primitive_id, Contact::NONE,
- .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
- }
} else
sqrDistLowerBound = distToCollision * distToCollision;
- internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
- distToCollision, c1, c2);
+ internal::updateDistanceLowerBoundFromLeaf(
+ this->request, *this->result, distToCollision, c1, c2, -normal);
assert(this->result->isCollision() || sqrDistLowerBound > 0);
}
diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h
index 9880bec43c81efbbcd75bc620735a75e94d52856..c4c40a20f5e40b478a1c145654e7f6c5a97a9747 100644
--- a/include/hpp/fcl/internal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/internal/traversal_node_bvhs.h
@@ -216,23 +216,22 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
if (distToCollision <=
this->request.collision_distance_threshold) { // collision
sqrDistLowerBound = 0;
- Vec3f p(p1); // contact point
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).
- if (distance > 0) {
- normal = (p2 - p1).normalized();
- p = .5 * (p1 + p2);
- }
+ // The normal is already computed by GJK, no need to recompute it.
+ // if (distance > 0) {
+ // normal = (p2 - p1).normalized();
+ // }
this->result->addContact(Contact(this->model1, this->model2,
- primitive_id1, primitive_id2, p,
- normal, -distance));
+ primitive_id1, primitive_id2, p1, p2,
+ normal, distance));
}
} else
sqrDistLowerBound = distToCollision * distToCollision;
internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
- distToCollision, p1, p2);
+ distToCollision, p1, p2, normal);
}
Vec3f* vertices1;
diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h
index 9fdcb85d1aafcf24d00fddf706ae5a6b6bd1d6a0..1774c68f641c24091dc7cc1e4cd59a3c295f85c2 100644
--- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h
+++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h
@@ -1,7 +1,7 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2021, INRIA.
+ * Copyright (c) 2021-2023, INRIA.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -45,6 +45,7 @@
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/internal/traversal_node_base.h>
#include <hpp/fcl/internal/traversal.h>
+#include <hpp/fcl/internal/intersect.h>
#include <hpp/fcl/hfield.h>
#include <hpp/fcl/shape/convex.h>
@@ -73,26 +74,27 @@ Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
"max_height is lower than min_height"); // Check whether the geometry
// is degenerated
- Vec3f* pts = new Vec3f[8];
- pts[0] = Vec3f(x0, y0, min_height);
- pts[1] = Vec3f(x0, y1, min_height);
- pts[2] = Vec3f(x1, y1, min_height);
- pts[3] = Vec3f(x1, y0, min_height);
- pts[4] = Vec3f(x0, y0, cell(0, 0));
- pts[5] = Vec3f(x0, y1, cell(1, 0));
- pts[6] = Vec3f(x1, y1, cell(1, 1));
- pts[7] = Vec3f(x1, y0, cell(0, 1));
-
- Quadrilateral* polygons = new Quadrilateral[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 Convex<Quadrilateral>(true,
- pts, // points
+ std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
+ Vec3f(x0, y0, min_height),
+ Vec3f(x0, y1, min_height),
+ Vec3f(x1, y1, min_height),
+ Vec3f(x1, y0, min_height),
+ Vec3f(x0, y0, cell(0, 0)),
+ Vec3f(x0, y1, cell(1, 0)),
+ Vec3f(x1, y1, cell(1, 1)),
+ Vec3f(x1, y0, cell(0, 1)),
+ }));
+
+ std::shared_ptr<std::vector<Quadrilateral>> polygons(
+ new std::vector<Quadrilateral>(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 Convex<Quadrilateral>(pts, // points
8, // num points
polygons,
6 // number of polygons
@@ -121,56 +123,278 @@ void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
HPP_FCL_UNUSED_VARIABLE(max_height);
{
- Vec3f* pts = new Vec3f[8];
- pts[0] = Vec3f(x0, y0, min_height);
- pts[1] = Vec3f(x0, y1, min_height);
- pts[2] = Vec3f(x1, y1, min_height);
- pts[3] = Vec3f(x1, y0, min_height);
- pts[4] = Vec3f(x0, y0, cell(0, 0));
- pts[5] = Vec3f(x0, y1, cell(1, 0));
- pts[6] = Vec3f(x1, y1, cell(1, 1));
- pts[7] = Vec3f(x1, y0, cell(0, 1));
-
- Triangle* triangles = new Triangle[8];
- triangles[0].set(0, 1, 3); // bottom
- triangles[1].set(4, 5, 7); // top
- triangles[2].set(0, 1, 4);
- triangles[3].set(4, 1, 5);
- triangles[4].set(1, 7, 3);
- triangles[5].set(1, 5, 7);
- triangles[6].set(0, 3, 7);
- triangles[7].set(7, 4, 0);
-
- convex1.set(true,
- pts, // points
- 8, // num points
+ std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
+ Vec3f(x0, y0, min_height),
+ Vec3f(x0, y1, min_height),
+ Vec3f(x1, y0, min_height),
+ Vec3f(x0, y0, cell(0, 0)),
+ Vec3f(x0, y1, cell(1, 0)),
+ Vec3f(x1, y0, cell(0, 1)),
+ }));
+
+ std::shared_ptr<std::vector<Triangle>> triangles(
+ new std::vector<Triangle>(8));
+ (*triangles)[0].set(0, 1, 2); // bottom
+ (*triangles)[1].set(3, 5, 4); // top
+ (*triangles)[2].set(0, 3, 1);
+ (*triangles)[3].set(3, 4, 1);
+ (*triangles)[4].set(1, 5, 2);
+ (*triangles)[5].set(1, 4, 5);
+ (*triangles)[6].set(0, 2, 5);
+ (*triangles)[7].set(5, 3, 0);
+
+ convex1.set(pts, // points
+ 6, // num points
triangles,
8 // number of polygons
);
}
{
- Vec3f* pts = new Vec3f[8];
- memcpy(pts, convex1.points, 8 * sizeof(Vec3f));
-
- Triangle* triangles = new Triangle[8];
- triangles[0].set(3, 2, 1); // top
- triangles[1].set(5, 6, 7); // bottom
- triangles[2].set(1, 2, 5);
- triangles[3].set(5, 2, 6);
- triangles[4].set(1, 3, 7);
- triangles[5].set(1, 7, 5);
- triangles[6].set(2, 3, 7);
- triangles[7].set(6, 2, 3);
-
- convex2.set(true,
- pts, // points
- 8, // num points
+ std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
+ Vec3f(x0, y1, min_height),
+ Vec3f(x1, y1, min_height),
+ Vec3f(x1, y0, min_height),
+ Vec3f(x0, y1, cell(1, 0)),
+ Vec3f(x1, y1, cell(1, 1)),
+ Vec3f(x1, y0, cell(0, 1)),
+ }));
+
+ std::shared_ptr<std::vector<Triangle>> triangles(
+ new std::vector<Triangle>(8));
+ (*triangles)[0].set(2, 0, 1); // bottom
+ (*triangles)[1].set(3, 5, 4); // top
+ (*triangles)[2].set(0, 3, 1);
+ (*triangles)[3].set(3, 4, 1);
+ (*triangles)[4].set(0, 2, 5);
+ (*triangles)[5].set(0, 5, 3);
+ (*triangles)[6].set(1, 5, 2);
+ (*triangles)[7].set(4, 2, 1);
+
+ convex2.set(pts, // points
+ 6, // num points
triangles,
8 // number of polygons
);
}
}
+
+inline Vec3f projectTriangle(const Vec3f& pointA, const Vec3f& pointB,
+ const Vec3f& pointC, const Vec3f& point) {
+ const Project::ProjectResult result =
+ Project::projectTriangle(pointA, pointB, pointC, point);
+ Vec3f res = result.parameterization[0] * pointA +
+ result.parameterization[1] * pointB +
+ result.parameterization[2] * pointC;
+
+ return res;
+}
+
+template <typename Polygone, typename Shape>
+bool binCorrection(const Convex<Polygone>& convex, const Shape& shape,
+ const Transform3f& shape_pose, FCL_REAL& distance,
+ Vec3f& contact_1, Vec3f& contact_2, Vec3f& normal,
+ Vec3f& normal_top, bool& is_collision) {
+ const Polygone& top_triangle = (*(convex.polygons))[1];
+ const std::vector<Vec3f>& points = *(convex.points);
+ const Vec3f pointA = points[top_triangle[0]];
+ const Vec3f pointB = points[top_triangle[1]];
+ const Vec3f pointC = points[top_triangle[2]];
+ normal_top = (pointB - pointA).cross(pointC - pointA).normalized();
+ if (normal_top[2] < 0) normal_top *= -1.;
+
+ assert(!normal_top.array().isNaN().any() && "normal_top is ill-defined");
+
+ const Vec3f contact_1_projected =
+ projectTriangle(pointA, pointB, pointC, contact_1);
+
+ bool hfield_witness_is_on_bin_side;
+ if (!contact_1_projected.isApprox(contact_1)) {
+ hfield_witness_is_on_bin_side = true;
+ } else {
+ hfield_witness_is_on_bin_side = false;
+ normal = normal_top;
+ }
+
+ // We correct only if there is a collision with the bin
+ if (is_collision) {
+ int hint = 0;
+ const Vec3f _support = getSupport(
+ &shape, -shape_pose.rotation().transpose() * normal_top, true, hint);
+ const Vec3f support =
+ shape_pose.rotation() * _support + shape_pose.translation();
+
+ // Project support into the inclined bin having triangle
+ const FCL_REAL offset_plane = normal_top.dot(pointA);
+ const Plane projection_plane(normal_top, offset_plane);
+ const FCL_REAL distance_support_projection_plane =
+ projection_plane.signedDistance(support);
+
+ const Vec3f projected_support =
+ support - distance_support_projection_plane * normal_top;
+
+ // We need now to project the projected in the triangle shape
+ contact_1 = projectTriangle(pointA, pointB, pointC, projected_support);
+ contact_2 = contact_1 + distance_support_projection_plane * normal_top;
+ normal = normal_top;
+ distance = -(contact_1 - contact_2).norm();
+ }
+
+ return hfield_witness_is_on_bin_side;
+}
+
+template <typename Polygone, typename Shape, int Options>
+bool shapeDistance(const GJKSolver* nsolver, const Convex<Polygone>& convex1,
+ const Convex<Polygone>& convex2, const Transform3f& tf1,
+ const Shape& shape, const Transform3f& tf2,
+ FCL_REAL& distance, Vec3f& c1, Vec3f& c2, Vec3f& normal,
+ Vec3f& normal_top, bool& hfield_witness_is_on_bin_side) {
+ enum { RTIsIdentity = Options & RelativeTransformationIsIdentity };
+
+ const Transform3f Id;
+ Vec3f contact1_1, contact1_2, normal1, normal1_top;
+ Vec3f contact2_1, contact2_2, normal2, normal2_top;
+ FCL_REAL distance1, distance2;
+ bool collision1, collision2;
+ bool hfield_witness_is_on_bin_side1, hfield_witness_is_on_bin_side2;
+ if (RTIsIdentity)
+ nsolver->shapeDistance(convex1, Id, shape, tf2, distance1, contact1_1,
+ contact1_2, normal1);
+ else
+ nsolver->shapeDistance(convex1, tf1, shape, tf2, distance1, contact1_1,
+ contact1_2, normal1);
+ collision1 = (distance1 < 0);
+
+ hfield_witness_is_on_bin_side1 =
+ binCorrection(convex1, shape, tf2, distance1, contact1_1, contact1_2,
+ normal1, normal1_top, collision1);
+
+ if (RTIsIdentity)
+ nsolver->shapeDistance(convex2, Id, shape, tf2, distance2, contact2_1,
+ contact2_2, normal);
+ else
+ nsolver->shapeDistance(convex2, tf1, shape, tf2, distance2, contact2_1,
+ contact2_2, normal2);
+ collision2 = (distance2 < 0);
+
+ hfield_witness_is_on_bin_side2 =
+ binCorrection(convex2, 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;
+}
+
+template <typename Polygone, typename Shape, int Options>
+bool shapeCollision(const GJKSolver* nsolver, const Convex<Polygone>& convex1,
+ const Convex<Polygone>& convex2, const Transform3f& tf1,
+ const Shape& shape, const Transform3f& tf2,
+ FCL_REAL& distance_lower_bound, Vec3f& contact_point,
+ Vec3f& normal) {
+ enum { RTIsIdentity = Options & RelativeTransformationIsIdentity };
+
+ const Transform3f Id;
+ Vec3f contact_point2, normal2;
+ FCL_REAL distance_lower_bound2;
+ bool collision1, collision2;
+ if (RTIsIdentity)
+ collision1 =
+ nsolver->shapeIntersect(convex1, Id, shape, tf2, distance_lower_bound,
+ true, &contact_point, &normal);
+ else
+ collision1 =
+ nsolver->shapeIntersect(convex1, tf1, shape, tf2, distance_lower_bound,
+ true, &contact_point, &normal);
+
+ if (RTIsIdentity)
+ collision2 =
+ nsolver->shapeIntersect(convex2, Id, shape, tf2, distance_lower_bound2,
+ true, &contact_point2, &normal2);
+ else
+ collision2 =
+ nsolver->shapeIntersect(convex2, tf1, shape, tf2, distance_lower_bound2,
+ true, &contact_point2, &normal2);
+
+ if (collision1 && collision2) {
+ // In some case, EPA might returns something like
+ // -(std::numeric_limits<FCL_REAL>::max)().
+ if (distance_lower_bound != -(std::numeric_limits<FCL_REAL>::max)() &&
+ distance_lower_bound2 != -(std::numeric_limits<FCL_REAL>::max)()) {
+ if (distance_lower_bound > distance_lower_bound2) // switch values
+ {
+ distance_lower_bound = distance_lower_bound2;
+ contact_point = contact_point2;
+ normal = normal2;
+ }
+ } else if (distance_lower_bound2 !=
+ -(std::numeric_limits<FCL_REAL>::max)()) {
+ distance_lower_bound = distance_lower_bound2;
+ contact_point = contact_point2;
+ normal = normal2;
+ }
+ return true;
+ } else if (collision1) {
+ return true;
+ } else if (collision2) {
+ distance_lower_bound = distance_lower_bound2;
+ contact_point = contact_point2;
+ normal = normal2;
+ return true;
+ }
+
+ return false;
+}
} // namespace details
/// @brief Traversal node for collision between height field and shape
@@ -198,6 +422,7 @@ class HeightFieldShapeCollisionTraversalNode
nsolver = NULL;
shape_inflation.setZero();
+ count = 0;
}
/// @brief Whether the BV node in the first BVH tree is leaf
@@ -243,112 +468,10 @@ class HeightFieldShapeCollisionTraversalNode
return disjoint;
}
- template <typename Polygone>
- bool shapeDistance(const Convex<Polygone>& convex1,
- const Convex<Polygone>& convex2, const Transform3f& tf1,
- const S& shape, const Transform3f& tf2, FCL_REAL& distance,
- Vec3f& c1, Vec3f& c2, Vec3f& normal) const {
- const Transform3f Id;
- Vec3f contact2_1, contact2_2, normal2;
- FCL_REAL distance2;
- bool collision1, collision2;
- if (RTIsIdentity)
- collision1 = !nsolver->shapeDistance(convex1, Id, shape, tf2, distance,
- c1, c2, normal);
- else
- collision1 = !nsolver->shapeDistance(convex1, tf1, shape, tf2, distance,
- c1, c2, normal);
-
- if (RTIsIdentity)
- collision2 = !nsolver->shapeDistance(convex2, Id, shape, tf2, distance2,
- c1, c2, normal);
- else
- collision2 = !nsolver->shapeDistance(convex2, tf1, shape, tf2, distance2,
- contact2_1, contact2_2, normal2);
-
- if (collision1 && collision2) {
- if (distance > distance2) // switch values
- {
- distance = distance2;
- c1 = contact2_1;
- c2 = contact2_2;
- normal = normal2;
- }
- return true;
- } else if (collision1) {
- return true;
- } else if (collision2) {
- distance = distance2;
- c1 = contact2_1;
- c2 = contact2_2;
- normal = normal2;
- return true;
- }
-
- return false;
- }
-
- template <typename Polygone>
- bool shapeCollision(const Convex<Polygone>& convex1,
- const Convex<Polygone>& convex2, const Transform3f& tf1,
- const S& shape, const Transform3f& tf2,
- FCL_REAL& distance_lower_bound, Vec3f& contact_point,
- Vec3f& normal) const {
- const Transform3f Id;
- Vec3f contact_point2, normal2;
- FCL_REAL distance_lower_bound2;
- bool collision1, collision2;
- if (RTIsIdentity)
- collision1 =
- nsolver->shapeIntersect(convex1, Id, shape, tf2, distance_lower_bound,
- true, &contact_point, &normal);
- else
- collision1 = nsolver->shapeIntersect(convex1, tf1, shape, tf2,
- distance_lower_bound, true,
- &contact_point, &normal);
-
- if (RTIsIdentity)
- collision2 = nsolver->shapeIntersect(convex2, Id, shape, tf2,
- distance_lower_bound2, true,
- &contact_point2, &normal2);
- else
- collision2 = nsolver->shapeIntersect(convex2, tf1, shape, tf2,
- distance_lower_bound2, true,
- &contact_point2, &normal2);
-
- if (collision1 && collision2) {
- // In some case, EPA might returns something like
- // -(std::numeric_limits<FCL_REAL>::max)().
- if (distance_lower_bound != -(std::numeric_limits<FCL_REAL>::max)() &&
- distance_lower_bound2 != -(std::numeric_limits<FCL_REAL>::max)()) {
- if (distance_lower_bound > distance_lower_bound2) // switch values
- {
- distance_lower_bound = distance_lower_bound2;
- contact_point = contact_point2;
- normal = normal2;
- }
- } else if (distance_lower_bound2 !=
- -(std::numeric_limits<FCL_REAL>::max)()) {
- distance_lower_bound = distance_lower_bound2;
- contact_point = contact_point2;
- normal = normal2;
- }
- return true;
- } else if (collision1) {
- return true;
- } else if (collision2) {
- distance_lower_bound = distance_lower_bound2;
- contact_point = contact_point2;
- normal = normal2;
- return true;
- }
-
- return false;
- }
-
/// @brief Intersection testing between leaves (one Convex and one shape)
void leafCollides(unsigned int b1, unsigned int /*b2*/,
FCL_REAL& sqrDistLowerBound) const {
+ count++;
if (this->enable_statistics) this->num_leaf_tests++;
const HFNode<BV>& node = this->model1->getBV(b1);
@@ -363,33 +486,33 @@ class HeightFieldShapeCollisionTraversalNode
ConvexTriangle convex1, convex2;
details::buildConvexTriangles(node, *this->model1, convex1, convex2);
+ // Compute aabb_local for BoundingVolumeGuess case in the GJK solver
+ if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ convex1.computeLocalAABB();
+ convex2.computeLocalAABB();
+ }
+
FCL_REAL distance;
// Vec3f contact_point, normal;
- Vec3f c1, c2, normal;
+ Vec3f c1, c2, normal, normal_top;
+ bool hfield_witness_is_on_bin_side;
- bool collision =
- this->shapeDistance(convex1, convex2, this->tf1, *(this->model2),
- this->tf2, distance, c1, c2, normal);
+ bool collision = details::shapeDistance<Triangle, S, Options>(
+ nsolver, convex1, convex2, this->tf1, *(this->model2), this->tf2,
+ distance, c1, c2, normal, normal_top, hfield_witness_is_on_bin_side);
- // this->shapeCollision(convex1, convex2, this->tf1, *(this->model2),
- // this->tf2,
- // distance, contact_point, normal);
-
- FCL_REAL distToCollision = distance - this->request.security_margin;
+ FCL_REAL distToCollision =
+ distance - this->request.security_margin * (normal_top.dot(normal));
if (distToCollision <= this->request.collision_distance_threshold) {
sqrDistLowerBound = 0;
- if (this->request.num_max_contacts > this->result->numContacts()) {
- this->result->addContact(Contact(this->model1, this->model2, (int)b1,
- (int)Contact::NONE, .5 * (c1 + c2),
- (c2 - c1).normalized(), -distance));
- }
- } else if (collision && this->request.security_margin >= 0) {
- sqrDistLowerBound = 0;
- if (this->request.num_max_contacts > this->result->numContacts()) {
- this->result->addContact(Contact(this->model1, this->model2, (int)b1,
- (int)Contact::NONE, c1, normal,
- -distance));
- assert(this->result->isCollision());
+ if (this->result->numContacts() < this->request.num_max_contacts) {
+ if (normal_top.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;
@@ -397,7 +520,7 @@ class HeightFieldShapeCollisionTraversalNode
// const Vec3f c1 = contact_point - distance * 0.5 * normal;
// const Vec3f c2 = contact_point + distance * 0.5 * normal;
internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
- distToCollision, c1, c2);
+ distToCollision, c1, c2, normal);
assert(this->result->isCollision() || sqrDistLowerBound > 0);
}
@@ -413,6 +536,7 @@ class HeightFieldShapeCollisionTraversalNode
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
+ mutable int count;
};
/// @}
diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h
index 7aea1bfa9eacb7682f05ef7c5d5f6fa3d69dec8b..e1b5c8ed9a23ee21678ba5cccb39b7e2d4fd14dc 100644
--- a/include/hpp/fcl/internal/traversal_node_octree.h
+++ b/include/hpp/fcl/internal/traversal_node_octree.h
@@ -3,6 +3,7 @@
*
* 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
@@ -42,7 +43,9 @@
#include <hpp/fcl/collision_data.h>
#include <hpp/fcl/internal/traversal_node_base.h>
+#include <hpp/fcl/internal/traversal_node_hfield_shape.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
+#include <hpp/fcl/hfield.h>
#include <hpp/fcl/octree.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
@@ -148,6 +151,34 @@ class HPP_FCL_DLLAPI OcTreeSolver {
tree2->getRootBV(), tf1, tf2);
}
+ template <typename BV>
+ void OcTreeHeightFieldIntersect(
+ const OcTree* tree1, const HeightField<BV>* tree2, const Transform3f& tf1,
+ const Transform3f& tf2, const CollisionRequest& request_,
+ CollisionResult& result_, FCL_REAL& sqrDistLowerBound) const {
+ crequest = &request_;
+ cresult = &result_;
+
+ OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(),
+ tree1->getRootBV(), tree2, 0, tf1, tf2,
+ sqrDistLowerBound);
+ }
+
+ template <typename BV>
+ void HeightFieldOcTreeIntersect(const HeightField<BV>* tree1,
+ const OcTree* tree2, const Transform3f& tf1,
+ const Transform3f& tf2,
+ const CollisionRequest& request_,
+ CollisionResult& result_,
+ FCL_REAL& sqrDistLowerBound) const {
+ crequest = &request_;
+ cresult = &result_;
+
+ HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(),
+ tree2->getRootBV(), tf1, tf2,
+ sqrDistLowerBound);
+ }
+
/// @brief collision between octree and shape
template <typename S>
void OcTreeShapeIntersect(const OcTree* tree, const S& s,
@@ -225,6 +256,10 @@ class HPP_FCL_DLLAPI OcTreeSolver {
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ box.computeLocalAABB();
+ }
+
FCL_REAL dist;
Vec3f closest_p1, closest_p2, normal;
solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1, closest_p2,
@@ -294,6 +329,9 @@ class HPP_FCL_DLLAPI OcTreeSolver {
Box box;
Transform3f 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);
@@ -309,6 +347,8 @@ class HPP_FCL_DLLAPI OcTreeSolver {
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);
}
@@ -339,11 +379,16 @@ class HPP_FCL_DLLAPI OcTreeSolver {
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
- int primitive_id = tree2->getBV(root2).primitiveId();
- const Triangle& tri_id = tree2->tri_indices[primitive_id];
- const Vec3f& p1 = tree2->vertices[tri_id[0]];
- const Vec3f& p2 = tree2->vertices[tri_id[1]];
- const Vec3f& p3 = tree2->vertices[tri_id[2]];
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ box.computeLocalAABB();
+ }
+
+ size_t primitive_id =
+ static_cast<size_t>(tree2->getBV(root2).primitiveId());
+ const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
+ const Vec3f& p1 = (*(tree2->vertices))[tri_id[0]];
+ const Vec3f& p2 = (*(tree2->vertices))[tri_id[1]];
+ const Vec3f& p3 = (*(tree2->vertices))[tri_id[2]];
FCL_REAL dist;
Vec3f closest_p1, closest_p2, normal;
@@ -351,7 +396,8 @@ class HPP_FCL_DLLAPI OcTreeSolver {
closest_p1, closest_p2, normal);
dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()),
- primitive_id, closest_p1, closest_p2, normal);
+ static_cast<int>(primitive_id), closest_p1, closest_p2,
+ normal);
return drequest->isSatisfied(*dresult);
} else
@@ -451,34 +497,32 @@ class HPP_FCL_DLLAPI OcTreeSolver {
Box box;
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ box.computeLocalAABB();
+ }
- int primitive_id = bvn2.primitiveId();
- const Triangle& tri_id = tree2->tri_indices[primitive_id];
- const Vec3f& p1 = tree2->vertices[tri_id[0]];
- const Vec3f& p2 = tree2->vertices[tri_id[1]];
- const Vec3f& p3 = tree2->vertices[tri_id[2]];
+ size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
+ const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
+ const Vec3f& p1 = (*(tree2->vertices))[tri_id[0]];
+ const Vec3f& p2 = (*(tree2->vertices))[tri_id[1]];
+ const Vec3f& p3 = (*(tree2->vertices))[tri_id[2]];
Vec3f c1, c2, normal;
FCL_REAL distance;
- bool collision = solver->shapeTriangleInteraction(
- box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal);
+ solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, distance,
+ c1, c2, normal);
FCL_REAL distToCollision = distance - crequest->security_margin;
if (cresult->numContacts() < crequest->num_max_contacts) {
- if (collision) {
- cresult->addContact(Contact(tree1, tree2,
- (int)(root1 - tree1->getRoot()),
- primitive_id, c1, normal, -distance));
- } else if (distToCollision < 0) {
+ if (distToCollision <= crequest->collision_distance_threshold) {
cresult->addContact(Contact(
- tree1, tree2, (int)(root1 - tree1->getRoot()), primitive_id,
- .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
+ tree1, tree2, (int)(root1 - tree1->getRoot()),
+ static_cast<int>(primitive_id), c1, c2, normal, distance));
}
}
- internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult,
- distToCollision, c1, c2);
-
+ internal::updateDistanceLowerBoundFromLeaf(
+ *crequest, *cresult, distToCollision, c1, c2, normal);
return crequest->isSatisfied(*cresult);
}
@@ -509,6 +553,240 @@ class HPP_FCL_DLLAPI OcTreeSolver {
return false;
}
+ /// \return True if the request is satisfied.
+ template <typename BV>
+ bool OcTreeHeightFieldIntersectRecurse(
+ const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
+ const HeightField<BV>* tree2, unsigned int root2, const Transform3f& tf1,
+ const Transform3f& tf2, FCL_REAL& 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<BV> 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);
+ FCL_REAL 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;
+ Transform3f box_tf;
+ constructBox(bv1, tf1, box, box_tf);
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ box.computeLocalAABB();
+ }
+
+ typedef Convex<Triangle> ConvexTriangle;
+ ConvexTriangle convex1, convex2;
+ details::buildConvexTriangles(bvn2, *tree2, convex1, convex2);
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ convex1.computeLocalAABB();
+ convex2.computeLocalAABB();
+ }
+
+ Vec3f c1, c2, normal, normal_top;
+ FCL_REAL distance;
+ bool hfield_witness_is_on_bin_side;
+
+ bool collision = details::shapeDistance<Triangle, Box, 0>(
+ solver, convex1, convex2, tf2, box, box_tf, distance, c2, c1, normal,
+ normal_top, hfield_witness_is_on_bin_side);
+
+ FCL_REAL 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
+ sqrDistLowerBound = distToCollision * distToCollision;
+
+ // const Vec3f c1 = contact_point - distance * 0.5 * normal;
+ // const Vec3f 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 <typename BV>
+ bool HeightFieldOcTreeIntersectRecurse(
+ const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2,
+ const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1,
+ const Transform3f& tf2, FCL_REAL& 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<BV> 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);
+ FCL_REAL 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;
+ Transform3f box_tf;
+ constructBox(bv2, tf2, box, box_tf);
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ box.computeLocalAABB();
+ }
+
+ typedef Convex<Triangle> ConvexTriangle;
+ ConvexTriangle convex1, convex2;
+ details::buildConvexTriangles(bvn1, *tree1, convex1, convex2);
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ convex1.computeLocalAABB();
+ convex2.computeLocalAABB();
+ }
+
+ Vec3f c1, c2, normal, normal_top;
+ FCL_REAL distance;
+ bool hfield_witness_is_on_bin_side;
+
+ bool collision = details::shapeDistance<Triangle, Box, 0>(
+ solver, convex1, convex2, tf1, box, box_tf, distance, c1, c2, normal,
+ normal_top, hfield_witness_is_on_bin_side);
+
+ FCL_REAL 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
+ sqrDistLowerBound = distToCollision * distToCollision;
+
+ // const Vec3f c1 = contact_point - distance * 0.5 * normal;
+ // const Vec3f 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,
@@ -522,6 +800,11 @@ class HPP_FCL_DLLAPI OcTreeSolver {
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ box1.computeLocalAABB();
+ box2.computeLocalAABB();
+ }
+
FCL_REAL dist;
Vec3f closest_p1, closest_p2, normal;
solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist, closest_p1,
@@ -619,7 +902,7 @@ class HPP_FCL_DLLAPI OcTreeSolver {
sqrt(sqrDistLowerBound) - crequest->security_margin;
return false;
}
- if (!crequest->enable_contact) { // Overlap
+ if (crequest->enable_contact) { // Overlap
if (cresult->numContacts() < crequest->num_max_contacts)
cresult->addContact(
Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
@@ -637,26 +920,26 @@ class HPP_FCL_DLLAPI OcTreeSolver {
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);
+ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
+ box1.computeLocalAABB();
+ box2.computeLocalAABB();
+ }
+
FCL_REAL distance;
Vec3f c1, c2, normal;
- bool collision = solver->shapeDistance(box1, box1_tf, box2, box2_tf,
- distance, c1, c2, normal);
+ solver->shapeDistance(box1, box1_tf, box2, box2_tf, distance, c1, c2,
+ normal);
FCL_REAL distToCollision = distance - crequest->security_margin;
if (cresult->numContacts() < crequest->num_max_contacts) {
- if (collision)
- cresult->addContact(
- Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
- static_cast<int>(root2 - tree2->getRoot()), c1, normal,
- -distance));
- else if (distToCollision <= 0)
+ if (distToCollision <= crequest->collision_distance_threshold)
cresult->addContact(
Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
- static_cast<int>(root2 - tree2->getRoot()),
- .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
+ static_cast<int>(root2 - tree2->getRoot()), c1, c2,
+ normal, distance));
}
- internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult,
- distToCollision, c1, c2);
+ internal::updateDistanceLowerBoundFromLeaf(
+ *crequest, *cresult, distToCollision, c1, c2, normal);
return crequest->isSatisfied(*cresult);
}
@@ -840,7 +1123,6 @@ class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode
void leafCollides(unsigned int, unsigned int,
FCL_REAL& sqrDistLowerBound) const {
- std::cout << "leafCollides" << std::endl;
otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
sqrDistLowerBound *= sqrDistLowerBound;
@@ -854,6 +1136,68 @@ class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode
const OcTreeSolver* otsolver;
};
+/// @brief Traversal node for octree-height-field collision
+template <typename BV>
+class HPP_FCL_DLLAPI OcTreeHeightFieldCollisionTraversalNode
+ : public CollisionTraversalNodeBase {
+ public:
+ OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
+ : CollisionTraversalNodeBase(request) {
+ model1 = NULL;
+ model2 = NULL;
+
+ otsolver = NULL;
+ }
+
+ bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
+ return false;
+ }
+
+ void leafCollides(unsigned int, unsigned int,
+ FCL_REAL& sqrDistLowerBound) const {
+ otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request,
+ *result, sqrDistLowerBound);
+ }
+
+ const OcTree* model1;
+ const HeightField<BV>* model2;
+
+ Transform3f tf1, tf2;
+
+ const OcTreeSolver* otsolver;
+};
+
+/// @brief Traversal node for octree-height-field collision
+template <typename BV>
+class HPP_FCL_DLLAPI HeightFieldOcTreeCollisionTraversalNode
+ : public CollisionTraversalNodeBase {
+ public:
+ HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request)
+ : CollisionTraversalNodeBase(request) {
+ model1 = NULL;
+ model2 = NULL;
+
+ otsolver = NULL;
+ }
+
+ bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
+ return false;
+ }
+
+ void leafCollides(unsigned int, unsigned int,
+ FCL_REAL& sqrDistLowerBound) const {
+ otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request,
+ *result, sqrDistLowerBound);
+ }
+
+ const HeightField<BV>* model1;
+ const OcTree* model2;
+
+ Transform3f tf1, tf2;
+
+ const OcTreeSolver* otsolver;
+};
+
/// @}
/// @addtogroup Traversal_For_Distance
diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h
index e73512634650ed9bea731651e8c3bbbc4ba586d5..545b5fd728aafef0664245618a564609210c54e1 100644
--- a/include/hpp/fcl/internal/traversal_node_setup.h
+++ b/include/hpp/fcl/internal/traversal_node_setup.h
@@ -222,6 +222,46 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
return true;
}
+/// @brief Initialize traversal node for collision between one octree and one
+/// height field, given current object transform
+template <typename BV>
+bool initialize(OcTreeHeightFieldCollisionTraversalNode<BV>& node,
+ const OcTree& model1, const Transform3f& tf1,
+ const HeightField<BV>& model2, const Transform3f& 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 <typename BV>
+bool initialize(HeightFieldOcTreeCollisionTraversalNode<BV>& node,
+ const HeightField<BV>& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& 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 <typename BV>
@@ -298,11 +338,13 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
std::invalid_argument)
- if (!tf1.isIdentity()) // TODO(jcarpent): vectorized version
+ if (!tf1.isIdentity() &&
+ model1.vertices.get()) // TODO(jcarpent): vectorized version
{
std::vector<Vec3f> vertices_transformed(model1.num_vertices);
+ const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
for (unsigned int i = 0; i < model1.num_vertices; ++i) {
- const Vec3f& p = model1.vertices[i];
+ const Vec3f& p = model1_vertices_[i];
Vec3f new_v = tf1.transform(p);
vertices_transformed[i] = new_v;
}
@@ -322,8 +364,9 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
computeBV(model2, tf2, node.model2_bv);
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
+ node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+ node.tri_indices =
+ model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
node.result = &result;
@@ -350,8 +393,9 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
computeBV(model2, tf2, node.model2_bv);
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
+ node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+ node.tri_indices =
+ model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
node.result = &result;
@@ -407,8 +451,9 @@ static inline bool setupShapeMeshCollisionOrientedNode(
computeBV(model1, tf1, node.model1_bv);
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
+ node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
+ node.tri_indices =
+ model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
node.result = &result;
@@ -434,10 +479,11 @@ bool initialize(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
std::invalid_argument)
- if (!tf1.isIdentity()) {
+ if (!tf1.isIdentity() && model1.vertices.get()) {
std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
+ const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
for (unsigned int i = 0; i < model1.num_vertices; ++i) {
- Vec3f& p = model1.vertices[i];
+ const Vec3f& p = model1_vertices_[i];
Vec3f new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
@@ -449,10 +495,11 @@ bool initialize(
tf1.setIdentity();
}
- if (!tf2.isIdentity()) {
+ if (!tf2.isIdentity() && model2.vertices.get()) {
std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
+ const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
for (unsigned int i = 0; i < model2.num_vertices; ++i) {
- Vec3f& p = model2.vertices[i];
+ const Vec3f& p = model2_vertices_[i];
Vec3f new_v = tf2.transform(p);
vertices_transformed2[i] = new_v;
}
@@ -469,11 +516,13 @@ bool initialize(
node.model2 = &model2;
node.tf2 = tf2;
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
+ node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+ node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
+ 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;
@@ -494,11 +543,13 @@ bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
std::invalid_argument)
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
+ node.vertices1 = model1.vertices ? model1.vertices->data() : NULL;
+ node.vertices2 = model2.vertices ? model2.vertices->data() : NULL;
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
+ 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;
@@ -549,10 +600,11 @@ bool initialize(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
std::invalid_argument)
- if (!tf1.isIdentity()) {
+ if (!tf1.isIdentity() && model1.vertices.get()) {
std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
+ const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
for (unsigned int i = 0; i < model1.num_vertices; ++i) {
- const Vec3f& p = model1.vertices[i];
+ const Vec3f& p = model1_vertices_[i];
Vec3f new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
@@ -564,10 +616,11 @@ bool initialize(
tf1.setIdentity();
}
- if (!tf2.isIdentity()) {
+ if (!tf2.isIdentity() && model2.vertices.get()) {
std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
+ const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
for (unsigned int i = 0; i < model2.num_vertices; ++i) {
- const Vec3f& p = model2.vertices[i];
+ const Vec3f& p = model2_vertices_[i];
Vec3f new_v = tf2.transform(p);
vertices_transformed2[i] = new_v;
}
@@ -587,11 +640,13 @@ bool initialize(
node.model2 = &model2;
node.tf2 = tf2;
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
+ node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+ node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
+ 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;
}
@@ -619,11 +674,13 @@ bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
node.model2 = &model2;
node.tf2 = tf2;
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
+ node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
+ node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
+ node.tri_indices1 =
+ model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
+ node.tri_indices2 =
+ model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
tf2.getTranslation(), node.RT.R, node.RT.T);
@@ -644,10 +701,11 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
std::invalid_argument)
- if (!tf1.isIdentity()) {
+ if (!tf1.isIdentity() && model1.vertices.get()) {
+ const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
for (unsigned int i = 0; i < model1.num_vertices; ++i) {
- const Vec3f& p = model1.vertices[i];
+ const Vec3f& p = model1_vertices_[i];
Vec3f new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
@@ -668,8 +726,9 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
node.tf2 = tf2;
node.nsolver = nsolver;
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
+ 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);
@@ -689,10 +748,11 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV>& node, const S& model1,
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
std::invalid_argument)
- if (!tf2.isIdentity()) {
+ if (!tf2.isIdentity() && model2.vertices.get()) {
std::vector<Vec3f> vertices_transformed(model2.num_vertices);
+ const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
for (unsigned int i = 0; i < model2.num_vertices; ++i) {
- const Vec3f& p = model2.vertices[i];
+ const Vec3f& p = model2_vertices_[i];
Vec3f new_v = tf2.transform(p);
vertices_transformed[i] = new_v;
}
@@ -713,8 +773,9 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV>& node, const S& model1,
node.tf2 = tf2;
node.nsolver = nsolver;
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
+ node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
+ node.tri_indices =
+ model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
computeBV(model1, tf1, node.model1_bv);
@@ -745,8 +806,9 @@ static inline bool setupMeshShapeDistanceOrientedNode(
computeBV(model2, tf2, node.model2_bv);
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
+ node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
+ node.tri_indices =
+ model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
return true;
}
@@ -812,8 +874,9 @@ static inline bool setupShapeMeshDistanceOrientedNode(
computeBV(model1, tf1, node.model1_bv);
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
+ node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
+ node.tri_indices =
+ model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
node.R = tf2.getRotation();
node.T = tf2.getTranslation();
diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h
index 9b4cec0a6ff99362025646d13157e2a945fb29df..9db5b5a75497e2166a5634e2959f712558cda442 100644
--- a/include/hpp/fcl/math/transform.h
+++ b/include/hpp/fcl/math/transform.h
@@ -38,14 +38,16 @@
#ifndef HPP_FCL_TRANSFORM_H
#define HPP_FCL_TRANSFORM_H
+#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/data_types.h>
namespace hpp {
namespace fcl {
-typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
+HPP_FCL_DEPRECATED typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
+typedef Eigen::Quaternion<FCL_REAL> Quatf;
-static inline std::ostream& operator<<(std::ostream& o, const Quaternion3f& q) {
+static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) {
o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
return o;
}
@@ -74,14 +76,14 @@ class HPP_FCL_DLLAPI Transform3f {
/// @brief Construct transform from rotation and translation
template <typename Vector3Like>
- Transform3f(const Quaternion3f& q_, const Eigen::MatrixBase<Vector3Like>& T_)
+ Transform3f(const Quatf& q_, const Eigen::MatrixBase<Vector3Like>& T_)
: R(q_.toRotationMatrix()), T(T_) {}
/// @brief Construct transform from rotation
Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {}
/// @brief Construct transform from rotation
- Transform3f(const Quaternion3f& q_) : R(q_), T(Vec3f::Zero()) {}
+ Transform3f(const Quatf& q_) : R(q_), T(Vec3f::Zero()) {}
/// @brief Construct transform from translation
Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {}
@@ -115,7 +117,7 @@ class HPP_FCL_DLLAPI Transform3f {
inline Matrix3f& rotation() { return R; }
/// @brief get quaternion
- inline Quaternion3f getQuatRotation() const { return Quaternion3f(R); }
+ inline Quatf getQuatRotation() const { return Quatf(R); }
/// @brief set transform from rotation and translation
template <typename Matrix3Like, typename Vector3Like>
@@ -126,7 +128,7 @@ class HPP_FCL_DLLAPI Transform3f {
}
/// @brief set transform from rotation and translation
- inline void setTransform(const Quaternion3f& q_, const Vec3f& T_) {
+ inline void setTransform(const Quatf& q_, const Vec3f& T_) {
R = q_.toRotationMatrix();
T = T_;
}
@@ -144,9 +146,7 @@ class HPP_FCL_DLLAPI Transform3f {
}
/// @brief set transform from rotation
- inline void setQuatRotation(const Quaternion3f& q_) {
- R = q_.toRotationMatrix();
- }
+ inline void setQuatRotation(const Quatf& q_) { R = q_.toRotationMatrix(); }
/// @brief transform a given vector by the transform
template <typename Derived>
@@ -206,9 +206,9 @@ class HPP_FCL_DLLAPI Transform3f {
};
template <typename Derived>
-inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
- FCL_REAL angle) {
- return Quaternion3f(Eigen::AngleAxis<FCL_REAL>(angle, axis));
+inline Quatf fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
+ FCL_REAL angle) {
+ return Quatf(Eigen::AngleAxis<FCL_REAL>(angle, axis));
}
} // namespace fcl
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index cce65d0b9cc370bee340e7d3463c076680fe2889..282a0f47969ad1862d378a50e67bd3a5d3a60169 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -3,6 +3,7 @@
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * Copyright (c) 2021-2022, INRIA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -162,6 +163,8 @@ struct HPP_FCL_DLLAPI GJK {
/// Valid: GJK converged and the shapes are not in collision.
/// Inside: GJK converged and the shapes are in collision.
/// Failed: GJK did not converge.
+ /// EarlyStopped: GJK found a separating hyperplane and exited before
+ /// converting. The shapes are not in collision.
enum Status { Valid, Inside, Failed, EarlyStopped };
MinkowskiDiff const* shape;
@@ -252,10 +255,9 @@ struct HPP_FCL_DLLAPI GJK {
const FCL_REAL& omega);
/// @brief Get GJK number of iterations.
- inline size_t getIterations() { return iterations; }
-
- /// @brief Get GJK tolerance.
- inline FCL_REAL getTolerance() { return tolerance; }
+ inline int getIterationsMomentumStopped() const {
+ return iterations_momentum_stop;
+ }
private:
SimplexV store_v[4];
@@ -266,10 +268,14 @@ struct HPP_FCL_DLLAPI GJK {
Status status;
unsigned int max_iterations;
- FCL_REAL tolerance;
FCL_REAL distance_upper_bound;
+ int iterations_momentum_stop;
+
+ public:
+ FCL_REAL tolerance;
size_t iterations;
+ private:
/// @brief discard one vertex from the simplex
inline void removeVertex(Simplex& simplex);
@@ -290,8 +296,8 @@ struct HPP_FCL_DLLAPI GJK {
/// 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.
+ /// 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
@@ -303,11 +309,6 @@ struct HPP_FCL_DLLAPI GJK {
bool projectTetrahedraOrigin(const Simplex& current, Simplex& next);
};
-static const size_t EPA_MAX_FACES = 128;
-static const size_t EPA_MAX_VERTICES = 64;
-static const FCL_REAL EPA_EPS = 0.000001;
-static const size_t EPA_MAX_ITERATIONS = 255;
-
/// @brief class for EPA algorithm
struct HPP_FCL_DLLAPI EPA {
typedef GJK::SimplexV SimplexV;
@@ -357,11 +358,13 @@ struct HPP_FCL_DLLAPI EPA {
SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
};
- private:
- unsigned int max_face_num;
- unsigned int max_vertex_num;
- unsigned int max_iterations;
+ public:
+ size_t max_face_num;
+ size_t max_vertex_num;
+ size_t max_iterations;
+
FCL_REAL tolerance;
+ size_t iterations;
public:
enum Status {
@@ -385,8 +388,8 @@ struct HPP_FCL_DLLAPI EPA {
size_t nextsv;
SimplexList hull, stock;
- EPA(unsigned int max_face_num_, unsigned int max_vertex_num_,
- unsigned int max_iterations_, FCL_REAL tolerance_)
+ EPA(size_t max_face_num_, size_t max_vertex_num_, size_t max_iterations_,
+ FCL_REAL tolerance_)
: max_face_num(max_face_num_),
max_vertex_num(max_vertex_num_),
max_iterations(max_iterations_),
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 519f3aecca13c0f72d33252a5ec9ea9e7dadc532..daa72bf5661a0141d68b279a671134eb9c370448 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -5,7 +5,7 @@
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
* All rights reserved.
- * Copyright (c) 2021-2022, INRIA
+ * 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
@@ -132,7 +132,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
gjk.getClosestPoints(shape, w0, w1);
distance_lower_bound = gjk.distance;
if (normal)
- (*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized();
+ (*normal).noalias() = tf1.getRotation() * (w1 - w0).normalized();
if (contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
return true;
} else {
@@ -213,7 +213,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
gjk.getClosestPoints(shape, w0, w1);
distance = gjk.distance;
normal.noalias() = tf1.getRotation() * (w0 - w1).normalized();
- p1 = p2 = tf1.transform((w0 + w1) / 2);
+ p1 = tf1.transform(w0);
+ p2 = tf1.transform(w1);
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
@@ -225,17 +226,27 @@ struct HPP_FCL_DLLAPI GJKSolver {
epa.getClosestPoints(shape, w0, w1);
distance = -epa.depth;
normal.noalias() = tf1.getRotation() * epa.normal;
- p1 = p2 = tf1.transform(w0 - epa.normal * (epa.depth * 0.5));
+ p1 = tf1.transform(w0);
+ p2 = tf1.transform(w1);
assert(distance <= 1e-6);
} else {
distance = -(std::numeric_limits<FCL_REAL>::max)();
gjk.getClosestPoints(shape, w0, w1);
- p1 = p2 = tf1.transform(w0);
+ p1 = tf1.transform(w0);
+ p2 = tf1.transform(w1);
}
}
break;
case details::GJK::Valid:
case details::GJK::EarlyStopped:
+ col = false;
+ gjk.getClosestPoints(shape, w0, w1);
+ distance = gjk.distance;
+ normal.noalias() = -tf1.getRotation() * gjk.ray;
+ normal.normalize();
+ p1 = tf1.transform(w0);
+ p2 = tf1.transform(w1);
+ break;
case details::GJK::Failed:
col = false;
@@ -247,11 +258,13 @@ struct HPP_FCL_DLLAPI GJKSolver {
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
+ normal.setZero();
assert(distance > 0);
break;
default:
assert(false && "should not reach type part.");
- throw std::logic_error("GJKSolver: should not reach this part.");
+ HPP_FCL_THROW_PRETTY("GJKSolver: should not reach this part.",
+ std::logic_error)
}
return col;
}
@@ -296,7 +309,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
// assert (distance == (w0 - w1).norm());
distance = gjk.distance;
- normal.noalias() = tf1.getRotation() * gjk.ray;
+ normal.noalias() = -tf1.getRotation() * gjk.ray;
normal.normalize();
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
@@ -318,6 +331,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
normal.normalize();
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
+ return true;
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
@@ -333,7 +347,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
normal.noalias() = tf1.getRotation() * epa.normal;
p1 = tf1.transform(w0);
p2 = tf1.transform(w1);
- return false;
+ return true;
}
distance = -(std::numeric_limits<FCL_REAL>::max)();
gjk.getClosestPoints(shape, p1, p2);
@@ -373,12 +387,6 @@ struct HPP_FCL_DLLAPI GJKSolver {
support_func_cached_guess = support_func_guess_t::Zero();
distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
- // EPS settings
- epa_max_face_num = 128;
- epa_max_vertex_num = 64;
- epa_max_iterations = 255;
- epa_tolerance = 1e-6;
-
set(request);
}
@@ -400,6 +408,12 @@ struct HPP_FCL_DLLAPI GJKSolver {
cached_guess = request.cached_gjk_guess;
support_func_cached_guess = request.cached_support_func_guess;
}
+
+ // EPA settings
+ epa_max_face_num = request.epa_max_face_num;
+ epa_max_vertex_num = request.epa_max_vertex_num;
+ epa_max_iterations = request.epa_max_iterations;
+ epa_tolerance = request.epa_tolerance;
}
/// @brief Constructor from a CollisionRequest
@@ -411,12 +425,6 @@ struct HPP_FCL_DLLAPI GJKSolver {
support_func_cached_guess = support_func_guess_t::Zero();
distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
- // EPS settings
- epa_max_face_num = 128;
- epa_max_vertex_num = 64;
- epa_max_iterations = 255;
- epa_tolerance = 1e-6;
-
set(request);
}
@@ -443,6 +451,12 @@ struct HPP_FCL_DLLAPI GJKSolver {
// security margin. Otherwise, we will likely miss some collisions.
distance_upper_bound = (std::max)(
0., (std::max)(request.distance_upper_bound, request.security_margin));
+
+ // EPA settings
+ epa_max_face_num = request.epa_max_face_num;
+ epa_max_vertex_num = request.epa_max_vertex_num;
+ epa_max_iterations = request.epa_max_iterations;
+ epa_tolerance = request.epa_tolerance;
}
/// @brief Copy constructor
@@ -473,16 +487,16 @@ struct HPP_FCL_DLLAPI GJKSolver {
bool operator!=(const GJKSolver& other) const { return !(*this == other); }
/// @brief maximum number of simplex face used in EPA algorithm
- unsigned int epa_max_face_num;
+ mutable size_t epa_max_face_num;
/// @brief maximum number of simplex vertex used in EPA algorithm
- unsigned int epa_max_vertex_num;
+ mutable size_t epa_max_vertex_num;
/// @brief maximum number of iterations used for EPA iterations
- unsigned int epa_max_iterations;
+ mutable size_t epa_max_iterations;
/// @brief the threshold used in EPA to stop iteration
- FCL_REAL epa_tolerance;
+ mutable FCL_REAL epa_tolerance;
/// @brief the threshold used in GJK to stop iteration
mutable FCL_REAL gjk_tolerance;
diff --git a/include/hpp/fcl/octree.h b/include/hpp/fcl/octree.h
index 19d9d1fd8787b7676ceb7dc2343345d1c998b3f5..5f45c342000e49ced16f2bcf0a0670c82399e9a0 100644
--- a/include/hpp/fcl/octree.h
+++ b/include/hpp/fcl/octree.h
@@ -3,6 +3,7 @@
*
* 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
@@ -38,7 +39,7 @@
#ifndef HPP_FCL_OCTREE_H
#define HPP_FCL_OCTREE_H
-#include <boost/array.hpp>
+#include <algorithm>
#include <octomap/octomap.h>
#include <hpp/fcl/fwd.hh>
@@ -51,7 +52,7 @@ namespace fcl {
/// @brief Octree is one type of collision geometry which can encode uncertainty
/// information in the sensor data.
class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
- private:
+ protected:
shared_ptr<const octomap::OcTree> tree;
FCL_REAL default_occupancy;
@@ -85,6 +86,7 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
free_threshold = 0;
}
+ /// \brief Copy constructor
OcTree(const OcTree& other)
: CollisionGeometry(other),
tree(other.tree),
@@ -92,13 +94,38 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
occupancy_threshold(other.occupancy_threshold),
free_threshold(other.free_threshold) {}
+ /// \brief Clone *this into a new Octree
OcTree* clone() const { return new OcTree(*this); }
+ /// \brief Returns the tree associated to the underlying octomap OcTree.
+ shared_ptr<const octomap::OcTree> 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() {
- aabb_local = getRootBV();
+ typedef Eigen::Matrix<float, 3, 1> 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 = min_extent = Eigen::Map<Vec3float>(&it.getCoordinate().x());
+ for (++it; it != end; ++it) {
+ Eigen::Map<Vec3float> pos(&it.getCoordinate().x());
+ max_extent = max_extent.array().max(pos.array());
+ min_extent = min_extent.array().min(pos.array());
+ }
+
+ // Account for the size of the boxes.
+ const FCL_REAL resolution = tree->getResolution();
+ max_extent.array() += float(resolution / 2.);
+ min_extent.array() -= float(resolution / 2.);
+
+ aabb_local = AABB(min_extent.cast<FCL_REAL>(), max_extent.cast<FCL_REAL>());
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
@@ -111,9 +138,15 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
}
- /// @brief Returns the depth of octree
+ /// @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
+ FCL_REAL getResolution() const { return tree->getResolution(); }
+
/// @brief get the root node of the octree
OcTreeNode* getRoot() const { return tree->getRoot(); }
@@ -137,8 +170,8 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
/// @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<boost::array<FCL_REAL, 6> > toBoxes() const {
- std::vector<boost::array<FCL_REAL, 6> > boxes;
+ std::vector<Vec6f> toBoxes() const {
+ std::vector<Vec6f> boxes;
boxes.reserve(tree->size() / 2);
for (octomap::OcTree::iterator
it = tree->begin((unsigned char)tree->getTreeDepth()),
@@ -146,20 +179,42 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
it != end; ++it) {
// if(tree->isNodeOccupied(*it))
if (isNodeOccupied(&*it)) {
- FCL_REAL size = it.getSize();
FCL_REAL x = it.getX();
FCL_REAL y = it.getY();
FCL_REAL z = it.getZ();
+ FCL_REAL size = it.getSize();
FCL_REAL c = (*it).getOccupancy();
FCL_REAL t = tree->getOccupancyThres();
- boost::array<FCL_REAL, 6> box = {{x, y, z, size, c, t}};
+ Vec6f box;
+ box << x, y, z, size, c, t;
boxes.push_back(box);
}
}
return boxes;
}
+ /// \brief Returns a byte description of *this
+ std::vector<uint8_t> tobytes() const {
+ typedef Eigen::Matrix<float, 3, 1> Vec3float;
+ const size_t total_size = (tree->size() * sizeof(FCL_REAL) * 3) / 2;
+ std::vector<uint8_t> bytes;
+ bytes.reserve(total_size);
+
+ for (octomap::OcTree::iterator
+ it = tree->begin((unsigned char)tree->getTreeDepth()),
+ end = tree->end();
+ it != end; ++it) {
+ const Vec3f box_pos =
+ Eigen::Map<Vec3float>(&it.getCoordinate().x()).cast<FCL_REAL>();
+ if (isNodeOccupied(&*it))
+ std::copy(box_pos.data(), box_pos.data() + sizeof(FCL_REAL) * 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
FCL_REAL getOccupancyThres() const { return occupancy_threshold; }
@@ -225,7 +280,7 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
if (other_ptr == nullptr) return false;
const OcTree& other = *other_ptr;
- return tree.get() == other.tree.get() &&
+ return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) &&
default_occupancy == other.default_occupancy &&
occupancy_threshold == other.occupancy_threshold &&
free_threshold == other.free_threshold;
diff --git a/include/hpp/fcl/serialization/BVH_model.h b/include/hpp/fcl/serialization/BVH_model.h
index d48d20b2ac2997ee21a50a9c4e9835122ef4fa18..47e4fb787a2ff31a19d75423fe25859a6ae07874 100644
--- a/include/hpp/fcl/serialization/BVH_model.h
+++ b/include/hpp/fcl/serialization/BVH_model.h
@@ -32,10 +32,11 @@ void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model,
if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED ||
bvh_model.build_state == BVH_BUILD_STATE_UPDATED) &&
(bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) {
- throw std::invalid_argument(
+ HPP_FCL_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.");
+ "The BVHModel could not be serialized.",
+ std::invalid_argument);
}
ar &make_nvp("base",
@@ -43,37 +44,13 @@ void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model,
bvh_model));
ar &make_nvp("num_vertices", bvh_model.num_vertices);
- if (bvh_model.num_vertices > 0) {
- typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> AsVertixMatrix;
- const Eigen::Map<const AsVertixMatrix> vertices_map(
- reinterpret_cast<const double *>(bvh_model.vertices), 3,
- bvh_model.num_vertices);
- ar &make_nvp("vertices", vertices_map);
- }
+ ar &make_nvp("vertices", bvh_model.vertices);
ar &make_nvp("num_tris", bvh_model.num_tris);
- if (bvh_model.num_tris > 0) {
- typedef Eigen::Matrix<Triangle::index_type, 3, Eigen::Dynamic>
- AsTriangleMatrix;
- const Eigen::Map<const AsTriangleMatrix> tri_indices_map(
- reinterpret_cast<const Triangle::index_type *>(bvh_model.tri_indices),
- 3, bvh_model.num_tris);
- ar &make_nvp("tri_indices", tri_indices_map);
- }
+ ar &make_nvp("tri_indices", bvh_model.tri_indices);
ar &make_nvp("build_state", bvh_model.build_state);
- if (bvh_model.prev_vertices) {
- const bool has_prev_vertices = true;
- ar << make_nvp("has_prev_vertices", has_prev_vertices);
- typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> AsVertixMatrix;
- const Eigen::Map<const AsVertixMatrix> prev_vertices_map(
- reinterpret_cast<const double *>(bvh_model.prev_vertices), 3,
- bvh_model.num_vertices);
- ar &make_nvp("prev_vertices", prev_vertices_map);
- } else {
- const bool has_prev_vertices = false;
- ar &make_nvp("has_prev_vertices", has_prev_vertices);
- }
+ ar &make_nvp("prev_vertices", bvh_model.prev_vertices);
// if(bvh_model.convex)
// {
@@ -96,65 +73,14 @@ void load(Archive &ar, hpp::fcl::BVHModelBase &bvh_model,
boost::serialization::base_object<hpp::fcl::CollisionGeometry>(
bvh_model));
- unsigned int num_vertices;
- ar >> make_nvp("num_vertices", num_vertices);
- if (num_vertices != bvh_model.num_vertices) {
- delete[] bvh_model.vertices;
- bvh_model.vertices = NULL;
- bvh_model.num_vertices = num_vertices;
- if (num_vertices > 0) bvh_model.vertices = new Vec3f[num_vertices];
- }
- if (num_vertices > 0) {
- typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> AsVertixMatrix;
- Eigen::Map<AsVertixMatrix> vertices_map(
- reinterpret_cast<double *>(bvh_model.vertices), 3,
- bvh_model.num_vertices);
- ar >> make_nvp("vertices", vertices_map);
- } else
- bvh_model.vertices = NULL;
-
- unsigned int num_tris;
- ar >> make_nvp("num_tris", num_tris);
-
- if (num_tris != bvh_model.num_tris) {
- delete[] bvh_model.tri_indices;
- bvh_model.tri_indices = NULL;
- bvh_model.num_tris = num_tris;
- if (num_tris > 0) bvh_model.tri_indices = new Triangle[num_tris];
- }
- if (num_tris > 0) {
- typedef Eigen::Matrix<Triangle::index_type, 3, Eigen::Dynamic>
- AsTriangleMatrix;
- Eigen::Map<AsTriangleMatrix> tri_indices_map(
- reinterpret_cast<Triangle::index_type *>(bvh_model.tri_indices), 3,
- bvh_model.num_tris);
- ar &make_nvp("tri_indices", tri_indices_map);
- } else
- bvh_model.tri_indices = NULL;
+ 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);
- typedef internal::BVHModelBaseAccessor Accessor;
- reinterpret_cast<Accessor &>(bvh_model).num_tris_allocated = num_tris;
- reinterpret_cast<Accessor &>(bvh_model).num_vertices_allocated = num_vertices;
-
- bool has_prev_vertices;
- ar >> make_nvp("has_prev_vertices", has_prev_vertices);
- if (has_prev_vertices) {
- if (num_vertices != bvh_model.num_vertices) {
- delete[] bvh_model.prev_vertices;
- bvh_model.prev_vertices = NULL;
- if (num_vertices > 0) bvh_model.prev_vertices = new Vec3f[num_vertices];
- }
- if (num_vertices > 0) {
- typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> AsVertixMatrix;
- Eigen::Map<AsVertixMatrix> prev_vertices_map(
- reinterpret_cast<double *>(bvh_model.prev_vertices), 3,
- bvh_model.num_vertices);
- ar &make_nvp("prev_vertices", prev_vertices_map);
- }
- } else
- bvh_model.prev_vertices = NULL;
+ ar >> make_nvp("prev_vertices", bvh_model.prev_vertices);
// bool has_convex = true;
// ar >> make_nvp("has_convex",has_convex);
@@ -228,14 +154,14 @@ void save(Archive &ar, const hpp::fcl::BVHModel<BV> &bvh_model_,
// }
//
- if (bvh_model.bvs) {
+ 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<const char *>(bvh_model.bvs),
+ reinterpret_cast<const char *>(bvh_model.bvs->data()),
sizeof(Node) *
(std::size_t)bvh_model.num_bvs)); // Assuming BVs are POD.
} else {
@@ -281,16 +207,18 @@ void load(Archive &ar, hpp::fcl::BVHModel<BV> &bvh_model_,
ar >> make_nvp("num_bvs", num_bvs);
if (num_bvs != bvh_model.num_bvs) {
- delete[] bvh_model.bvs;
- bvh_model.bvs = NULL;
+ bvh_model.bvs.reset();
bvh_model.num_bvs = num_bvs;
- if (num_bvs > 0) bvh_model.bvs = new BVNode<BV>[num_bvs];
+ if (num_bvs > 0)
+ bvh_model.bvs.reset(new
+ typename BVHModel<BV>::bv_node_vector_t(num_bvs));
}
if (num_bvs > 0) {
- ar >> make_nvp("bvs", make_array(reinterpret_cast<char *>(bvh_model.bvs),
- sizeof(Node) * (std::size_t)num_bvs));
+ ar >> make_nvp("bvs",
+ make_array(reinterpret_cast<char *>(bvh_model.bvs->data()),
+ sizeof(Node) * (std::size_t)num_bvs));
} else
- bvh_model.bvs = NULL;
+ bvh_model.bvs.reset();
}
}
@@ -302,7 +230,7 @@ namespace fcl {
namespace internal {
template <typename BV>
-struct memory_footprint_evaluator< ::hpp::fcl::BVHModel<BV> > {
+struct memory_footprint_evaluator<::hpp::fcl::BVHModel<BV>> {
static size_t run(const ::hpp::fcl::BVHModel<BV> &bvh_model) {
return static_cast<size_t>(bvh_model.memUsage(false));
}
@@ -312,4 +240,13 @@ struct memory_footprint_evaluator< ::hpp::fcl::BVHModel<BV> > {
} // namespace fcl
} // namespace hpp
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::AABB>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::OBB>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::RSS>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::OBBRSS>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::kIOS>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<16>>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<18>>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<24>>)
+
#endif // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H
diff --git a/include/hpp/fcl/serialization/collision_data.h b/include/hpp/fcl/serialization/collision_data.h
index a40f532796438a2a8ac717dfc812930d6f3b4681..c399780855213a2461db5456a9f91b6e03977118 100644
--- a/include/hpp/fcl/serialization/collision_data.h
+++ b/include/hpp/fcl/serialization/collision_data.h
@@ -17,6 +17,7 @@ void save(Archive& ar, const hpp::fcl::Contact& contact,
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);
}
@@ -27,6 +28,10 @@ void load(Archive& ar, hpp::fcl::Contact& contact,
ar >> make_nvp("b1", contact.b1);
ar >> make_nvp("b2", contact.b2);
ar >> make_nvp("normal", contact.normal);
+ std::array<hpp::fcl::Vec3f, 2> 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;
@@ -71,6 +76,7 @@ void serialize(Archive& ar, hpp::fcl::CollisionRequest& collision_request,
collision_request.enable_distance_lower_bound);
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 <class Archive>
@@ -80,6 +86,8 @@ void save(Archive& ar, const hpp::fcl::CollisionResult& collision_result,
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 <class Archive>
@@ -94,6 +102,11 @@ void load(Archive& ar, hpp::fcl::CollisionResult& collision_result,
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<hpp::fcl::Vec3f, 2> 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);
}
HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionResult)
@@ -115,7 +128,7 @@ void save(Archive& ar, const hpp::fcl::DistanceResult& distance_result,
ar& make_nvp("base", boost::serialization::base_object<hpp::fcl::QueryResult>(
distance_result));
ar& make_nvp("min_distance", distance_result.min_distance);
- ar& make_nvp("nearest_points", make_array(distance_result.nearest_points, 2));
+ 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);
@@ -128,8 +141,10 @@ void load(Archive& ar, hpp::fcl::DistanceResult& distance_result,
make_nvp("base", boost::serialization::base_object<hpp::fcl::QueryResult>(
distance_result));
ar >> make_nvp("min_distance", distance_result.min_distance);
- ar >>
- make_nvp("nearest_points", make_array(distance_result.nearest_points, 2));
+ std::array<hpp::fcl::Vec3f, 2> 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);
diff --git a/include/hpp/fcl/serialization/collision_object.h b/include/hpp/fcl/serialization/collision_object.h
index 4264b38e353fd1df43ba387152e139fe4246ddd9..018728cf57af1e0e67f97c1d5689ab77fead4674 100644
--- a/include/hpp/fcl/serialization/collision_object.h
+++ b/include/hpp/fcl/serialization/collision_object.h
@@ -41,4 +41,55 @@ HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionGeometry)
} // namespace serialization
} // namespace boost
+namespace hpp {
+namespace fcl {
+
+// fwd declaration
+template <typename BV>
+class HeightField;
+
+template <typename PolygonT>
+class Convex;
+
+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<CollisionGeometry> {
+ template <class Archive>
+ static void on(Archive& ar) {
+ ar.template register_type<Box>();
+ ar.template register_type<Sphere>();
+ ar.template register_type<Ellipsoid>();
+ ar.template register_type<TriangleP>();
+ ar.template register_type<Capsule>();
+ ar.template register_type<Cone>();
+ ar.template register_type<Cylinder>();
+ ar.template register_type<Halfspace>();
+ ar.template register_type<Plane>();
+ ar.template register_type<OcTree>();
+ ar.template register_type<HeightField<OBB>>();
+ ar.template register_type<HeightField<OBBRSS>>();
+ ar.template register_type<HeightField<AABB>>();
+ ar.template register_type<Convex<Triangle>>();
+ ;
+ }
+};
+} // namespace serialization
+} // namespace fcl
+} // namespace hpp
+
#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H
diff --git a/include/hpp/fcl/serialization/convex.h b/include/hpp/fcl/serialization/convex.h
index dc027bc84edb9ce94b211150201bedb9b34eb2e9..29a4f0310539f0f93256c56d48f615c4d3b13ed9 100644
--- a/include/hpp/fcl/serialization/convex.h
+++ b/include/hpp/fcl/serialization/convex.h
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2022 INRIA
+// Copyright (c) 2022-2024 INRIA
//
#ifndef HPP_FCL_SERIALIZATION_CONVEX_H
@@ -19,7 +19,6 @@ namespace serialization {
namespace internal {
struct ConvexBaseAccessor : hpp::fcl::ConvexBase {
typedef hpp::fcl::ConvexBase Base;
- using Base::own_storage_;
};
} // namespace internal
@@ -29,31 +28,59 @@ void serialize(Archive &ar, hpp::fcl::ConvexBase &convex_base,
const unsigned int /*version*/) {
using namespace hpp::fcl;
- typedef internal::ConvexBaseAccessor Accessor;
- Accessor &accessor = reinterpret_cast<Accessor &>(convex_base);
-
ar &make_nvp("base", boost::serialization::base_object<hpp::fcl::ShapeBase>(
convex_base));
const unsigned int num_points_previous = convex_base.num_points;
+ const unsigned int num_normals_and_offsets_previous =
+ convex_base.num_normals_and_offsets;
ar &make_nvp("num_points", convex_base.num_points);
+ ar &make_nvp("num_normals_and_offsets", convex_base.num_normals_and_offsets);
if (Archive::is_loading::value) {
- if (num_points_previous != convex_base.num_points ||
- !accessor.own_storage_) {
- delete[] convex_base.points;
- convex_base.points = new hpp::fcl::Vec3f[convex_base.num_points];
- accessor.own_storage_ = true;
+ 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<Vec3f>(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<Vec3f>(convex_base.num_normals_and_offsets));
+ convex_base.offsets.reset(
+ new std::vector<double>(convex_base.num_normals_and_offsets));
+ }
}
}
- {
+ if (convex_base.num_points > 0) {
typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> MatrixPoints;
Eigen::Map<MatrixPoints> points_map(
- reinterpret_cast<double *>(convex_base.points), 3,
+ reinterpret_cast<double *>(convex_base.points->data()), 3,
convex_base.num_points);
ar &make_nvp("points", points_map);
}
+ if (convex_base.num_normals_and_offsets > 0) {
+ typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> MatrixPoints;
+ Eigen::Map<MatrixPoints> normals_map(
+ reinterpret_cast<double *>(convex_base.normals->data()), 3,
+ convex_base.num_normals_and_offsets);
+ ar &make_nvp("normals", normals_map);
+ }
+
+ if (convex_base.num_normals_and_offsets > 0) {
+ typedef Eigen::Matrix<FCL_REAL, 1, Eigen::Dynamic> VecOfDoubles;
+ Eigen::Map<VecOfDoubles> offsets_map(
+ reinterpret_cast<double *>(convex_base.offsets->data()), 1,
+ convex_base.num_normals_and_offsets);
+ ar &make_nvp("offsets", offsets_map);
+ }
+
ar &make_nvp("center", convex_base.center);
// We don't save neighbors as they will be computed directly by calling
// fillNeighbors.
@@ -75,19 +102,18 @@ void serialize(Archive &ar, hpp::fcl::Convex<PolygonT> &convex_,
typedef internal::ConvexAccessor<PolygonT> Accessor;
Accessor &convex = reinterpret_cast<Accessor &>(convex_);
- ar &make_nvp("base", boost::serialization::base_object<ConvexBase>(convex));
+ ar &make_nvp("base", boost::serialization::base_object<ConvexBase>(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) {
- delete[] convex.polygons;
- convex.polygons = new PolygonT[convex.num_polygons];
+ convex.polygons.reset(new std::vector<PolygonT>(convex.num_polygons));
}
}
- ar &make_array<PolygonT>(convex.polygons, convex.num_polygons);
+ ar &make_array<PolygonT>(convex.polygons->data(), convex.num_polygons);
if (Archive::is_loading::value) convex.fillNeighbors();
}
@@ -95,6 +121,9 @@ void serialize(Archive &ar, hpp::fcl::Convex<PolygonT> &convex_,
} // namespace serialization
} // namespace boost
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(hpp::fcl::Convex<hpp::fcl::Triangle>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(hpp::fcl::Convex<hpp::fcl::Quadrilateral>)
+
namespace hpp {
namespace fcl {
diff --git a/include/hpp/fcl/serialization/fwd.h b/include/hpp/fcl/serialization/fwd.h
index 51ef149082a8b98074ca46fad02c3b69041a8fd3..0075ac064983ae7942da712c120d32846336ef6b 100644
--- a/include/hpp/fcl/serialization/fwd.h
+++ b/include/hpp/fcl/serialization/fwd.h
@@ -1,12 +1,22 @@
//
-// Copyright (c) 2021 INRIA
+// Copyright (c) 2021-2024 INRIA
//
#ifndef HPP_FCL_SERIALIZATION_FWD_H
#define HPP_FCL_SERIALIZATION_FWD_H
+#include <type_traits>
+
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/archive/xml_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/export.hpp>
#include "hpp/fcl/serialization/eigen.h"
@@ -16,4 +26,88 @@
split_free(ar, value, version); \
}
+#define HPP_FCL_SERIALIZATION_DECLARE_EXPORT(T) \
+ BOOST_CLASS_EXPORT_KEY(T) \
+ namespace boost { \
+ namespace archive { \
+ namespace detail { \
+ namespace extra_detail { \
+ template <> \
+ struct init_guid<T> { \
+ static guid_initializer<T> const& g; \
+ }; \
+ } \
+ } \
+ } \
+ } \
+ /**/
+
+#define HPP_FCL_SERIALIZATION_DEFINE_EXPORT(T) \
+ namespace boost { \
+ namespace archive { \
+ namespace detail { \
+ namespace extra_detail { \
+ guid_initializer<T> const& init_guid<T>::g = \
+ ::boost::serialization::singleton< \
+ guid_initializer<T> >::get_mutable_instance() \
+ .export_guid(); \
+ } \
+ } \
+ } \
+ } \
+ /**/
+
+namespace hpp {
+namespace fcl {
+namespace serialization {
+namespace detail {
+
+template <class Derived, class Base>
+struct init_cast_register {};
+
+template <class Derived, class Base>
+struct cast_register_initializer {
+ void init(std::true_type) const {
+ boost::serialization::void_cast_register<Derived, Base>();
+ }
+
+ void init(std::false_type) const {}
+
+ cast_register_initializer const& init() const {
+ BOOST_STATIC_WARNING((std::is_base_of<Base, Derived>::value));
+ init(std::is_base_of<Base, Derived>());
+ return *this;
+ }
+};
+
+} // namespace detail
+
+template <typename T>
+struct register_type {
+ template <class Archive>
+ static void on(Archive& /*ar*/) {}
+};
+} // namespace serialization
+} // namespace fcl
+} // namespace hpp
+
+#define HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base) \
+ namespace hpp { \
+ namespace fcl { \
+ namespace serialization { \
+ namespace detail { \
+ template <> \
+ struct init_cast_register<Derived, Base> { \
+ static cast_register_initializer<Derived, Base> const& g; \
+ }; \
+ cast_register_initializer<Derived, Base> const& init_cast_register< \
+ Derived, Base>::g = \
+ ::boost::serialization::singleton< \
+ cast_register_initializer<Derived, Base> >::get_mutable_instance() \
+ .init(); \
+ } \
+ } \
+ } \
+ }
+
#endif // ifndef HPP_FCL_SERIALIZATION_FWD_H
diff --git a/include/hpp/fcl/serialization/geometric_shapes.h b/include/hpp/fcl/serialization/geometric_shapes.h
index 0459c5f60e91e5924593f328567ce05c6324d304..c56b23ac2862b5448d3fa2299a9bd6c8cfbd3261 100644
--- a/include/hpp/fcl/serialization/geometric_shapes.h
+++ b/include/hpp/fcl/serialization/geometric_shapes.h
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2021 INRIA
+// Copyright (c) 2021-2024 INRIA
//
#ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H
@@ -7,6 +7,7 @@
#include "hpp/fcl/shape/geometric_shapes.h"
#include "hpp/fcl/serialization/fwd.h"
+#include "hpp/fcl/serialization/collision_object.h"
namespace boost {
namespace serialization {
@@ -101,4 +102,16 @@ void serialize(Archive& ar, hpp::fcl::Plane& plane,
} // namespace serialization
} // namespace boost
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::ShapeBase)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionGeometry)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::TriangleP)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Box)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Sphere)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Ellipsoid)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Capsule)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Cone)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Cylinder)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Halfspace)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Plane)
+
#endif // ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H
diff --git a/include/hpp/fcl/serialization/hfield.h b/include/hpp/fcl/serialization/hfield.h
index 7ae2433ef0f3719d547e6d0f19737b53219545c0..15c4ad5bdefc0ef65dc608b54fe17d948df002db 100644
--- a/include/hpp/fcl/serialization/hfield.h
+++ b/include/hpp/fcl/serialization/hfield.h
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2021 INRIA
+// Copyright (c) 2021-2024 INRIA
//
#ifndef HPP_FCL_SERIALIZATION_HFIELD_H
@@ -72,4 +72,9 @@ void serialize(Archive &ar, hpp::fcl::HeightField<BV> &hf_model,
} // namespace serialization
} // namespace boost
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::HeightField<::hpp::fcl::AABB>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::HeightField<::hpp::fcl::OBB>)
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(
+ ::hpp::fcl::HeightField<::hpp::fcl::OBBRSS>)
+
#endif // ifndef HPP_FCL_SERIALIZATION_HFIELD_H
diff --git a/include/hpp/fcl/serialization/octree.h b/include/hpp/fcl/serialization/octree.h
new file mode 100644
index 0000000000000000000000000000000000000000..8ea430022c6a5a0d8134a7c77ef948959d72b65a
--- /dev/null
+++ b/include/hpp/fcl/serialization/octree.h
@@ -0,0 +1,103 @@
+//
+// Copyright (c) 2023-2024 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_OCTREE_H
+#define HPP_FCL_SERIALIZATION_OCTREE_H
+
+#include <sstream>
+#include <iostream>
+
+#include <boost/serialization/string.hpp>
+
+#include "hpp/fcl/octree.h"
+#include "hpp/fcl/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct OcTreeAccessor : hpp::fcl::OcTree {
+ typedef hpp::fcl::OcTree Base;
+ using Base::default_occupancy;
+ using Base::free_threshold;
+ using Base::occupancy_threshold;
+ using Base::tree;
+};
+} // namespace internal
+
+template <class Archive>
+void save_construct_data(Archive &ar, const hpp::fcl::OcTree *octree_ptr,
+ const unsigned int /*version*/) {
+ const double resolution = octree_ptr->getResolution();
+ ar << make_nvp("resolution", resolution);
+}
+
+template <class Archive>
+void save(Archive &ar, const hpp::fcl::OcTree &octree,
+ const unsigned int /*version*/) {
+ typedef internal::OcTreeAccessor Accessor;
+ const Accessor &access = reinterpret_cast<const Accessor &>(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<hpp::fcl::CollisionGeometry>(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 <class Archive>
+void load_construct_data(Archive &ar, hpp::fcl::OcTree *octree_ptr,
+ const unsigned int /*version*/) {
+ double resolution;
+ ar >> make_nvp("resolution", resolution);
+ new (octree_ptr) hpp::fcl::OcTree(resolution);
+}
+
+template <class Archive>
+void load(Archive &ar, hpp::fcl::OcTree &octree,
+ const unsigned int /*version*/) {
+ typedef internal::OcTreeAccessor Accessor;
+ Accessor &access = reinterpret_cast<Accessor &>(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<const octomap::OcTree>(
+ dynamic_cast<octomap::OcTree *>(new_tree));
+
+ ar >> make_nvp("base", base_object<hpp::fcl::CollisionGeometry>(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 <class Archive>
+void serialize(Archive &ar, hpp::fcl::OcTree &octree,
+ const unsigned int version) {
+ split_free(ar, octree, version);
+}
+
+} // namespace serialization
+} // namespace boost
+
+HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::OcTree)
+
+#endif // ifndef HPP_FCL_SERIALIZATION_OCTREE_H
diff --git a/include/hpp/fcl/shape/convex.h b/include/hpp/fcl/shape/convex.h
index dc348e9d43a4fecadbe4b6f56e14633e6b1634b9..99ea403fefed1c1f18c7a5e5950613895946cde7 100644
--- a/include/hpp/fcl/shape/convex.h
+++ b/include/hpp/fcl/shape/convex.h
@@ -50,7 +50,7 @@ template <typename PolygonT>
class Convex : public ConvexBase {
public:
/// @brief Construct an uninitialized convex object
- Convex() : ConvexBase(), polygons(NULL), num_polygons(0) {}
+ Convex() : ConvexBase(), num_polygons(0) {}
/// @brief Constructing a convex, providing normal and offset of each polytype
/// surface, and the points and shape topology information \param ownStorage
@@ -61,8 +61,9 @@ class Convex : public ConvexBase {
/// \param polygons_ \copydoc Convex::polygons
/// \param num_polygons_ the number of polygons.
/// \note num_polygons_ is not the allocated size of polygons_.
- Convex(bool ownStorage, Vec3f* points_, unsigned int num_points_,
- PolygonT* polygons_, unsigned int num_polygons_);
+ Convex(std::shared_ptr<std::vector<Vec3f>> points_, unsigned int num_points_,
+ std::shared_ptr<std::vector<PolygonT>> polygons_,
+ unsigned int num_polygons_);
/// @brief Copy constructor
/// Only the list of neighbors is copied.
@@ -73,7 +74,7 @@ class Convex : public ConvexBase {
/// @brief An array of PolygonT object.
/// PolygonT should contains a list of vertices for each polygon,
/// in counter clockwise order.
- PolygonT* polygons;
+ std::shared_ptr<std::vector<PolygonT>> polygons;
unsigned int num_polygons;
/// based on http://number-none.com/blow/inertia/bb_inertia.doc
@@ -94,8 +95,9 @@ class Convex : public ConvexBase {
/// \param num_polygons the number of polygons.
/// \note num_polygons is not the allocated size of polygons.
///
- void set(bool ownStorage, Vec3f* points, unsigned int num_points,
- PolygonT* polygons, unsigned int num_polygons);
+ void set(std::shared_ptr<std::vector<Vec3f>> points, unsigned int num_points,
+ std::shared_ptr<std::vector<PolygonT>> polygons,
+ unsigned int num_polygons);
/// @brief Clone (deep copy).
virtual Convex<PolygonT>* clone() const;
diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx
index 2a53de92841cf0542e703fb6f8ecdc3ca4870780..49934c8f1f20a0c6727519a744dbfb4988031ec3 100644
--- a/include/hpp/fcl/shape/details/convex.hxx
+++ b/include/hpp/fcl/shape/details/convex.hxx
@@ -39,41 +39,39 @@
#include <set>
#include <vector>
+#include <iostream>
namespace hpp {
namespace fcl {
template <typename PolygonT>
-Convex<PolygonT>::Convex(bool own_storage, Vec3f* points_,
- unsigned int num_points_, PolygonT* polygons_,
+Convex<PolygonT>::Convex(std::shared_ptr<std::vector<Vec3f>> points_,
+ unsigned int num_points_,
+ std::shared_ptr<std::vector<PolygonT>> polygons_,
unsigned int num_polygons_)
: ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) {
- initialize(own_storage, points_, num_points_);
+ initialize(points_, num_points_);
fillNeighbors();
}
template <typename PolygonT>
Convex<PolygonT>::Convex(const Convex<PolygonT>& other)
- : ConvexBase(other),
- polygons(other.polygons),
- num_polygons(other.num_polygons) {
- if (own_storage_) {
- polygons = new PolygonT[num_polygons];
- std::copy(other.polygons, other.polygons + num_polygons, polygons);
- }
+ : ConvexBase(other), num_polygons(other.num_polygons) {
+ if (other.polygons.get()) {
+ polygons.reset(new std::vector<PolygonT>(*(other.polygons)));
+ } else
+ polygons.reset();
}
template <typename PolygonT>
-Convex<PolygonT>::~Convex() {
- if (own_storage_) delete[] polygons;
-}
+Convex<PolygonT>::~Convex() {}
template <typename PolygonT>
-void Convex<PolygonT>::set(bool own_storage, Vec3f* points_,
- unsigned int num_points_, PolygonT* polygons_,
+void Convex<PolygonT>::set(std::shared_ptr<std::vector<Vec3f>> points_,
+ unsigned int num_points_,
+ std::shared_ptr<std::vector<PolygonT>> polygons_,
unsigned int num_polygons_) {
- if (own_storage_) delete[] polygons;
- ConvexBase::set(own_storage, points_, num_points_);
+ ConvexBase::set(points_, num_points_);
num_polygons = num_polygons_;
polygons = polygons_;
@@ -83,17 +81,7 @@ void Convex<PolygonT>::set(bool own_storage, Vec3f* points_,
template <typename PolygonT>
Convex<PolygonT>* Convex<PolygonT>::clone() const {
- Vec3f* cloned_points = new Vec3f[num_points];
- std::copy(points, points + num_points, cloned_points);
-
- PolygonT* cloned_polygons = new PolygonT[num_polygons];
- std::copy(polygons, polygons + num_polygons, cloned_polygons);
-
- Convex* copy_ptr = new Convex(true, cloned_points, num_points,
- cloned_polygons, num_polygons);
-
- copy_ptr->ShapeBase::operator=(*this);
- return copy_ptr;
+ return new Convex(*this);
}
template <typename PolygonT>
@@ -107,13 +95,27 @@ Matrix3f Convex<PolygonT>::computeMomentofInertia() const {
C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0,
1 / 120.0, 1 / 120.0, 1 / 60.0;
+ if (!(points.get())) {
+ std::cerr
+ << "Error in `Convex::computeMomentofInertia`! Convex has no vertices."
+ << std::endl;
+ return C;
+ }
+ const std::vector<Vec3f>& points_ = *points;
+ if (!(polygons.get())) {
+ std::cerr
+ << "Error in `Convex::computeMomentofInertia`! Convex has no polygons."
+ << std::endl;
+ return C;
+ }
+ const std::vector<PolygonT>& polygons_ = *polygons;
for (unsigned int i = 0; i < num_polygons; ++i) {
- const PolygonT& polygon = polygons[i];
+ const PolygonT& polygon = polygons_[i];
// compute the center of the polygon
Vec3f plane_center(0, 0, 0);
for (size_type j = 0; j < polygon.size(); ++j)
- plane_center += points[polygon[(index_type)j]];
+ plane_center += points_[polygon[(index_type)j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the
@@ -123,8 +125,8 @@ Matrix3f Convex<PolygonT>::computeMomentofInertia() const {
index_type e_first = polygon[static_cast<index_type>(j)];
index_type e_second =
polygon[static_cast<index_type>((j + 1) % polygon.size())];
- const Vec3f& v1 = points[e_first];
- const Vec3f& v2 = points[e_second];
+ const Vec3f& v1 = points_[e_first];
+ const Vec3f& v2 = points_[e_second];
Matrix3f A;
A << v1.transpose(), v2.transpose(),
v3.transpose(); // this is A' in the original document
@@ -142,12 +144,24 @@ Vec3f Convex<PolygonT>::computeCOM() const {
Vec3f com(0, 0, 0);
FCL_REAL vol = 0;
+ if (!(points.get())) {
+ std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices."
+ << std::endl;
+ return com;
+ }
+ const std::vector<Vec3f>& points_ = *points;
+ if (!(polygons.get())) {
+ std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons."
+ << std::endl;
+ return com;
+ }
+ const std::vector<PolygonT>& polygons_ = *polygons;
for (unsigned int i = 0; i < num_polygons; ++i) {
- const PolygonT& polygon = polygons[i];
+ const PolygonT& polygon = polygons_[i];
// compute the center of the polygon
Vec3f plane_center(0, 0, 0);
for (size_type j = 0; j < polygon.size(); ++j)
- plane_center += points[polygon[(index_type)j]];
+ plane_center += points_[polygon[(index_type)j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the
@@ -157,11 +171,11 @@ Vec3f Convex<PolygonT>::computeCOM() const {
index_type e_first = polygon[static_cast<index_type>(j)];
index_type e_second =
polygon[static_cast<index_type>((j + 1) % polygon.size())];
- const Vec3f& v1 = points[e_first];
- const Vec3f& v2 = points[e_second];
+ const Vec3f& v1 = points_[e_first];
+ const Vec3f& v2 = points_[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
- com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
+ com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol;
}
}
@@ -174,13 +188,25 @@ FCL_REAL Convex<PolygonT>::computeVolume() const {
typedef typename PolygonT::index_type index_type;
FCL_REAL vol = 0;
+ if (!(points.get())) {
+ std::cerr << "Error in `Convex::computeVolume`! Convex has no vertices."
+ << std::endl;
+ return vol;
+ }
+ const std::vector<Vec3f>& points_ = *points;
+ if (!(polygons.get())) {
+ std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons."
+ << std::endl;
+ return vol;
+ }
+ const std::vector<PolygonT>& polygons_ = *polygons;
for (unsigned int i = 0; i < num_polygons; ++i) {
- const PolygonT& polygon = polygons[i];
+ const PolygonT& polygon = polygons_[i];
// compute the center of the polygon
Vec3f plane_center(0, 0, 0);
for (size_type j = 0; j < polygon.size(); ++j)
- plane_center += points[polygon[(index_type)j]];
+ plane_center += points_[polygon[(index_type)j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the
@@ -190,8 +216,8 @@ FCL_REAL Convex<PolygonT>::computeVolume() const {
index_type e_first = polygon[static_cast<index_type>(j)];
index_type e_second =
polygon[static_cast<index_type>((j + 1) % polygon.size())];
- const Vec3f& v1 = points[e_first];
- const Vec3f& v2 = points[e_second];
+ const Vec3f& v1 = points_[e_first];
+ const Vec3f& v2 = points_[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
}
@@ -202,16 +228,20 @@ FCL_REAL Convex<PolygonT>::computeVolume() const {
template <typename PolygonT>
void Convex<PolygonT>::fillNeighbors() {
- if (neighbors) delete[] neighbors;
- neighbors = new Neighbors[num_points];
+ neighbors.reset(new std::vector<Neighbors>(num_points));
typedef typename PolygonT::size_type size_type;
typedef typename PolygonT::index_type index_type;
- std::vector<std::set<index_type> > nneighbors(num_points);
+ std::vector<std::set<index_type>> nneighbors(num_points);
unsigned int c_nneighbors = 0;
+ if (!(polygons.get())) {
+ std::cerr << "Error in `Convex::fillNeighbors`! Convex has no polygons."
+ << std::endl;
+ }
+ const std::vector<PolygonT>& polygons_ = *polygons;
for (unsigned int l = 0; l < num_polygons; ++l) {
- const PolygonT& polygon = polygons[l];
+ const PolygonT& polygon = polygons_[l];
const size_type n = polygon.size();
for (size_type j = 0; j < polygon.size(); ++j) {
@@ -231,12 +261,12 @@ void Convex<PolygonT>::fillNeighbors() {
}
}
- if (nneighbors_) delete[] nneighbors_;
- nneighbors_ = new unsigned int[c_nneighbors];
+ nneighbors_.reset(new std::vector<unsigned int>(c_nneighbors));
- unsigned int* p_nneighbors = nneighbors_;
+ unsigned int* p_nneighbors = nneighbors_->data();
+ std::vector<Neighbors>& neighbors_ = *neighbors;
for (unsigned int i = 0; i < num_points; ++i) {
- Neighbors& n = neighbors[i];
+ Neighbors& n = neighbors_[i];
if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error);
n.count_ = (unsigned char)nneighbors[i].size();
@@ -244,7 +274,7 @@ void Convex<PolygonT>::fillNeighbors() {
p_nneighbors =
std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
}
- assert(p_nneighbors == nneighbors_ + c_nneighbors);
+ assert(p_nneighbors == nneighbors_->data() + c_nneighbors);
}
} // namespace fcl
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index c469bd9725db6947578ffd84da707d8ab85ceb5a..cd1197fda8cee2c1c893fcf90156ce8c4341e8b2 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -43,6 +43,14 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/data_types.h>
#include <string.h>
+#include <vector>
+#include <memory>
+
+#ifdef HPP_FCL_HAS_QHULL
+namespace orgQhull {
+class Qhull;
+}
+#endif
namespace hpp {
namespace fcl {
@@ -592,37 +600,48 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase {
/// Qhull.
/// \note hpp-fcl must have been compiled with option \c HPP_FCL_HAS_QHULL set
/// to \c ON.
- static ConvexBase* convexHull(const Vec3f* points, unsigned int num_points,
- bool keepTriangles,
+ static ConvexBase* convexHull(std::shared_ptr<std::vector<Vec3f>>& points,
+ unsigned int num_points, bool keepTriangles,
const char* qhullCommand = NULL);
- virtual ~ConvexBase();
-
- /// @brief Clone (deep copy).
- virtual ConvexBase* clone() const {
- ConvexBase* copy_ptr = new ConvexBase(*this);
- ConvexBase& copy = *copy_ptr;
+ // TODO(louis): put this method in private sometime in the future.
+ HPP_FCL_DEPRECATED static ConvexBase* convexHull(
+ const Vec3f* points, unsigned int num_points, bool keepTriangles,
+ const char* qhullCommand = NULL);
- if (!copy.own_storage_) {
- copy.points = new Vec3f[copy.num_points];
- std::copy(points, points + num_points, copy.points);
- }
- copy.own_storage_ = true;
- copy.ShapeBase::operator=(*this);
+ virtual ~ConvexBase();
- return copy_ptr;
- }
+ /// @brief Clone (deep copy).
+ /// This method is consistent with BVHModel `clone` method.
+ /// The copy constructor is called, which duplicates the data.
+ virtual ConvexBase* clone() const { return new ConvexBase(*this); }
/// @brief Compute AABB
void computeLocalAABB();
- /// @brief Get node type: a conex polytope
+ /// @brief Get node type: a convex polytope
NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
/// @brief An array of the points of the polygon.
- Vec3f* points;
+ std::shared_ptr<std::vector<Vec3f>> points;
unsigned int num_points;
+ /// @brief An array of the normals of the polygon.
+ std::shared_ptr<std::vector<Vec3f>> normals;
+ /// @brief An array of the offsets to the normals of the polygon.
+ /// Note: there are as many offsets as normals.
+ std::shared_ptr<std::vector<double>> offsets;
+ unsigned int num_normals_and_offsets;
+
+#ifdef HPP_FCL_HAS_QHULL
+ public:
+ void buildDoubleDescription();
+
+ protected:
+ void buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh);
+#endif
+
+ public:
struct HPP_FCL_DLLAPI Neighbors {
unsigned char count_;
unsigned int* n_;
@@ -649,10 +668,11 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase {
bool operator!=(const Neighbors& other) const { return !(*this == other); }
};
+
/// 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 to them.
- Neighbors* neighbors;
+ std::shared_ptr<std::vector<Neighbors>> 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)
@@ -663,11 +683,9 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase {
/// Initialization is done with ConvexBase::initialize.
ConvexBase()
: ShapeBase(),
- points(NULL),
num_points(0),
- neighbors(NULL),
- nneighbors_(NULL),
- own_storage_(false) {}
+ num_normals_and_offsets(0),
+ center(Vec3f::Zero()) {}
/// @brief Initialize the points of the convex shape
/// This also initializes the ConvexBase::center.
@@ -675,22 +693,22 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase {
/// \param ownStorage weither the ConvexBase owns the data.
/// \param points_ list of 3D points ///
/// \param num_points_ number of 3D points
- void initialize(bool ownStorage, Vec3f* points_, unsigned int num_points_);
+ void initialize(std::shared_ptr<std::vector<Vec3f>> points_,
+ unsigned int num_points_);
/// @brief Set the points of the convex shape.
///
/// \param ownStorage weither the ConvexBase owns the data.
/// \param points_ list of 3D points ///
/// \param num_points_ number of 3D points
- void set(bool ownStorage, Vec3f* points_, unsigned int num_points_);
+ void set(std::shared_ptr<std::vector<Vec3f>> points_,
+ unsigned int num_points_);
/// @brief Copy constructor
/// Only the list of neighbors is copied.
ConvexBase(const ConvexBase& other);
- unsigned int* nneighbors_;
-
- bool own_storage_;
+ std::shared_ptr<std::vector<unsigned int>> nneighbors_;
private:
void computeCenter();
@@ -703,12 +721,48 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase {
if (num_points != other.num_points) return false;
- for (unsigned int i = 0; i < num_points; ++i) {
- if (points[i] != other.points[i]) return false;
+ if ((!(points.get()) && other.points.get()) ||
+ (points.get() && !(other.points.get())))
+ return false;
+ if (points.get() && other.points.get()) {
+ const std::vector<Vec3f>& points_ = *points;
+ const std::vector<Vec3f>& other_points_ = *(other.points);
+ for (unsigned int i = 0; i < num_points; ++i) {
+ if (points_[i] != (other_points_)[i]) return false;
+ }
+ }
+
+ if ((!(neighbors.get()) && other.neighbors.get()) ||
+ (neighbors.get() && !(other.neighbors.get())))
+ return false;
+ if (neighbors.get() && other.neighbors.get()) {
+ const std::vector<Neighbors>& neighbors_ = *neighbors;
+ const std::vector<Neighbors>& other_neighbors_ = *(other.neighbors);
+ for (unsigned int i = 0; i < num_points; ++i) {
+ if (neighbors_[i] != other_neighbors_[i]) return false;
+ }
+ }
+
+ if ((!(normals.get()) && other.normals.get()) ||
+ (normals.get() && !(other.normals.get())))
+ return false;
+ if (normals.get() && other.normals.get()) {
+ const std::vector<Vec3f>& normals_ = *normals;
+ const std::vector<Vec3f>& other_normals_ = *(other.normals);
+ for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
+ if (normals_[i] != other_normals_[i]) return false;
+ }
}
- for (unsigned int i = 0; i < num_points; ++i) {
- if (neighbors[i] != other.neighbors[i]) return false;
+ if ((!(offsets.get()) && other.offsets.get()) ||
+ (offsets.get() && !(other.offsets.get())))
+ return false;
+ if (offsets.get() && other.offsets.get()) {
+ const std::vector<double>& offsets_ = *offsets;
+ const std::vector<double>& other_offsets_ = *(other.offsets);
+ for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
+ if (offsets_[i] != other_offsets_[i]) return false;
+ }
}
return center == other.center;
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index df7abe3d3fd75ca543d1ebc8218d7227cee1d2b5..8154917ee1f6dded0a6b0cac4fde902914746ed5 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -49,6 +49,7 @@ SET(${LIBRARY_NAME}_HEADERS
broadphase/broadphase_collision_manager.hh
broadphase/broadphase_callbacks.hh
pickle.hh
+ utils/std-pair.hh
)
SET(ENABLE_PYTHON_DOXYGEN_AUTODOC TRUE CACHE BOOL "Enable automatic documentation of Python bindings from Doxygen documentation")
@@ -129,7 +130,6 @@ IF(ENABLE_DOXYGEN_AUTODOC)
TARGET_COMPILE_DEFINITIONS(${LIBRARY_NAME} PRIVATE -DHPP_FCL_HAS_DOXYGEN_AUTODOC)
ENDIF()
-TARGET_LINK_BOOST_PYTHON(${LIBRARY_NAME} PUBLIC)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC
${PROJECT_NAME}
eigenpy::eigenpy
diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc
index 49953bcb0af58699015295a16da5baf06884ade7..9e5ee524347c3ca8396fe27cab960981d5eb0cee 100644
--- a/python/broadphase/broadphase.cc
+++ b/python/broadphase/broadphase.cc
@@ -34,6 +34,7 @@
#include <hpp/fcl/fwd.hh>
#include "../fcl.hh"
+#include "../utils/std-pair.hh"
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
@@ -66,25 +67,34 @@ void exposeBroadPhase() {
DistanceCallBackBaseWrapper::expose();
// CollisionCallBackDefault
- bp::class_<CollisionCallBackDefault, bp::bases<CollisionCallBackBase> >(
+ bp::class_<CollisionCallBackDefault, bp::bases<CollisionCallBackBase>>(
"CollisionCallBackDefault", bp::no_init)
.def(dv::init<CollisionCallBackDefault>())
.DEF_RW_CLASS_ATTRIB(CollisionCallBackDefault, data);
// DistanceCallBackDefault
- bp::class_<DistanceCallBackDefault, bp::bases<DistanceCallBackBase> >(
+ bp::class_<DistanceCallBackDefault, bp::bases<DistanceCallBackBase>>(
"DistanceCallBackDefault", bp::no_init)
.def(dv::init<DistanceCallBackDefault>())
.DEF_RW_CLASS_ATTRIB(DistanceCallBackDefault, data);
// CollisionCallBackCollect
- bp::class_<CollisionCallBackCollect, bp::bases<CollisionCallBackBase> >(
+ bp::class_<CollisionCallBackCollect, bp::bases<CollisionCallBackBase>>(
"CollisionCallBackCollect", bp::no_init)
.def(dv::init<CollisionCallBackCollect, const size_t>())
.DEF_CLASS_FUNC(CollisionCallBackCollect, numCollisionPairs)
.DEF_CLASS_FUNC2(CollisionCallBackCollect, getCollisionPairs,
bp::return_value_policy<bp::copy_const_reference>())
- .DEF_CLASS_FUNC(CollisionCallBackCollect, exist);
+ .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<CollisionCallBackCollect::CollisionPair>::registration();
bp::class_<CollisionData>("CollisionData", bp::no_init)
.def(dv::init<CollisionData>())
@@ -118,9 +128,9 @@ void exposeBroadPhase() {
detail::SpatialHash>
HashTable;
typedef SpatialHashingCollisionManager<HashTable> Derived;
- bp::class_<Derived, bp::bases<BroadPhaseCollisionManager> >(
+ bp::class_<Derived, bp::bases<BroadPhaseCollisionManager>>(
"SpatialHashingCollisionManager", bp::no_init)
.def(dv::init<Derived, FCL_REAL, const Vec3f &, const Vec3f &,
- bp::optional<unsigned int> >());
+ bp::optional<unsigned int>>());
}
}
diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc
index 9801851f672fc35afa8450e2c094d8413847931a..45c1cf39a9e9cdf3cb58a1ac919a4c9772259a92 100644
--- a/python/collision-geometries.cc
+++ b/python/collision-geometries.cc
@@ -83,16 +83,19 @@ struct BVHModelBaseWrapper {
static Vec3f& 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];
+ return (*(bvh.vertices))[i];
}
static RefRowMatrixX3 vertices(BVHModelBase& bvh) {
- return MapRowMatrixX3(bvh.vertices[0].data(), bvh.num_vertices, 3);
+ if (bvh.num_vertices > 0)
+ return MapRowMatrixX3(bvh.vertices->data()->data(), bvh.num_vertices, 3);
+ else
+ return MapRowMatrixX3(NULL, bvh.num_vertices, 3);
}
static Triangle 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];
+ return (*bvh.tri_indices)[i];
}
};
@@ -101,7 +104,7 @@ void exposeBVHModel(const std::string& bvname) {
typedef BVHModel<BV> BVH;
const std::string type_name = "BVHModel" + bvname;
- class_<BVH, bases<BVHModelBase>, shared_ptr<BVH> >(
+ class_<BVH, bases<BVHModelBase>, shared_ptr<BVH>>(
type_name.c_str(), doxygen::class_doc<BVH>(), no_init)
.def(dv::init<BVH>())
.def(dv::init<BVH, const BVH&>())
@@ -120,12 +123,12 @@ void exposeHeightField(const std::string& bvname) {
typedef typename Geometry::Node Node;
const std::string type_name = "HeightField" + bvname;
- class_<Geometry, bases<Base>, shared_ptr<Geometry> >(
+ class_<Geometry, bases<Base>, shared_ptr<Geometry>>(
type_name.c_str(), doxygen::class_doc<Geometry>(), no_init)
- .def(dv::init<HeightField<BV> >())
+ .def(dv::init<HeightField<BV>>())
.def(dv::init<HeightField<BV>, const HeightField<BV>&>())
.def(dv::init<HeightField<BV>, FCL_REAL, FCL_REAL, const MatrixXf&,
- bp::optional<FCL_REAL> >())
+ bp::optional<FCL_REAL>>())
.DEF_CLASS_FUNC(Geometry, getXDim)
.DEF_CLASS_FUNC(Geometry, getYDim)
@@ -157,23 +160,49 @@ struct ConvexBaseWrapper {
typedef Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> RowMatrixX3;
typedef Eigen::Map<RowMatrixX3> MapRowMatrixX3;
typedef Eigen::Ref<RowMatrixX3> RefRowMatrixX3;
+ typedef Eigen::VectorXd VecOfDoubles;
+ typedef Eigen::Map<VecOfDoubles> MapVecOfDoubles;
+ typedef Eigen::Ref<VecOfDoubles> RefVecOfDoubles;
static Vec3f& point(const ConvexBase& convex, unsigned int i) {
if (i >= convex.num_points)
throw std::out_of_range("index is out of range");
- return convex.points[i];
+ return (*(convex.points))[i];
}
static RefRowMatrixX3 points(const ConvexBase& convex) {
- return MapRowMatrixX3(convex.points[0].data(), convex.num_points, 3);
+ return MapRowMatrixX3((*(convex.points))[0].data(), convex.num_points, 3);
+ }
+
+ static Vec3f& normal(const ConvexBase& 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 ConvexBase& convex) {
+ return MapRowMatrixX3((*(convex.normals))[0].data(),
+ convex.num_normals_and_offsets, 3);
+ }
+
+ static double offset(const ConvexBase& 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 RefVecOfDoubles offsets(const ConvexBase& convex) {
+ return MapVecOfDoubles(convex.offsets->data(),
+ convex.num_normals_and_offsets, 1);
}
static list neighbors(const ConvexBase& convex, unsigned int i) {
if (i >= convex.num_points)
throw std::out_of_range("index is out of range");
list n;
- for (unsigned char j = 0; j < convex.neighbors[i].count(); ++j)
- n.append(convex.neighbors[i][j]);
+ const std::vector<ConvexBase::Neighbors>& neighbors_ = *(convex.neighbors);
+ for (unsigned char j = 0; j < neighbors_[i].count(); ++j)
+ n.append(neighbors_[i][j]);
return n;
}
@@ -191,16 +220,21 @@ struct ConvexWrapper {
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];
+ return (*convex.polygons)[i];
}
static shared_ptr<Convex_t> constructor(const Vec3fs& _points,
const Triangles& _tris) {
- Vec3f* points = new Vec3f[_points.size()];
- for (std::size_t i = 0; i < _points.size(); ++i) points[i] = _points[i];
- Triangle* tris = new Triangle[_tris.size()];
- for (std::size_t i = 0; i < _tris.size(); ++i) tris[i] = _tris[i];
- return shared_ptr<Convex_t>(new Convex_t(true, points,
+ std::shared_ptr<std::vector<Vec3f>> points(
+ new std::vector<Vec3f>(_points.size()));
+ std::vector<Vec3f>& points_ = *points;
+ for (std::size_t i = 0; i < _points.size(); ++i) points_[i] = _points[i];
+
+ std::shared_ptr<std::vector<Triangle>> tris(
+ new std::vector<Triangle>(_tris.size()));
+ std::vector<Triangle>& tris_ = *tris;
+ for (std::size_t i = 0; i < _tris.size(); ++i) tris_[i] = _tris[i];
+ return shared_ptr<Convex_t>(new Convex_t(points,
(unsigned int)_points.size(), tris,
(unsigned int)_tris.size()));
}
@@ -222,9 +256,9 @@ void exposeComputeMemoryFootprint() {
defComputeMemoryFootprint<Halfspace>();
defComputeMemoryFootprint<TriangleP>();
- defComputeMemoryFootprint<BVHModel<OBB> >();
- defComputeMemoryFootprint<BVHModel<RSS> >();
- defComputeMemoryFootprint<BVHModel<OBBRSS> >();
+ defComputeMemoryFootprint<BVHModel<OBB>>();
+ defComputeMemoryFootprint<BVHModel<RSS>>();
+ defComputeMemoryFootprint<BVHModel<OBBRSS>>();
}
void exposeShapes() {
@@ -233,7 +267,7 @@ void exposeShapes() {
//.def ("getObjectType", &CollisionGeometry::getObjectType)
;
- class_<Box, bases<ShapeBase>, shared_ptr<Box> >(
+ class_<Box, bases<ShapeBase>, shared_ptr<Box>>(
"Box", doxygen::class_doc<ShapeBase>(), no_init)
.def(dv::init<Box>())
.def(dv::init<Box, const Box&>())
@@ -244,7 +278,7 @@ void exposeShapes() {
return_value_policy<manage_new_object>())
.def_pickle(PickleObject<Box>());
- class_<Capsule, bases<ShapeBase>, shared_ptr<Capsule> >(
+ class_<Capsule, bases<ShapeBase>, shared_ptr<Capsule>>(
"Capsule", doxygen::class_doc<Capsule>(), no_init)
.def(dv::init<Capsule>())
.def(dv::init<Capsule, FCL_REAL, FCL_REAL>())
@@ -255,7 +289,7 @@ void exposeShapes() {
return_value_policy<manage_new_object>())
.def_pickle(PickleObject<Capsule>());
- class_<Cone, bases<ShapeBase>, shared_ptr<Cone> >(
+ class_<Cone, bases<ShapeBase>, shared_ptr<Cone>>(
"Cone", doxygen::class_doc<Cone>(), no_init)
.def(dv::init<Cone>())
.def(dv::init<Cone, FCL_REAL, FCL_REAL>())
@@ -270,39 +304,50 @@ void exposeShapes() {
"ConvexBase", doxygen::class_doc<ConvexBase>(), no_init)
.DEF_RO_CLASS_ATTRIB(ConvexBase, center)
.DEF_RO_CLASS_ATTRIB(ConvexBase, num_points)
+ .DEF_RO_CLASS_ATTRIB(ConvexBase, num_normals_and_offsets)
.def("point", &ConvexBaseWrapper::point, bp::args("self", "index"),
"Retrieve the point given by its index.",
bp::return_internal_reference<>())
.def("points", &ConvexBaseWrapper::point, bp::args("self", "index"),
"Retrieve the point given by its index.",
::hpp::fcl::python::deprecated_member<
- bp::return_internal_reference<> >())
+ bp::return_internal_reference<>>())
.def("points", &ConvexBaseWrapper::points, bp::args("self"),
"Retrieve all the points.",
bp::with_custodian_and_ward_postcall<0, 1>())
// .add_property ("points",
// bp::make_function(&ConvexBaseWrapper::points,bp::with_custodian_and_ward_postcall<0,1>()),
// "Points of the convex.")
+ .def("normal", &ConvexBaseWrapper::normal, bp::args("self", "index"),
+ "Retrieve the normal given by its index.",
+ bp::return_internal_reference<>())
+ .def("normals", &ConvexBaseWrapper::normals, bp::args("self"),
+ "Retrieve all the normals.",
+ bp::with_custodian_and_ward_postcall<0, 1>())
+ .def("offset", &ConvexBaseWrapper::offset, bp::args("self", "index"),
+ "Retrieve the offset given by its index.")
+ .def("offsets", &ConvexBaseWrapper::offsets, bp::args("self"),
+ "Retrieve all the offsets.",
+ bp::with_custodian_and_ward_postcall<0, 1>())
.def("neighbors", &ConvexBaseWrapper::neighbors)
.def("convexHull", &ConvexBaseWrapper::convexHull,
- doxygen::member_func_doc(&ConvexBase::convexHull),
+ // doxygen::member_func_doc(&ConvexBase::convexHull),
return_value_policy<manage_new_object>())
.staticmethod("convexHull")
.def("clone", &ConvexBase::clone,
doxygen::member_func_doc(&ConvexBase::clone),
return_value_policy<manage_new_object>());
- class_<Convex<Triangle>, bases<ConvexBase>, shared_ptr<Convex<Triangle> >,
- noncopyable>("Convex", doxygen::class_doc<Convex<Triangle> >(),
- no_init)
+ class_<Convex<Triangle>, bases<ConvexBase>, shared_ptr<Convex<Triangle>>,
+ noncopyable>("Convex", doxygen::class_doc<Convex<Triangle>>(), no_init)
.def("__init__", make_constructor(&ConvexWrapper<Triangle>::constructor))
- .def(dv::init<Convex<Triangle> >())
+ .def(dv::init<Convex<Triangle>>())
.def(dv::init<Convex<Triangle>, const Convex<Triangle>&>())
.DEF_RO_CLASS_ATTRIB(Convex<Triangle>, num_polygons)
.def("polygons", &ConvexWrapper<Triangle>::polygons)
- .def_pickle(PickleObject<Convex<Triangle> >());
+ .def_pickle(PickleObject<Convex<Triangle>>());
- class_<Cylinder, bases<ShapeBase>, shared_ptr<Cylinder> >(
+ class_<Cylinder, bases<ShapeBase>, shared_ptr<Cylinder>>(
"Cylinder", doxygen::class_doc<Cylinder>(), no_init)
.def(dv::init<Cylinder>())
.def(dv::init<Cylinder, FCL_REAL, FCL_REAL>())
@@ -314,7 +359,7 @@ void exposeShapes() {
return_value_policy<manage_new_object>())
.def_pickle(PickleObject<Cylinder>());
- class_<Halfspace, bases<ShapeBase>, shared_ptr<Halfspace> >(
+ class_<Halfspace, bases<ShapeBase>, shared_ptr<Halfspace>>(
"Halfspace", doxygen::class_doc<Halfspace>(), no_init)
.def(dv::init<Halfspace, const Vec3f&, FCL_REAL>())
.def(dv::init<Halfspace, const Halfspace&>())
@@ -327,7 +372,7 @@ void exposeShapes() {
return_value_policy<manage_new_object>())
.def_pickle(PickleObject<Halfspace>());
- class_<Plane, bases<ShapeBase>, shared_ptr<Plane> >(
+ class_<Plane, bases<ShapeBase>, shared_ptr<Plane>>(
"Plane", doxygen::class_doc<Plane>(), no_init)
.def(dv::init<Plane, const Vec3f&, FCL_REAL>())
.def(dv::init<Plane, const Plane&>())
@@ -339,7 +384,7 @@ void exposeShapes() {
return_value_policy<manage_new_object>())
.def_pickle(PickleObject<Plane>());
- class_<Sphere, bases<ShapeBase>, shared_ptr<Sphere> >(
+ class_<Sphere, bases<ShapeBase>, shared_ptr<Sphere>>(
"Sphere", doxygen::class_doc<Sphere>(), no_init)
.def(dv::init<Sphere>())
.def(dv::init<Sphere, const Sphere&>())
@@ -349,7 +394,7 @@ void exposeShapes() {
return_value_policy<manage_new_object>())
.def_pickle(PickleObject<Sphere>());
- class_<Ellipsoid, bases<ShapeBase>, shared_ptr<Ellipsoid> >(
+ class_<Ellipsoid, bases<ShapeBase>, shared_ptr<Ellipsoid>>(
"Ellipsoid", doxygen::class_doc<Ellipsoid>(), no_init)
.def(dv::init<Ellipsoid>())
.def(dv::init<Ellipsoid, FCL_REAL, FCL_REAL, FCL_REAL>())
@@ -361,7 +406,7 @@ void exposeShapes() {
return_value_policy<manage_new_object>())
.def_pickle(PickleObject<Ellipsoid>());
- class_<TriangleP, bases<ShapeBase>, shared_ptr<TriangleP> >(
+ class_<TriangleP, bases<ShapeBase>, shared_ptr<TriangleP>>(
"TriangleP", doxygen::class_doc<TriangleP>(), no_init)
.def(dv::init<TriangleP>())
.def(dv::init<TriangleP, const Vec3f&, const Vec3f&, const Vec3f&>())
@@ -582,7 +627,7 @@ void exposeCollisionGeometries() {
.def("vertices", &BVHModelBaseWrapper::vertex, bp::args("self", "index"),
"Retrieve the vertex given by its index.",
::hpp::fcl::python::deprecated_member<
- bp::return_internal_reference<> >())
+ bp::return_internal_reference<>>())
.def("vertices", &BVHModelBaseWrapper::vertices, bp::args("self"),
"Retrieve all the vertices.",
bp::with_custodian_and_ward_postcall<0, 1>())
@@ -634,11 +679,11 @@ void exposeCollisionObject() {
if (!eigenpy::register_symbolic_link_to_registered_type<CollisionObject>()) {
class_<CollisionObject, CollisionObjectPtr_t>("CollisionObject", no_init)
.def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
- bp::optional<bool> >())
+ bp::optional<bool>>())
.def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
- const Transform3f&, bp::optional<bool> >())
+ const Transform3f&, bp::optional<bool>>())
.def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
- const Matrix3f&, const Vec3f&, bp::optional<bool> >())
+ const Matrix3f&, const Vec3f&, bp::optional<bool>>())
.DEF_CLASS_FUNC(CollisionObject, getObjectType)
.DEF_CLASS_FUNC(CollisionObject, getNodeType)
diff --git a/python/collision.cc b/python/collision.cc
index eebd892f9551efdfc9f37d0da1cb035fab7484c2..ce84a718001308bea9df01e10a81462a2f8a3c4b 100644
--- a/python/collision.cc
+++ b/python/collision.cc
@@ -59,6 +59,15 @@ const CollisionGeometry* geto(const Contact& c) {
return index == 1 ? c.o1 : c.o2;
}
+struct ContactWrapper {
+ static Vec3f getNearestPoint1(const Contact& contact) {
+ return contact.nearest_points[0];
+ }
+ static Vec3f getNearestPoint2(const Contact& contact) {
+ return contact.nearest_points[1];
+ }
+};
+
void exposeCollisionAPI() {
if (!eigenpy::register_symbolic_link_to_registered_type<
CollisionRequestFlag>()) {
@@ -152,9 +161,14 @@ void exposeCollisionAPI() {
make_function(&geto<2>,
return_value_policy<reference_existing_object>()),
doxygen::class_attrib_doc<Contact>("o2"))
+ .def("getNearestPoint1", &ContactWrapper::getNearestPoint1,
+ doxygen::class_attrib_doc<Contact>("nearest_points"))
+ .def("getNearestPoint2", &ContactWrapper::getNearestPoint2,
+ doxygen::class_attrib_doc<Contact>("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)
diff --git a/python/distance.cc b/python/distance.cc
index d69427f23aac510b883b7e9a89a1a484e2892bd1..8c8090bb68f1d8aeb36d9a9c14ca1c520dd1f4f9 100644
--- a/python/distance.cc
+++ b/python/distance.cc
@@ -88,6 +88,7 @@ void exposeDistanceAPI() {
doxygen::class_attrib_doc<DistanceResult>("nearest_points"))
.def("getNearestPoint2", &DistanceRequestWrapper::getNearestPoint2,
doxygen::class_attrib_doc<DistanceResult>("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)
diff --git a/python/gjk.cc b/python/gjk.cc
index c9e9f856f33015e13f54689669d892c90036b337..0673a96847ed4911e1c7ec738f3e80975727517f 100644
--- a/python/gjk.cc
+++ b/python/gjk.cc
@@ -87,10 +87,19 @@ void exposeGJK() {
if (!eigenpy::register_symbolic_link_to_registered_type<GJKVariant>()) {
enum_<GJKVariant>("GJKVariant")
.value("DefaultGJK", GJKVariant::DefaultGJK)
+ .value("PolyakAcceleration", GJKVariant::PolyakAcceleration)
.value("NesterovAcceleration", GJKVariant::NesterovAcceleration)
.export_values();
}
+ if (!eigenpy::register_symbolic_link_to_registered_type<GJKInitialGuess>()) {
+ enum_<GJKInitialGuess>("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>("GJKConvergenceCriterion")
@@ -117,12 +126,13 @@ void exposeGJK() {
.DEF_RW_CLASS_ATTRIB(GJK, gjk_variant)
.DEF_RW_CLASS_ATTRIB(GJK, convergence_criterion)
.DEF_RW_CLASS_ATTRIB(GJK, convergence_criterion_type)
+ .DEF_RW_CLASS_ATTRIB(GJK, iterations)
+ .DEF_RW_CLASS_ATTRIB(GJK, tolerance)
.DEF_CLASS_FUNC(GJK, evaluate)
.DEF_CLASS_FUNC(GJK, hasClosestPoints)
.DEF_CLASS_FUNC(GJK, hasPenetrationInformation)
.DEF_CLASS_FUNC(GJK, getClosestPoints)
.DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak)
- .DEF_CLASS_FUNC(GJK, getGuessFromSimplex)
- .DEF_CLASS_FUNC(GJK, getIterations);
+ .DEF_CLASS_FUNC(GJK, getGuessFromSimplex);
}
}
diff --git a/python/math.cc b/python/math.cc
index 9e92cb12ba4330e61cafba181636ff454019b5fe..2793f27caaed1d0c25f8b7672a94dd48b79c032a 100644
--- a/python/math.cc
+++ b/python/math.cc
@@ -77,10 +77,9 @@ void exposeMaths() {
.def(dv::init<Transform3f>())
.def(dv::init<Transform3f, const Matrix3f::MatrixBase&,
const Vec3f::MatrixBase&>())
- .def(dv::init<Transform3f, const Quaternion3f&,
- const Vec3f::MatrixBase&>())
+ .def(dv::init<Transform3f, const Quatf&, const Vec3f::MatrixBase&>())
.def(dv::init<Transform3f, const Matrix3f&>())
- .def(dv::init<Transform3f, const Quaternion3f&>())
+ .def(dv::init<Transform3f, const Quatf&>())
.def(dv::init<Transform3f, const Vec3f&>())
.def(dv::init<Transform3f, const Transform3f&>())
@@ -102,7 +101,7 @@ void exposeMaths() {
&Transform3f::setTransform<Matrix3f, Vec3f>))
.def(dv::member_func(
"setTransform",
- static_cast<void (Transform3f::*)(const Quaternion3f&, const Vec3f&)>(
+ static_cast<void (Transform3f::*)(const Quatf&, const Vec3f&)>(
&Transform3f::setTransform)))
.def(dv::member_func("setIdentity", &Transform3f::setIdentity))
.def(dv::member_func("Identity", &Transform3f::Identity))
diff --git a/python/octree.cc b/python/octree.cc
index e0721af11872c0e6bdc983b7170847ea4862cca0..76ebc8b7f27ba8850ce16150e88803d336498469 100644
--- a/python/octree.cc
+++ b/python/octree.cc
@@ -1,12 +1,32 @@
#include "fcl.hh"
+#include <eigenpy/std-vector.hpp>
+
#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/octree.h>
#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
#include "doxygen_autodoc/functions.h"
+#include "doxygen_autodoc/hpp/fcl/octree.h"
+#endif
+
+bp::object toPyBytes(std::vector<uint8_t>& 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 hpp::fcl::OcTree& self) {
+ std::vector<uint8_t> bytes = self.tobytes();
+ return toPyBytes(bytes);
+}
void exposeOctree() {
using namespace hpp::fcl;
@@ -16,7 +36,11 @@ void exposeOctree() {
bp::class_<OcTree, bp::bases<CollisionGeometry>, shared_ptr<OcTree> >(
"OcTree", doxygen::class_doc<OcTree>(), bp::no_init)
.def(dv::init<OcTree, FCL_REAL>())
+ .def("clone", &OcTree::clone, doxygen::member_func_doc(&OcTree::clone),
+ bp::return_value_policy<bp::manage_new_object>())
.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))
@@ -24,7 +48,12 @@ void exposeOctree() {
&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("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<Vec6f>();
+ eigenpy::StdVectorPythonVisitor<std::vector<Vec6f>, true>::expose(
+ "StdVec_Vec6");
}
diff --git a/python/utils/std-pair.hh b/python/utils/std-pair.hh
new file mode 100644
index 0000000000000000000000000000000000000000..80946720122c5903d8462c8fc9cb5df269e992d4
--- /dev/null
+++ b/python/utils/std-pair.hh
@@ -0,0 +1,65 @@
+//
+// Copyright (c) 2023 INRIA
+//
+
+#ifndef HPP_FCL_PYTHON_UTILS_STD_PAIR_H
+#define HPP_FCL_PYTHON_UTILS_STD_PAIR_H
+
+#include <boost/python.hpp>
+#include <utility>
+
+template <typename pair_type>
+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<T1> elt1(tuple[0]);
+ if (!elt1.check()) return 0;
+ boost::python::extract<T2> 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<pair_type>*>(
+ reinterpret_cast<void*>(memory))
+ ->storage.bytes;
+ new (storage) pair_type(boost::python::extract<T1>(tuple[0]),
+ boost::python::extract<T2>(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<void* (*)(_object*)>(&convertible), &construct,
+ boost::python::type_id<pair_type>()
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+ ,
+ get_pytype
+#endif
+ );
+ }
+};
+
+#endif // ifndef HPP_FCL_PYTHON_UTILS_STD_PAIR_H
diff --git a/python/version.cc b/python/version.cc
index f83798629e90fa12b77f9db82f7091ca58420d94..4bc0eea9337a880f1653e78372991ee355de950e 100644
--- a/python/version.cc
+++ b/python/version.cc
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2019 CNRS INRIA
+// Copyright (c) 2019-2023 CNRS INRIA
//
#include "hpp/fcl/config.hh"
@@ -25,6 +25,18 @@ void exposeVersion() {
bp::scope().attr("HPP_FCL_MINOR_VERSION") = HPP_FCL_MINOR_VERSION;
bp::scope().attr("HPP_FCL_PATCH_VERSION") = HPP_FCL_PATCH_VERSION;
+#if HPP_FCL_HAS_QHULL
+ bp::scope().attr("WITH_QHULL") = true;
+#else
+ bp::scope().attr("WITH_QHULL") = false;
+#endif
+
+#if HPP_FCL_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 hpp-fcl is at least"
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 10d5089538bc0fce4ec03b59e7586ed53ffff426..69b740034fac0ab19536f3234b66e90051ad0ba7 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -116,10 +116,10 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) {
inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
OBB b;
b.To = (b1.To + b2.To) * 0.5;
- Quaternion3f q0(b1.axes), q1(b2.axes);
+ Quatf q0(b1.axes), q1(b2.axes);
if (q0.dot(q1) < 0) q1.coeffs() *= -1;
- Quaternion3f q((q0.coeffs() + q1.coeffs()).normalized());
+ Quatf q((q0.coeffs() + q1.coeffs()).normalized());
b.axes = q.toRotationMatrix();
Vec3f vertex[8], diff;
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index 7aff1b6b329325e77810632a4c11185f7805c330..3327fd071dae6df3aae092250ea9a19cdd4444d7 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -36,6 +36,7 @@
/** \author Jia Pan */
+#include "hpp/fcl/BV/BV_node.h"
#include <hpp/fcl/BVH/BVH_model.h>
#include <iostream>
@@ -51,10 +52,7 @@ namespace hpp {
namespace fcl {
BVHModelBase::BVHModelBase()
- : vertices(NULL),
- tri_indices(NULL),
- prev_vertices(NULL),
- num_tris(0),
+ : num_tris(0),
num_vertices(0),
build_state(BVH_BUILD_STATE_EMPTY),
num_tris_allocated(0),
@@ -68,24 +66,20 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other)
build_state(other.build_state),
num_tris_allocated(other.num_tris),
num_vertices_allocated(other.num_vertices) {
- if (other.vertices) {
- vertices = new Vec3f[num_vertices];
- std::copy(other.vertices, other.vertices + num_vertices, vertices);
+ if (other.vertices.get() && other.vertices->size() > 0) {
+ vertices.reset(new std::vector<Vec3f>(*(other.vertices)));
} else
- vertices = nullptr;
+ vertices.reset();
- if (other.tri_indices) {
- tri_indices = new Triangle[num_tris];
- std::copy(other.tri_indices, other.tri_indices + num_tris, tri_indices);
+ if (other.tri_indices.get() && other.tri_indices->size() > 0) {
+ tri_indices.reset(new std::vector<Triangle>(*(other.tri_indices)));
} else
- tri_indices = nullptr;
+ tri_indices.reset();
- if (other.prev_vertices) {
- prev_vertices = new Vec3f[num_vertices];
- std::copy(other.prev_vertices, other.prev_vertices + num_vertices,
- prev_vertices);
+ if (other.prev_vertices.get() && other.prev_vertices->size() > 0) {
+ prev_vertices.reset(new std::vector<Vec3f>(*(other.prev_vertices)));
} else
- prev_vertices = nullptr;
+ prev_vertices.reset();
}
bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
@@ -98,15 +92,34 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
if (!result) return false;
- for (size_t k = 0; k < static_cast<size_t>(num_tris); ++k)
- if (tri_indices[k] != other.tri_indices[k]) return false;
+ if ((!(tri_indices.get()) && other.tri_indices.get()) ||
+ (tri_indices.get() && !(other.tri_indices.get())))
+ return false;
+ if (tri_indices.get() && other.tri_indices.get()) {
+ const std::vector<Triangle>& tri_indices_ = *(tri_indices);
+ const std::vector<Triangle>& other_tri_indices_ = *(other.tri_indices);
+ for (size_t k = 0; k < static_cast<size_t>(num_tris); ++k)
+ if (tri_indices_[k] != other_tri_indices_[k]) return false;
+ }
- for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k)
- if (vertices[k] != other.vertices[k]) return false;
+ if ((!(vertices.get()) && other.vertices.get()) ||
+ (vertices.get() && !(other.vertices.get())))
+ return false;
+ if (vertices.get() && other.vertices.get()) {
+ const std::vector<Vec3f>& vertices_ = *(vertices);
+ const std::vector<Vec3f>& other_vertices_ = *(other.vertices);
+ for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k)
+ if (vertices_[k] != other_vertices_[k]) return false;
+ }
- if (prev_vertices != nullptr && other.prev_vertices != nullptr) {
+ if ((!(prev_vertices.get()) && other.prev_vertices.get()) ||
+ (prev_vertices.get() && !(other.prev_vertices.get())))
+ return false;
+ if (prev_vertices.get() && other.prev_vertices.get()) {
+ const std::vector<Vec3f>& prev_vertices_ = *(prev_vertices);
+ const std::vector<Vec3f>& other_prev_vertices_ = *(other.prev_vertices);
for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k) {
- if (prev_vertices[k] != other.prev_vertices[k]) return false;
+ if (prev_vertices_[k] != other_prev_vertices_[k]) return false;
}
}
@@ -114,18 +127,28 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
}
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) {
- Vec3f* points = vertices;
- Triangle* polygons = tri_indices;
+ std::shared_ptr<std::vector<Vec3f>> points = vertices;
+ std::shared_ptr<std::vector<Triangle>> polygons = tri_indices;
if (!share_memory) {
- points = new Vec3f[num_vertices];
- std::copy(vertices, vertices + num_vertices, points);
-
- polygons = new Triangle[num_tris];
- std::copy(tri_indices, tri_indices + num_tris, polygons);
+ points.reset(new std::vector<Vec3f>(*(vertices)));
+ polygons.reset(new std::vector<Triangle>(*(tri_indices)));
}
- convex.reset(new Convex<Triangle>(!share_memory, points, num_vertices,
- polygons, num_tris));
+ convex.reset(
+ new Convex<Triangle>(points, num_vertices, polygons, num_tris));
}
}
@@ -141,41 +164,26 @@ BVHModel<BV>::BVHModel(const BVHModel<BV>& other)
: BVHModelBase(other),
bv_splitter(other.bv_splitter),
bv_fitter(other.bv_fitter) {
- if (other.primitive_indices) {
- unsigned int num_primitives = 0;
- switch (other.getModelType()) {
- case BVH_MODEL_TRIANGLES:
- num_primitives = num_tris;
- break;
- case BVH_MODEL_POINTCLOUD:
- num_primitives = num_vertices;
- break;
- default:;
- }
-
- primitive_indices = new unsigned int[num_primitives];
- std::copy(other.primitive_indices, other.primitive_indices + num_primitives,
- primitive_indices);
+ if (other.primitive_indices.get()) {
+ primitive_indices.reset(
+ new std::vector<unsigned int>(*(other.primitive_indices)));
} else
- primitive_indices = nullptr;
+ primitive_indices.reset();
num_bvs = num_bvs_allocated = other.num_bvs;
- if (other.bvs) {
- bvs = new BVNode<BV>[num_bvs];
- std::copy(other.bvs, other.bvs + num_bvs, bvs);
+ if (other.bvs.get()) {
+ bvs.reset(new bv_node_vector_t(*(other.bvs)));
} else
- bvs = nullptr;
+ bvs.reset();
}
int BVHModelBase::beginModel(unsigned int num_tris_,
unsigned int num_vertices_) {
if (build_state != BVH_BUILD_STATE_EMPTY) {
- delete[] vertices;
- vertices = nullptr;
- delete[] tri_indices;
- tri_indices = nullptr;
- delete[] prev_vertices;
- prev_vertices = nullptr;
+ vertices.reset();
+ tri_indices.reset();
+ tri_indices.reset();
+ prev_vertices.reset();
num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = 0;
deleteBVs();
@@ -187,26 +195,33 @@ int BVHModelBase::beginModel(unsigned int num_tris_,
num_vertices_allocated = num_vertices_;
num_tris_allocated = num_tris_;
- tri_indices = new Triangle[num_tris_allocated];
-
- if (!tri_indices) {
- std::cerr << "BVH Error! Out of memory for tri_indices array on "
- "BeginModel() call!"
- << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
+ if (num_tris_allocated > 0) {
+ tri_indices.reset(new std::vector<Triangle>(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();
- vertices = new Vec3f[num_vertices_allocated];
- if (!vertices) {
- std::cerr
- << "BVH Error! Out of memory for vertices array on BeginModel() call!"
- << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
+ if (num_vertices_allocated > 0) {
+ vertices.reset(new std::vector<Vec3f>(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! Call beginModel() on a BVHModel that is not empty. "
+ << "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;
@@ -228,21 +243,23 @@ int BVHModelBase::addVertex(const Vec3f& p) {
}
if (num_vertices >= num_vertices_allocated) {
- Vec3f* temp = new Vec3f[num_vertices_allocated * 2];
- if (!temp) {
+ std::shared_ptr<std::vector<Vec3f>> temp(
+ new std::vector<Vec3f>(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;
}
- std::copy(vertices, vertices + num_vertices, temp);
- delete[] vertices;
+ for (size_t i = 0; i < num_vertices; ++i) {
+ (*temp)[i] = (*vertices)[i];
+ }
vertices = temp;
num_vertices_allocated *= 2;
}
- vertices[num_vertices] = p;
+ (*vertices)[num_vertices] = p;
num_vertices += 1;
return BVH_OK;
@@ -260,25 +277,29 @@ int BVHModelBase::addTriangles(const Matrixx3i& triangles) {
const unsigned int num_tris_to_add = (unsigned int)triangles.rows();
if (num_tris + num_tris_to_add > num_tris_allocated) {
- Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add];
- if (!temp) {
+ std::shared_ptr<std::vector<Triangle>> temp(
+ new std::vector<Triangle>(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;
}
- std::copy(tri_indices, tri_indices + num_tris, temp);
- delete[] tri_indices;
+ 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<Triangle>& 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::index_type>(triangle[0]),
- static_cast<Triangle::index_type>(triangle[1]),
- static_cast<Triangle::index_type>(triangle[2]));
+ tri_indices_[num_tris++].set(
+ static_cast<Triangle::index_type>(triangle[0]),
+ static_cast<Triangle::index_type>(triangle[1]),
+ static_cast<Triangle::index_type>(triangle[2]));
}
return BVH_OK;
@@ -295,21 +316,24 @@ int BVHModelBase::addVertices(const Matrixx3f& points) {
if (num_vertices + points.rows() > num_vertices_allocated) {
num_vertices_allocated = num_vertices + (unsigned int)points.rows();
- Vec3f* temp = new Vec3f[num_vertices_allocated];
- if (!temp) {
+ std::shared_ptr<std::vector<Vec3f>> temp(
+ new std::vector<Vec3f>(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;
}
- std::copy(vertices, vertices + num_vertices, temp);
- delete[] vertices;
+ for (size_t i = 0; i < num_vertices; ++i) {
+ (*temp)[i] = (*vertices)[i];
+ }
vertices = temp;
}
+ std::vector<Vec3f>& vertices_ = *vertices;
for (Eigen::DenseIndex id = 0; id < points.rows(); ++id)
- vertices[num_vertices++] = points.row(id).transpose();
+ vertices_[num_vertices++] = points.row(id).transpose();
return BVH_OK;
}
@@ -325,47 +349,51 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
}
if (num_vertices + 2 >= num_vertices_allocated) {
- Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + 2];
- if (!temp) {
+ std::shared_ptr<std::vector<Vec3f>> temp(
+ new std::vector<Vec3f>(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;
}
- std::copy(vertices, vertices + num_vertices, temp);
- delete[] vertices;
+ 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;
+ (*vertices)[num_vertices] = p1;
num_vertices++;
- vertices[num_vertices] = p2;
+ (*vertices)[num_vertices] = p2;
num_vertices++;
- vertices[num_vertices] = p3;
+ (*vertices)[num_vertices] = p3;
num_vertices++;
if (num_tris >= num_tris_allocated) {
- Triangle* temp = new Triangle[num_tris_allocated * 2];
- if (!temp) {
+ std::shared_ptr<std::vector<Triangle>> temp(
+ new std::vector<Triangle>(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;
}
- std::copy(tri_indices, tri_indices + num_tris, temp);
- delete[] tri_indices;
+ 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((Triangle::index_type)offset,
- (Triangle::index_type)(offset + 1),
- (Triangle::index_type)(offset + 2));
+ (*tri_indices)[num_tris].set((Triangle::index_type)offset,
+ (Triangle::index_type)(offset + 1),
+ (Triangle::index_type)(offset + 2));
num_tris++;
return BVH_OK;
@@ -373,7 +401,7 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
if (build_state == BVH_BUILD_STATE_PROCESSED) {
- std::cerr << "BVH Warning! Call addSubModel() in a wrong order. "
+ 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;
@@ -383,24 +411,26 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
const unsigned int num_vertices_to_add = (unsigned int)ps.size();
if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
- Vec3f* temp =
- new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
- if (!temp) {
+ std::shared_ptr<std::vector<Vec3f>> temp(new std::vector<Vec3f>(
+ 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;
}
- std::copy(vertices, vertices + num_vertices, temp);
- delete[] vertices;
+ 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<Vec3f>& vertices_ = *vertices;
for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
- vertices[num_vertices] = ps[i];
+ vertices_[num_vertices] = ps[i];
num_vertices++;
}
@@ -410,7 +440,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
const std::vector<Triangle>& ts) {
if (build_state == BVH_BUILD_STATE_PROCESSED) {
- std::cerr << "BVH Warning! Call addSubModel() in a wrong order. "
+ 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;
@@ -420,17 +450,18 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
const unsigned int num_vertices_to_add = (unsigned int)ps.size();
if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
- Vec3f* temp =
- new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
- if (!temp) {
+ std::shared_ptr<std::vector<Vec3f>> temp(new std::vector<Vec3f>(
+ 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;
}
- std::copy(vertices, vertices + num_vertices, temp);
- delete[] vertices;
+ 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;
@@ -438,32 +469,36 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
const unsigned int offset = num_vertices;
+ std::vector<Vec3f>& vertices_ = *vertices;
for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
- vertices[num_vertices] = ps[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) {
- Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1];
- if (!temp) {
+ std::shared_ptr<std::vector<Triangle>> temp(new std::vector<Triangle>(
+ 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;
}
- std::copy(tri_indices, tri_indices + num_tris, temp);
- delete[] tri_indices;
+ 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<Triangle>& tri_indices_ = *tri_indices;
for (size_t i = 0; i < (size_t)num_tris_to_add; ++i) {
const Triangle& t = ts[i];
- tri_indices[num_tris].set(t[0] + (size_t)offset, t[1] + (size_t)offset,
- t[2] + (size_t)offset);
+ tri_indices_[num_tris].set(t[0] + (size_t)offset, t[1] + (size_t)offset,
+ t[2] + (size_t)offset);
num_tris++;
}
@@ -487,36 +522,46 @@ int BVHModelBase::endModel() {
if (num_tris_allocated > num_tris) {
if (num_tris > 0) {
- Triangle* new_tris = new Triangle[num_tris];
- if (!new_tris) {
+ std::shared_ptr<std::vector<Triangle>> new_tris(
+ new std::vector<Triangle>(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;
}
- std::copy(tri_indices, tri_indices + num_tris, new_tris);
- delete[] tri_indices;
+
+ 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 {
- delete[] tri_indices;
- tri_indices = NULL;
+ tri_indices.reset();
num_tris = num_tris_allocated = 0;
}
}
if (num_vertices_allocated > num_vertices) {
- Vec3f* new_vertices = new Vec3f[num_vertices];
- if (!new_vertices) {
- std::cerr
- << "BVH Error! Out of memory for vertices array in endModel() call!"
- << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
+ if (num_vertices > 0) {
+ std::shared_ptr<std::vector<Vec3f>> new_vertices(
+ new std::vector<Vec3f>(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;
}
- std::copy(vertices, vertices + num_vertices, new_vertices);
- delete[] vertices;
- vertices = new_vertices;
- num_vertices_allocated = num_vertices;
}
// construct BVH tree
@@ -538,8 +583,7 @@ int BVHModelBase::beginReplaceModel() {
return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
}
- if (prev_vertices) delete[] prev_vertices;
- prev_vertices = NULL;
+ if (prev_vertices.get()) prev_vertices.reset();
num_vertex_updated = 0;
@@ -557,7 +601,7 @@ int BVHModelBase::replaceVertex(const Vec3f& p) {
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
- vertices[num_vertex_updated] = p;
+ (*vertices)[num_vertex_updated] = p;
num_vertex_updated++;
return BVH_OK;
@@ -573,11 +617,11 @@ int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2,
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
- vertices[num_vertex_updated] = p1;
+ (*vertices)[num_vertex_updated] = p1;
num_vertex_updated++;
- vertices[num_vertex_updated] = p2;
+ (*vertices)[num_vertex_updated] = p2;
num_vertex_updated++;
- vertices[num_vertex_updated] = p3;
+ (*vertices)[num_vertex_updated] = p3;
num_vertex_updated++;
return BVH_OK;
}
@@ -591,8 +635,9 @@ int BVHModelBase::replaceSubModel(const std::vector<Vec3f>& ps) {
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
+ std::vector<Vec3f>& vertices_ = *vertices;
for (unsigned int i = 0; i < ps.size(); ++i) {
- vertices[num_vertex_updated] = ps[i];
+ vertices_[num_vertex_updated] = ps[i];
num_vertex_updated++;
}
return BVH_OK;
@@ -635,13 +680,13 @@ int BVHModelBase::beginUpdateModel() {
return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
}
- if (prev_vertices) {
- Vec3f* temp = prev_vertices;
+ if (prev_vertices.get()) {
+ std::shared_ptr<std::vector<Vec3f>> temp = prev_vertices;
prev_vertices = vertices;
vertices = temp;
} else {
prev_vertices = vertices;
- vertices = new Vec3f[num_vertices];
+ vertices.reset(new std::vector<Vec3f>(num_vertices));
}
num_vertex_updated = 0;
@@ -660,7 +705,7 @@ int BVHModelBase::updateVertex(const Vec3f& p) {
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
- vertices[num_vertex_updated] = p;
+ (*vertices)[num_vertex_updated] = p;
num_vertex_updated++;
return BVH_OK;
@@ -676,11 +721,11 @@ int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2,
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
- vertices[num_vertex_updated] = p1;
+ (*vertices)[num_vertex_updated] = p1;
num_vertex_updated++;
- vertices[num_vertex_updated] = p2;
+ (*vertices)[num_vertex_updated] = p2;
num_vertex_updated++;
- vertices[num_vertex_updated] = p3;
+ (*vertices)[num_vertex_updated] = p3;
num_vertex_updated++;
return BVH_OK;
}
@@ -694,8 +739,9 @@ int BVHModelBase::updateSubModel(const std::vector<Vec3f>& ps) {
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
+ std::vector<Vec3f>& vertices_ = *vertices;
for (unsigned int i = 0; i < ps.size(); ++i) {
- vertices[num_vertex_updated] = ps[i];
+ vertices_[num_vertex_updated] = ps[i];
num_vertex_updated++;
}
return BVH_OK;
@@ -735,15 +781,16 @@ int BVHModelBase::endUpdateModel(bool refit, bool bottomup) {
void BVHModelBase::computeLocalAABB() {
AABB aabb_;
+ const std::vector<Vec3f>& vertices_ = *vertices;
for (unsigned int i = 0; i < num_vertices; ++i) {
- aabb_ += vertices[i];
+ aabb_ += vertices_[i];
}
aabb_center = aabb_.center();
aabb_radius = 0;
for (unsigned int i = 0; i < num_vertices; ++i) {
- FCL_REAL r = (aabb_center - vertices[i]).squaredNorm();
+ FCL_REAL r = (aabb_center - vertices_[i]).squaredNorm();
if (r > aabb_radius) aabb_radius = r;
}
@@ -759,16 +806,12 @@ BVHModel<BV>::BVHModel()
bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
bv_fitter(new BVFitter<BV>()),
num_bvs_allocated(0),
- primitive_indices(NULL),
- bvs(NULL),
num_bvs(0) {}
template <typename BV>
void BVHModel<BV>::deleteBVs() {
- delete[] bvs;
- bvs = NULL;
- delete[] primitive_indices;
- primitive_indices = NULL;
+ bvs.reset();
+ primitive_indices.reset();
num_bvs_allocated = num_bvs = 0;
}
@@ -781,9 +824,10 @@ bool BVHModel<BV>::allocateBVs() {
else
num_bvs_to_be_allocated = 2 * num_tris - 1;
- bvs = new BVNode<BV>[num_bvs_to_be_allocated];
- primitive_indices = new unsigned int[num_bvs_to_be_allocated];
- if (!bvs || !primitive_indices) {
+ bvs.reset(new bv_node_vector_t(num_bvs_to_be_allocated));
+ primitive_indices.reset(
+ new std::vector<unsigned int>(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;
@@ -814,9 +858,11 @@ int BVHModel<BV>::memUsage(const bool msg) const {
template <typename BV>
int BVHModel<BV>::buildTree() {
// set BVFitter
- bv_fitter->set(vertices, tri_indices, getModelType());
+ Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL;
+ Triangle* 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());
+ bv_splitter->set(vertices_, tri_indices_, getModelType());
num_bvs = 1;
@@ -833,7 +879,8 @@ int BVHModel<BV>::buildTree() {
return BVH_ERR_UNSUPPORTED_FUNCTION;
}
- for (unsigned int i = 0; i < num_primitives; ++i) primitive_indices[i] = i;
+ std::vector<unsigned int>& 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();
@@ -846,8 +893,9 @@ template <typename BV>
int BVHModel<BV>::recursiveBuildTree(int bv_id, unsigned int first_primitive,
unsigned int num_primitives) {
BVHModelType type = getModelType();
- BVNode<BV>* bvnode = bvs + bv_id;
- unsigned int* cur_primitive_indices = primitive_indices + first_primitive;
+ BVNode<BV>* 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);
@@ -864,15 +912,17 @@ int BVHModel<BV>::recursiveBuildTree(int bv_id, unsigned int first_primitive,
num_bvs += 2;
unsigned int c1 = 0;
+ const std::vector<Vec3f>& vertices_ = *vertices;
+ const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_primitives; ++i) {
Vec3f p;
if (type == BVH_MODEL_POINTCLOUD)
- p = vertices[cur_primitive_indices[i]];
+ p = vertices_[cur_primitive_indices[i]];
else if (type == BVH_MODEL_TRIANGLES) {
- const Triangle& t = tri_indices[cur_primitive_indices[i]];
- const Vec3f& p1 = vertices[t[0]];
- const Vec3f& p2 = vertices[t[1]];
- const Vec3f& p3 = vertices[t[2]];
+ const Triangle& t = tri_indices_[cur_primitive_indices[i]];
+ const Vec3f& p1 = vertices_[t[0]];
+ const Vec3f& p2 = vertices_[t[1]];
+ const Vec3f& p3 = vertices_[t[2]];
p = (p1 + p2 + p3) / 3.;
} else {
@@ -932,31 +982,32 @@ int BVHModel<BV>::refitTree_bottomup() {
template <typename BV>
int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
- BVNode<BV>* bvnode = bvs + bv_id;
+ BVNode<BV>* 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) {
+ if (prev_vertices.get()) {
Vec3f v[2];
- v[0] = prev_vertices[primitive_id];
- v[1] = vertices[primitive_id];
+ v[0] = (*prev_vertices)[static_cast<size_t>(primitive_id)];
+ v[1] = (*vertices)[static_cast<size_t>(primitive_id)];
fit(v, 2, bv);
} else
- fit(vertices + primitive_id, 1, bv);
+ fit(vertices->data() + primitive_id, 1, bv);
bvnode->bv = bv;
} else if (type == BVH_MODEL_TRIANGLES) {
BV bv;
- const Triangle& triangle = tri_indices[primitive_id];
+ const Triangle& triangle =
+ (*tri_indices)[static_cast<size_t>(primitive_id)];
- if (prev_vertices) {
+ if (prev_vertices.get()) {
Vec3f v[6];
for (Triangle::index_type i = 0; i < 3; ++i) {
- v[i] = prev_vertices[triangle[i]];
- v[i + 3] = vertices[triangle[i]];
+ v[i] = (*prev_vertices)[triangle[i]];
+ v[i + 3] = (*vertices)[triangle[i]];
}
fit(v, 6, bv);
@@ -967,7 +1018,7 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
// bvnode->num_primitives);
Vec3f v[3];
for (int i = 0; i < 3; ++i) {
- v[i] = vertices[triangle[(Triangle::index_type)i]];
+ v[i] = (*vertices)[triangle[(Triangle::index_type)i]];
}
fit(v, 3, bv);
@@ -981,7 +1032,8 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
} else {
recursiveRefitTree_bottomup(bvnode->leftChild());
recursiveRefitTree_bottomup(bvnode->rightChild());
- bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv;
+ bvnode->bv = (*bvs)[static_cast<size_t>(bvnode->leftChild())].bv +
+ (*bvs)[static_cast<size_t>(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 =
@@ -993,11 +1045,16 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
template <typename BV>
int BVHModel<BV>::refitTree_topdown() {
- bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType());
+ Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL;
+ Vec3f* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL;
+ Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
+ bv_fitter->set(vertices_, prev_vertices_, tri_indices_, getModelType());
+ BVNode<BV>* 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 bv = bv_fitter->fit(primitive_indices_ + bvs_[i].first_primitive,
+ bvs_[i].num_primitives);
+ bvs_[i].bv = bv;
}
bv_fitter->clear();
@@ -1008,11 +1065,14 @@ int BVHModel<BV>::refitTree_topdown() {
template <>
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
const Vec3f& parent_c) {
- OBB& obb = bvs[bv_id].bv;
- if (!bvs[bv_id].isLeaf()) {
- makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To);
-
- makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To);
+ bv_node_vector_t& bvs_ = *bvs;
+ OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv;
+ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
+ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
+ obb.axes, obb.To);
+
+ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
+ obb.axes, obb.To);
}
// make self parent relative
@@ -1026,11 +1086,14 @@ void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
template <>
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
const Vec3f& parent_c) {
- RSS& rss = bvs[bv_id].bv;
- if (!bvs[bv_id].isLeaf()) {
- makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axes, rss.Tr);
-
- makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axes, rss.Tr);
+ bv_node_vector_t& bvs_ = *bvs;
+ RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv;
+ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
+ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
+ rss.axes, rss.Tr);
+
+ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
+ rss.axes, rss.Tr);
}
// make self parent relative
@@ -1045,12 +1108,15 @@ template <>
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
Matrix3f& parent_axes,
const Vec3f& parent_c) {
- OBB& obb = bvs[bv_id].bv.obb;
- RSS& rss = bvs[bv_id].bv.rss;
- if (!bvs[bv_id].isLeaf()) {
- makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To);
-
- makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To);
+ bv_node_vector_t& bvs_ = *bvs;
+ OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv.obb;
+ RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv.rss;
+ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
+ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
+ obb.axes, obb.To);
+
+ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
+ obb.axes, obb.To);
}
// make self parent relative
@@ -1088,23 +1154,23 @@ NODE_TYPE BVHModel<OBBRSS>::getNodeType() const {
}
template <>
-NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const {
+NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const {
return BV_KDOP16;
}
template <>
-NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const {
+NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const {
return BV_KDOP18;
}
template <>
-NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const {
+NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const {
return BV_KDOP24;
}
-template class BVHModel<KDOP<16> >;
-template class BVHModel<KDOP<18> >;
-template class BVHModel<KDOP<24> >;
+template class BVHModel<KDOP<16>>;
+template class BVHModel<KDOP<18>>;
+template class BVHModel<KDOP<24>>;
template class BVHModel<OBB>;
template class BVHModel<AABB>;
template class BVHModel<RSS>;
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index 12c3f0009960f3a46f10bbe0212541c478b41e5f..0302744e48cb029e5229d769e86d96838097cd86 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -62,22 +62,24 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
std::vector<bool> keep_vertex(model.num_vertices, false);
std::vector<bool> keep_tri(model.num_tris, false);
unsigned int ntri = 0;
+ const std::vector<Vec3f>& model_vertices_ = *(model.vertices);
+ const std::vector<Triangle>& model_tri_indices_ = *(model.tri_indices);
for (unsigned int i = 0; i < model.num_tris; ++i) {
- const Triangle& t = model.tri_indices[i];
+ const Triangle& 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]])) {
+ if (aabb.contain(q * model_vertices_[t[j]])) {
keep_this_tri = true;
break;
}
}
- const Vec3f& p0 = model.vertices[t[0]];
- const Vec3f& p1 = model.vertices[t[1]];
- const Vec3f& p2 = model.vertices[t[2]];
+ const Vec3f& p0 = model_vertices_[t[0]];
+ const Vec3f& p1 = model_vertices_[t[1]];
+ const Vec3f& p2 = model_vertices_[t[2]];
Vec3f c1, c2, normal;
FCL_REAL distance;
if (!keep_this_tri &&
@@ -99,20 +101,22 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices));
std::vector<unsigned int> idxConversion(model.num_vertices);
assert(new_model->num_vertices == 0);
+ std::vector<Vec3f>& 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_vertices_[new_model->num_vertices] = model_vertices_[i];
new_model->num_vertices++;
}
}
assert(new_model->num_tris == 0);
+ std::vector<Triangle>& 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_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++;
}
}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 474e4a3310915be2f0b22760611e089d177f1758..4cfc36f957e19cdb3d5125a086d0cbee5dd66836 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,7 +1,7 @@
#
# Software License Agreement (BSD License)
#
-# Copyright (c) 2014, 2020 CNRS-LAAS
+# Copyright (c) 2014, 2020-2024 CNRS-LAAS INRIA
# Author: Florent Lamiraux
# All rights reserved.
#
@@ -78,6 +78,9 @@ set(${LIBRARY_NAME}_SOURCES
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/triangle_halfspace.cpp
intersect.cpp
@@ -93,6 +96,7 @@ set(${LIBRARY_NAME}_SOURCES
mesh_loader/assimp.cpp
mesh_loader/loader.cpp
hfield.cpp
+ serialization/serialization.cpp
)
if(HPP_FCL_HAS_OCTOMAP)
@@ -178,6 +182,10 @@ if(UNIX)
set_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH "${${PROJECT_NAME}_INSTALL_RPATH}")
endif()
+IF(MSVC)
+ target_compile_options(${PROJECT_NAME} PUBLIC "/bigobj")
+ENDIF()
+
# IDE sources and headers sorting
ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES)
ADD_HEADER_GROUP(PROJECT_HEADERS_FULL_PATH)
diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp
index e956012394401b4750e8a4319a3249a85a21db20..4da0c995af2667b9d64ddc21907410bbc1d0118f 100644
--- a/src/broadphase/broadphase_SaP.cpp
+++ b/src/broadphase/broadphase_SaP.cpp
@@ -124,10 +124,10 @@ void SaPCollisionManager::registerObjects(
std::sort(
endpoints.begin(), endpoints.end(),
std::bind(std::less<FCL_REAL>(),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
+ std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_1, coord),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
+ std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_2, coord)));
@@ -181,28 +181,29 @@ void SaPCollisionManager::registerObjects(
//==============================================================================
void SaPCollisionManager::registerObject(CollisionObject* obj) {
- SaPAABB* curr = new SaPAABB;
- curr->cached = obj->getAABB();
- curr->obj = obj;
- curr->lo = new EndPoint;
- curr->lo->minmax = 0;
- curr->lo->aabb = curr;
-
- curr->hi = new EndPoint;
- curr->hi->minmax = 1;
- curr->hi->aabb = curr;
-
+ // 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] = curr->lo;
- curr->lo->prev[coord] = curr->lo->next[coord] = nullptr;
+ 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 = curr->lo;
+ EndPoint* curr_lo = new_sap->lo;
FCL_REAL curr_lo_val = curr_lo->getVal()[coord];
while ((current->getVal()[coord] < curr_lo_val) &&
(current->next[coord] != nullptr))
@@ -211,13 +212,16 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) {
if (current->getVal()[coord] >= curr_lo_val) {
curr_lo->prev[coord] = current->prev[coord];
curr_lo->next[coord] = current;
- if (current->prev[coord] == nullptr)
- elist[coord] = curr_lo;
+ 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;
- } else {
+ 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;
@@ -225,16 +229,16 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) {
}
// now insert hi end point
- current = curr->lo;
+ current = new_sap->lo;
- EndPoint* curr_hi = curr->hi;
+ EndPoint* curr_hi = new_sap->hi;
FCL_REAL curr_hi_val = curr_hi->getVal()[coord];
if (coord == 0) {
while ((current->getVal()[coord] < curr_hi_val) &&
(current->next[coord] != nullptr)) {
- if (current != curr->lo)
- if (current->aabb->cached.overlap(curr->cached))
+ 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];
@@ -248,9 +252,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) {
if (current->getVal()[coord] >= curr_hi_val) {
curr_hi->prev[coord] = current->prev[coord];
curr_hi->next[coord] = current;
- if (current->prev[coord] == nullptr)
- elist[coord] = curr_hi;
- else
+ if (current->prev[coord] != nullptr)
current->prev[coord]->next[coord] = curr_hi;
current->prev[coord] = curr_hi;
@@ -261,9 +263,9 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) {
}
}
- AABB_arr.push_back(curr);
+ AABB_arr.push_back(new_sap);
- obj_aabb_map[obj] = curr;
+ obj_aabb_map[obj] = new_sap;
updateVelist();
}
@@ -275,8 +277,8 @@ void SaPCollisionManager::setup() {
FCL_REAL scale[3];
scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);
- ;
scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
+
int axis = 0;
if (scale[axis] < scale[1]) axis = 1;
if (scale[axis] < scale[2]) axis = 2;
@@ -289,19 +291,18 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) {
SaPAABB* current = updated_aabb;
- Vec3f new_min = current->obj->getAABB().min_;
- Vec3f new_max = current->obj->getAABB().max_;
+ const AABB current_aabb = current->obj->getAABB();
- SaPAABB dummy;
- dummy.cached = current->obj->getAABB();
+ const Vec3f& new_min = current_aabb.min_;
+ const Vec3f& 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((size_t)coord) > new_min[coord])
+ if (current->lo->getVal(coord) > new_min[coord])
direction = -1;
- else if (current->lo->getVal((size_t)coord) < new_min[coord])
+ else if (current->lo->getVal(coord) < new_min[coord])
direction = 1;
else
direction = 0;
@@ -309,11 +310,10 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) {
if (direction == -1) {
// first update the "lo" endpoint of the interval
if (current->lo->prev[coord] != nullptr) {
- temp = current->lo;
- while ((temp != nullptr) &&
- (temp->getVal((size_t)coord) > new_min[coord])) {
+ temp = current->lo->prev[coord];
+ while ((temp != nullptr) && (temp->getVal(coord) > new_min[coord])) {
if (temp->minmax == 1)
- if (temp->aabb->cached.overlap(dummy.cached))
+ if (temp->aabb->cached.overlap(current_aabb))
addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
temp = temp->prev[coord];
}
@@ -335,40 +335,46 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) {
}
}
- current->lo->getVal((size_t)coord) = new_min[coord];
+ // Update the value of the lower bound along axis coord
+ current->lo->getVal(coord) = new_min[coord];
// update hi end point
- temp = current->hi;
- while (temp->getVal((size_t)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];
- }
+ 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;
- 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->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((size_t)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;
+ temp = current->hi->next[coord];
while ((temp->next[coord] != nullptr) &&
- (temp->getVal((size_t)coord) < new_max[coord])) {
+ (temp->getVal(coord) < new_max[coord])) {
if (temp->minmax == 0)
- if (temp->aabb->cached.overlap(dummy.cached))
+ if (temp->aabb->cached.overlap(current_aabb))
addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
temp = temp->next[coord];
}
- if (temp->getVal((size_t)coord) < new_max[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;
@@ -384,31 +390,34 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) {
}
}
- current->hi->getVal((size_t)coord) = new_max[coord];
+ current->hi->getVal(coord) = new_max[coord];
// then, update the "lo" endpoint of the interval.
- temp = current->lo;
+ if (current->lo->next[coord] != nullptr) {
+ temp = current->lo->next[coord];
- while (temp->getVal((size_t)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];
- }
+ 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((size_t)coord) = new_min[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];
+ }
}
}
}
@@ -512,10 +521,10 @@ bool SaPCollisionManager::collide_(CollisionObject* obj,
const auto res_it = std::upper_bound(
velist[axis].begin(), velist[axis].end(), &dummy,
std::bind(std::less<FCL_REAL>(),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
+ std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_1, axis),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
+ std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_2, axis)));
@@ -526,8 +535,7 @@ bool SaPCollisionManager::collide_(CollisionObject* obj,
while (pos != end_pos) {
if (pos->aabb->obj != obj) {
- if ((pos->minmax == 0) &&
- (pos->aabb->hi->getVal((size_t)axis) >= min_val)) {
+ 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;
}
@@ -607,10 +615,10 @@ bool SaPCollisionManager::distance_(CollisionObject* obj,
const auto res_it = std::upper_bound(
velist[axis].begin(), velist[axis].end(), &dummy,
std::bind(std::less<FCL_REAL>(),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
+ std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_1, axis),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
+ std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_2, axis)));
@@ -622,8 +630,7 @@ bool SaPCollisionManager::distance_(CollisionObject* obj,
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((size_t)axis) >= min_val)) {
+ 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_) {
@@ -789,19 +796,19 @@ Vec3f& SaPCollisionManager::EndPoint::getVal() {
}
//==============================================================================
-FCL_REAL SaPCollisionManager::EndPoint::getVal(size_t i) const {
+FCL_REAL SaPCollisionManager::EndPoint::getVal(int i) const {
if (minmax)
- return aabb->cached.max_[(int)i];
+ return aabb->cached.max_[i];
else
- return aabb->cached.min_[(int)i];
+ return aabb->cached.min_[i];
}
//==============================================================================
-FCL_REAL& SaPCollisionManager::EndPoint::getVal(size_t i) {
+FCL_REAL& SaPCollisionManager::EndPoint::getVal(int i) {
if (minmax)
- return aabb->cached.max_[(int)i];
+ return aabb->cached.max_[i];
else
- return aabb->cached.min_[(int)i];
+ return aabb->cached.min_[i];
}
//==============================================================================
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
index ab36051c7f69d6b0c56dcc359828c50953e35fd6..9338cc42434d704194f8028f65c48f0d2c60da73 100644
--- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp
+++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
@@ -39,7 +39,7 @@
#include <limits>
-#if HPP_FCL_HAVE_OCTOMAP
+#ifdef HPP_FCL_HAVE_OCTOMAP
#include "hpp/fcl/octree.h"
#endif
@@ -515,7 +515,8 @@ void DynamicAABBTreeCollisionManager::update() {
DynamicAABBNode* node = it->second;
node->bv = obj->getAABB();
if (node->bv.volume() <= 0.)
- throw std::invalid_argument("The bounding volume has a negative volume.");
+ HPP_FCL_THROW_PRETTY("The bounding volume has a negative volume.",
+ std::invalid_argument)
}
dtree.refit();
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
index 2bc5cab0db2f1fb27358f0e1f11857378305a8b5..4921408372dd16c780f64029559d1f1073c36e85 100644
--- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
+++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
@@ -37,7 +37,7 @@
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
-#if HPP_FCL_HAVE_OCTOMAP
+#ifdef HPP_FCL_HAVE_OCTOMAP
#include "hpp/fcl/octree.h"
#endif
namespace hpp {
diff --git a/src/broadphase/default_broadphase_callbacks.cpp b/src/broadphase/default_broadphase_callbacks.cpp
index d76cd28ed47ed13fac8c328ee784bf93a968d24f..ea88687d4edad880639477d9205e296731907c46 100644
--- a/src/broadphase/default_broadphase_callbacks.cpp
+++ b/src/broadphase/default_broadphase_callbacks.cpp
@@ -112,6 +112,11 @@ CollisionCallBackCollect::getCollisionPairs() const {
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();
diff --git a/src/collision.cpp b/src/collision.cpp
index 244aee4d6f0e4d6246c7e946824fa5dc44a4fc59..52fc3ed1c43f8563c1e5c1487b9495e6ee2c0b68 100644
--- a/src/collision.cpp
+++ b/src/collision.cpp
@@ -56,6 +56,7 @@ void CollisionResult::swapObjects() {
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;
}
}
@@ -70,7 +71,7 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result) {
- // If securit margin is set to -infinity, return that there is no collision
+ // If security margin is set to -infinity, return that there is no collision
if (request.security_margin == -std::numeric_limits<FCL_REAL>::infinity()) {
result.clear();
return false;
@@ -104,6 +105,8 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
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]) {
@@ -169,9 +172,18 @@ std::size_t ComputeCollision::run(const Transform3f& tf1,
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);
}
+
+ if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+ solver.enable_cached_guess) {
+ result.cached_gjk_guess = solver.cached_guess;
+ result.cached_support_func_guess = solver.support_func_cached_guess;
+ }
+
return res;
}
@@ -191,12 +203,6 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1,
} else
res = run(tf1, tf2, request, result);
- if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
- solver.enable_cached_guess) {
- result.cached_gjk_guess = solver.cached_guess;
- result.cached_support_func_guess = solver.support_func_cached_guess;
- }
-
return res;
}
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 997c11444dc7b0b9c190286aab64239c11cef6d5..73d702afe350c73969e8b5a02a43e1820cf87be0 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -319,8 +319,10 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() {
&ShapeShapeCollide<Ellipsoid, Cylinder>;
collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] =
&ShapeShapeCollide<Ellipsoid, ConvexBase>;
- // TODO Louis: Ellipsoid - Plane
- // TODO Louis: Ellipsoid - Halfspace
+ collision_matrix[GEOM_ELLIPSOID][GEOM_PLANE] =
+ &ShapeShapeCollide<Ellipsoid, Plane>;
+ collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] =
+ &ShapeShapeCollide<Ellipsoid, Halfspace>;
collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] =
&ShapeShapeCollide<Ellipsoid, Ellipsoid>;
@@ -403,7 +405,8 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() {
collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane>;
collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] =
&ShapeShapeCollide<Plane, Halfspace>;
- // TODO Louis: Ellipsoid - Plane
+ collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] =
+ &ShapeShapeCollide<Plane, Ellipsoid>;
collision_matrix[GEOM_HALFSPACE][GEOM_BOX] =
&ShapeShapeCollide<Halfspace, Box>;
@@ -421,7 +424,8 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() {
&ShapeShapeCollide<Halfspace, Plane>;
collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] =
&ShapeShapeCollide<Halfspace, Halfspace>;
- // TODO Louis: Ellipsoid - Halfspace
+ collision_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] =
+ &ShapeShapeCollide<Halfspace, Ellipsoid>;
collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box>::collide;
collision_matrix[BV_AABB][GEOM_SPHERE] =
@@ -657,6 +661,10 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() {
&OctreeCollide<OcTree, BVHModel<KDOP<18> > >;
collision_matrix[GEOM_OCTREE][BV_KDOP24] =
&OctreeCollide<OcTree, BVHModel<KDOP<24> > >;
+ collision_matrix[GEOM_OCTREE][HF_AABB] =
+ &OctreeCollide<OcTree, HeightField<AABB> >;
+ collision_matrix[GEOM_OCTREE][HF_OBBRSS] =
+ &OctreeCollide<OcTree, HeightField<OBBRSS> >;
collision_matrix[BV_AABB][GEOM_OCTREE] =
&OctreeCollide<BVHModel<AABB>, OcTree>;
@@ -672,6 +680,10 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() {
&OctreeCollide<BVHModel<KDOP<18> >, OcTree>;
collision_matrix[BV_KDOP24][GEOM_OCTREE] =
&OctreeCollide<BVHModel<KDOP<24> >, OcTree>;
+ collision_matrix[HF_AABB][GEOM_OCTREE] =
+ &OctreeCollide<HeightField<AABB>, OcTree>;
+ collision_matrix[HF_OBBRSS][GEOM_OCTREE] =
+ &OctreeCollide<HeightField<OBBRSS>, OcTree>;
#endif
}
// template struct CollisionFunctionMatrix;
diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp
index 586f6f6352376124a8bf73a91561bf6c38900b3d..05fd42969192210617b974b995175a0c970f276b 100644
--- a/src/collision_utility.cpp
+++ b/src/collision_utility.cpp
@@ -60,7 +60,8 @@ CollisionGeometry* extractBVH(const CollisionGeometry* model,
case BV_KDOP24:
return extractBVHtpl<KDOP<24> >(model, pose, aabb);
default:
- throw std::runtime_error("Unknown type of bounding volume");
+ HPP_FCL_THROW_PRETTY("Unknown type of bounding volume",
+ std::runtime_error);
}
}
} // namespace details
@@ -72,8 +73,9 @@ CollisionGeometry* extract(const CollisionGeometry* model,
return details::extractBVH(model, pose, aabb);
// case OT_GEOM: return model;
default:
- throw std::runtime_error(
- "Extraction is not implemented for this type of object");
+ HPP_FCL_THROW_PRETTY(
+ "Extraction is not implemented for this type of object",
+ std::runtime_error);
}
}
} // namespace fcl
diff --git a/src/distance.cpp b/src/distance.cpp
index 1199cdca2aa1008d9ed24bb7fa84f322f24fd495..bc7f542bdbcf20b5c7d8d7822f0cf828c366a39c 100644
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -88,9 +88,8 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* tmpo = result.o1;
result.o1 = result.o2;
result.o2 = tmpo;
- Vec3f tmpn(result.nearest_points[0]);
- result.nearest_points[0] = result.nearest_points[1];
- result.nearest_points[1] = tmpn;
+ result.nearest_points[0].swap(result.nearest_points[1]);
+ result.normal *= -1;
}
}
} else {
@@ -153,6 +152,7 @@ FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2,
if (request.enable_nearest_points) {
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);
diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp
index af126ce7355da4d6dcbd580dc6d96835e4bc4422..16caadd1c9fe7d13735510ede01e89e8f89afce7 100644
--- a/src/distance/capsule_capsule.cpp
+++ b/src/distance/capsule_capsule.cpp
@@ -80,7 +80,7 @@ template <>
FCL_REAL ShapeShapeDistance<Capsule, Capsule>(
const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
- const DistanceRequest& request, DistanceResult& result) {
+ const DistanceRequest& /*request*/, DistanceResult& result) {
const Capsule* capsule1 = static_cast<const Capsule*>(o1);
const Capsule* capsule2 = static_cast<const Capsule*>(o2);
@@ -153,18 +153,15 @@ FCL_REAL ShapeShapeDistance<Capsule, Capsule>(
// witness points achieving the distance between the two segments
FCL_REAL distance = (w1 - w2).norm();
- const Vec3f normal = (w1 - w2) / distance;
- result.normal = normal;
// capsule spcecific distance computation
distance = distance - (radius1 + radius2);
result.min_distance = distance;
- // witness points for the capsules
- if (request.enable_nearest_points) {
- result.nearest_points[0] = w1 - radius1 * normal;
- result.nearest_points[1] = w2 + radius2 * normal;
- }
+ // Normal points from o1 to o2
+ result.normal = (w2 - w1).normalized();
+ result.nearest_points[0] = w1 + radius1 * result.normal;
+ result.nearest_points[1] = w2 - radius2 * result.normal;
return distance;
}
diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a9088da2acad429885413ee56b715258d9dfce9
--- /dev/null
+++ b/src/distance/ellipsoid_halfspace.cpp
@@ -0,0 +1,85 @@
+/*
+ * 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 <cmath>
+#include <limits>
+#include <hpp/fcl/math/transform.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+
+#include <hpp/fcl/internal/shape_shape_func.h>
+#include "../narrowphase/details.h"
+
+namespace hpp {
+namespace fcl {
+struct GJKSolver;
+
+template <>
+FCL_REAL ShapeShapeDistance<Ellipsoid, Halfspace>(
+ const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+ const DistanceRequest&, DistanceResult& result) {
+ const Ellipsoid& s1 = static_cast<const Ellipsoid&>(*o1);
+ const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
+ details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance,
+ result.nearest_points[1], result.nearest_points[0],
+ result.normal);
+ result.o1 = o1;
+ result.o2 = o2;
+ result.b1 = -1;
+ result.b2 = -1;
+ result.normal = -result.normal;
+ return result.min_distance;
+}
+
+template <>
+FCL_REAL ShapeShapeDistance<Halfspace, Ellipsoid>(
+ const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+ const DistanceRequest&, DistanceResult& result) {
+ const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
+ const Ellipsoid& s2 = static_cast<const Ellipsoid&>(*o2);
+ details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance,
+ result.nearest_points[0], result.nearest_points[1],
+ result.normal);
+ result.o1 = o1;
+ result.o2 = o2;
+ result.b1 = -1;
+ result.b2 = -1;
+ return result.min_distance;
+}
+} // namespace fcl
+
+} // namespace hpp
diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e8a4cdfb35d39b88afaf77970beef97806de679d
--- /dev/null
+++ b/src/distance/ellipsoid_plane.cpp
@@ -0,0 +1,85 @@
+/*
+ * 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 <cmath>
+#include <limits>
+#include <hpp/fcl/math/transform.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+
+#include <hpp/fcl/internal/shape_shape_func.h>
+#include "../narrowphase/details.h"
+
+namespace hpp {
+namespace fcl {
+struct GJKSolver;
+
+template <>
+FCL_REAL ShapeShapeDistance<Ellipsoid, Plane>(
+ const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+ const DistanceRequest&, DistanceResult& result) {
+ const Ellipsoid& s1 = static_cast<const Ellipsoid&>(*o1);
+ const Plane& s2 = static_cast<const Plane&>(*o2);
+ details::ellipsoidPlaneIntersect(s1, tf1, s2, tf2, result.min_distance,
+ result.nearest_points[0],
+ result.nearest_points[1], result.normal);
+ result.o1 = o1;
+ result.o2 = o2;
+ result.b1 = -1;
+ result.b2 = -1;
+ return result.min_distance;
+}
+
+template <>
+FCL_REAL ShapeShapeDistance<Plane, Ellipsoid>(
+ const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+ const DistanceRequest&, DistanceResult& result) {
+ const Plane& s1 = static_cast<const Plane&>(*o1);
+ const Ellipsoid& s2 = static_cast<const Ellipsoid&>(*o2);
+ details::ellipsoidPlaneIntersect(s2, tf2, s1, tf1, result.min_distance,
+ result.nearest_points[1],
+ result.nearest_points[0], result.normal);
+ result.o1 = o1;
+ result.o2 = o2;
+ result.b1 = -1;
+ result.b2 = -1;
+ result.normal = -result.normal;
+ return result.min_distance;
+}
+} // namespace fcl
+
+} // namespace hpp
diff --git a/src/distance/sphere_capsule.cpp b/src/distance/sphere_capsule.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce4fee0e8494918965807817e7f06cee07346d9d
--- /dev/null
+++ b/src/distance/sphere_capsule.cpp
@@ -0,0 +1,85 @@
+/*
+ * 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 <cmath>
+#include <limits>
+#include <hpp/fcl/math/transform.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+
+#include <hpp/fcl/internal/shape_shape_func.h>
+#include "../narrowphase/details.h"
+
+namespace hpp {
+namespace fcl {
+struct GJKSolver;
+
+template <>
+FCL_REAL ShapeShapeDistance<Sphere, Capsule>(
+ const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+ const DistanceRequest&, DistanceResult& result) {
+ const Sphere& s1 = static_cast<const Sphere&>(*o1);
+ const Capsule& s2 = static_cast<const Capsule&>(*o2);
+ details::sphereCapsuleDistance(s1, tf1, s2, tf2, result.min_distance,
+ result.nearest_points[0],
+ result.nearest_points[1], result.normal);
+ result.o1 = o1;
+ result.o2 = o2;
+ result.b1 = -1;
+ result.b2 = -1;
+ return result.min_distance;
+}
+
+template <>
+FCL_REAL ShapeShapeDistance<Capsule, Sphere>(
+ const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+ const DistanceRequest&, DistanceResult& result) {
+ const Capsule& s1 = static_cast<const Capsule&>(*o1);
+ const Sphere& s2 = static_cast<const Sphere&>(*o2);
+ details::sphereCapsuleDistance(s2, tf2, s1, tf1, result.min_distance,
+ result.nearest_points[1],
+ result.nearest_points[0], result.normal);
+ result.o1 = o1;
+ result.o2 = o2;
+ result.b1 = -1;
+ result.b2 = -1;
+ result.normal = -result.normal;
+ return result.min_distance;
+}
+
+} // namespace fcl
+
+} // namespace hpp
diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp
index 13a2b960b6da10138eded24f79d11627b43e107f..17038166e161a3b63d4349b92e41feee1f274b06 100644
--- a/src/distance/sphere_sphere.cpp
+++ b/src/distance/sphere_sphere.cpp
@@ -74,22 +74,10 @@ FCL_REAL ShapeShapeDistance<Sphere, Sphere>(
FCL_REAL dist = c1c2.norm();
Vec3f unit(0, 0, 0);
if (dist > epsilon) unit = c1c2 / dist;
- FCL_REAL penetrationDepth;
- penetrationDepth = r1 + r2 - dist;
- bool collision = (penetrationDepth >= 0);
- result.min_distance = -penetrationDepth;
- if (collision) {
- // Take contact point at the middle of intersection between each sphere
- // and segment [c1 c2].
- FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2);
- Vec3f contact = center1 + abscissa * unit;
- result.nearest_points[0] = result.nearest_points[1] = contact;
- return result.min_distance;
- } else {
- FCL_REAL abs1(r1), abs2(dist - r2);
- result.nearest_points[0] = center1 + abs1 * unit;
- result.nearest_points[1] = center1 + abs2 * unit;
- }
+ result.min_distance = dist - (r1 + r2);
+ result.normal = unit;
+ result.nearest_points[0] = center1 + r1 * unit;
+ result.nearest_points[1] = center2 - r2 * unit;
return result.min_distance;
}
@@ -101,7 +89,7 @@ std::size_t ShapeShapeCollider<Sphere, Sphere>::run(
const Sphere* s1 = static_cast<const Sphere*>(o1);
const Sphere* s2 = static_cast<const Sphere*>(o2);
- // We assume that capsules are centered at the origin.
+ // We assume that spheres are centered at the origin.
const fcl::Vec3f& center1 = tf1.getTranslation();
const fcl::Vec3f& center2 = tf2.getTranslation();
FCL_REAL r1 = s1->radius;
@@ -110,21 +98,17 @@ std::size_t ShapeShapeCollider<Sphere, Sphere>::run(
Vec3f c1c2 = center2 - center1;
FCL_REAL dist = c1c2.norm();
- Vec3f unit(0, 0, 0);
- if (dist > epsilon) unit = c1c2 / dist;
+ Vec3f normal(0, 0, 0);
+ if (dist > epsilon) normal = c1c2 / dist;
// Unlike in distance computation, we consider the security margin.
FCL_REAL distToCollision = dist - (r1 + r2 + margin);
+ Vec3f p1 = center1 + normal * r1;
+ Vec3f p2 = center2 - normal * r2;
internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
- center1 + unit * r1,
- center2 - unit * r2);
+ p1, p2, normal);
if (distToCollision <= request.collision_distance_threshold) {
- // Take contact point at the middle of intersection between each sphere
- // and segment [c1 c2].
- FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2);
- Vec3f contactPoint = center1 + abscissa * unit;
- Contact contact(o1, o2, -1, -1, contactPoint, unit,
- -(distToCollision + margin));
+ Contact contact(o1, o2, -1, -1, p1, p2, normal, distToCollision + margin);
result.addContact(contact);
return 1;
}
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index ca295473afdf05798403861284d90aec0c8606bd..8bc115da16ce9e3d50ccff0fd89a9fe1bfd892b6 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -66,6 +66,22 @@ FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1,
#endif
+HPP_FCL_LOCAL FCL_REAL distance_function_not_implemented(
+ const CollisionGeometry* o1, const Transform3f& /*tf1*/,
+ const CollisionGeometry* o2, const Transform3f& /*tf2*/,
+ const GJKSolver* /*nsolver*/, const DistanceRequest& /*request*/,
+ DistanceResult& /*result*/) {
+ NODE_TYPE node_type1 = o1->getNodeType();
+ NODE_TYPE node_type2 = o2->getNodeType();
+
+ HPP_FCL_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 <typename T_SH1, typename T_SH2>
FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
@@ -322,8 +338,10 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() {
&ShapeShapeDistance<Ellipsoid, Cylinder>;
distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] =
&ShapeShapeDistance<Ellipsoid, ConvexBase>;
- // TODO Louis: Ellipsoid - Plane
- // TODO Louis: Ellipsoid - Halfspace
+ distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] =
+ &ShapeShapeDistance<Ellipsoid, Plane>;
+ distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] =
+ &ShapeShapeDistance<Ellipsoid, Halfspace>;
distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] =
&ShapeShapeDistance<Ellipsoid, Ellipsoid>;
@@ -406,7 +424,8 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() {
distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane>;
distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] =
&ShapeShapeDistance<Plane, Halfspace>;
- // TODO Louis: Ellipsoid - Plane
+ distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] =
+ &ShapeShapeDistance<Plane, Ellipsoid>;
distance_matrix[GEOM_HALFSPACE][GEOM_BOX] =
&ShapeShapeDistance<Halfspace, Box>;
@@ -424,7 +443,8 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() {
&ShapeShapeDistance<Halfspace, Plane>;
distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] =
&ShapeShapeDistance<Halfspace, Halfspace>;
- // TODO Louis: Ellipsoid - Halfspace
+ distance_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] =
+ &ShapeShapeDistance<Halfspace, Ellipsoid>;
/* AABB distance not implemented */
/*
@@ -645,6 +665,8 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() {
&Distance<BVHModel<KDOP<18> >, OcTree>;
distance_matrix[BV_KDOP24][GEOM_OCTREE] =
&Distance<BVHModel<KDOP<24> >, OcTree>;
+ distance_matrix[HF_AABB][GEOM_OCTREE] = &distance_function_not_implemented;
+ distance_matrix[HF_OBBRSS][GEOM_OCTREE] = &distance_function_not_implemented;
#endif
}
// template struct DistanceFunctionMatrix;
diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp
index 634beba1f5c6973729ed8856a489896d3f75ad36..495ace6024066c5cc3cdc535d0b60242c4dd5036 100644
--- a/src/mesh_loader/assimp.cpp
+++ b/src/mesh_loader/assimp.cpp
@@ -89,12 +89,13 @@ void Loader::load(const std::string& resource_path) {
std::string("Could not load resource ") + resource_path +
std::string("\n") + importer->GetErrorString() + std::string("\n") +
"Hint: the mesh directory may be wrong.");
- throw std::invalid_argument(exception_message);
+ HPP_FCL_THROW_PRETTY(exception_message.c_str(), std::invalid_argument);
}
if (!scene->HasMeshes())
- throw std::invalid_argument(std::string("No meshes found in file ") +
- resource_path);
+ HPP_FCL_THROW_PRETTY(
+ (std::string("No meshes found in file ") + resource_path).c_str(),
+ std::invalid_argument);
}
/**
diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp
index 3153f1a8d2e1446dcf4f1053fd51c1faca768324..cc5543e1ea30bba687a62468a4ea44c9b909e087 100644
--- a/src/mesh_loader/loader.cpp
+++ b/src/mesh_loader/loader.cpp
@@ -85,7 +85,8 @@ BVHModelPtr_t MeshLoader::load(const std::string& filename,
case BV_KDOP24:
return _load<KDOP<24> >(filename, scale);
default:
- throw std::invalid_argument("Unhandled bouding volume type.");
+ HPP_FCL_THROW_PRETTY("Unhandled bouding volume type.",
+ std::invalid_argument);
}
}
@@ -94,8 +95,9 @@ CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) {
shared_ptr<octomap::OcTree> octree(new octomap::OcTree(filename));
return CollisionGeometryPtr_t(new hpp::fcl::OcTree(octree));
#else
- throw std::logic_error(
- "hpp-fcl compiled without OctoMap. Cannot create OcTrees.");
+ HPP_FCL_THROW_PRETTY(
+ "hpp-fcl compiled without OctoMap. Cannot create OcTrees.",
+ std::logic_error);
#endif
}
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
index bcc3b218da988845cf2ba9c0bb70a102b87f6d9f..7baed8c280f4e02f940e5e19cde668cf265ceede 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -4,6 +4,7 @@
* 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-2022, INRIA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -123,7 +124,6 @@ inline bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
p2 = segment_point - normal * s2.radius;
if (dist <= 0) {
- p1 = p2 = .5 * (p1 + p2);
return false;
}
return true;
@@ -179,23 +179,19 @@ inline bool sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
assert(fabs(dist) - (p1 - p2).norm() < eps);
} else {
// Center of sphere is on cylinder boundary
- normal = .5 * (A + B) - p2;
+ normal = p2 - .5 * (A + B);
+ assert(u.dot(normal) >= 0);
normal.normalize();
- p1 = p2;
dist = -r1;
+ p1 = S + r1 * normal;
}
}
} else if (s <= (s2.halfLength * 2)) {
// 0 < s <= s2.lz
normal = -v;
dist = dPS - r1 - r2;
- if (dPS <= r2) {
- // Sphere center is inside cylinder
- p1 = p2 = S;
- } else {
- p2 = P + r2 * v;
- p1 = S - r1 * v;
- }
+ p2 = P + r2 * v;
+ p1 = S - r1 * v;
} else {
// lz < s
if (dPS <= r2) {
@@ -216,16 +212,13 @@ inline bool sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
assert(fabs(dist) - (p1 - p2).norm() < eps);
} else {
// Center of sphere is on cylinder boundary
- normal = .5 * (A + B) - p2;
+ normal = p2 - .5 * (A + B);
normal.normalize();
- p1 = p2;
+ p1 = S + r1 * normal;
dist = -r1;
}
}
}
- if (dist < 0) {
- p1 = p2 = .5 * (p1 + p2);
- }
return (dist > 0);
}
@@ -1384,15 +1377,13 @@ inline bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1,
Halfspace new_s2 = transform(s2, tf2);
const Vec3f& center = tf1.getTranslation();
distance = new_s2.signedDistance(center) - s1.radius;
- if (distance <= 0) {
- normal = -new_s2.n; // pointing from s1 to s2
- p1 = p2 = center - new_s2.n * s1.radius - (distance * 0.5) * new_s2.n;
- return true;
- } else {
- p1 = center - s1.radius * new_s2.n;
- p2 = p1 - distance * new_s2.n;
- return false;
- }
+ normal = -new_s2.n; // pointing from s1 to s2
+ p1 = center - s1.radius * new_s2.n;
+ p2 = p1 - distance * new_s2.n;
+ assert(new_s2.distance(p2) <
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+ if (distance > 0) return false;
+ return true;
}
/// @brief box half space, a, b, c = +/- edge size
@@ -1419,6 +1410,10 @@ inline bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
Vec3f& normal) {
+ // TODO: when witness face of box is parallel to the plane/halfspace,
+ // we may want to return more than one contact point. For example,
+ // we can return the four corners of the box.
+ static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
Halfspace new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
@@ -1429,37 +1424,25 @@ inline bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
// A: scalar products of each side with normal
const Vec3f A(Q.cwiseProduct(s1.halfSide));
+ normal = -new_s2.n;
distance = new_s2.signedDistance(T) - A.lpNorm<1>();
+
+ // compute p1 -> point of the box deepest inside the hyperplane
+ p1 = T;
+ for (Vec3f::Index i = 0; i < 3; ++i) {
+ // scalar product between box axis and hyperplane normal
+ FCL_REAL alpha(R.col(i).dot(new_s2.n));
+ if (alpha > eps) {
+ p1 -= R.col(i) * s1.halfSide[i];
+ } else if (alpha < -eps) {
+ p1 += R.col(i) * s1.halfSide[i];
+ }
+ }
+ p2 = p1 - distance * new_s2.n;
+ assert(new_s2.signedDistance(p2) < 3 * eps);
if (distance > 0) {
- p1.noalias() = T + R * (A.array() > 0).select(s1.halfSide, -s1.halfSide);
- p2.noalias() = p1 - distance * new_s2.n;
return false;
}
-
- /// find deepest point
- Vec3f p(T);
- int sign = 0;
-
- if (std::abs(Q[0] - 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
- std::abs(Q[0] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) {
- sign = (A[0] > 0) ? -1 : 1;
- p += R.col(0) * (s1.halfSide[0] * sign);
- } else if (std::abs(Q[1] - 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
- std::abs(Q[1] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) {
- sign = (A[1] > 0) ? -1 : 1;
- p += R.col(1) * (s1.halfSide[1] * sign);
- } else if (std::abs(Q[2] - 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
- std::abs(Q[2] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) {
- sign = (A[2] > 0) ? -1 : 1;
- p += R.col(2) * (s1.halfSide[2] * sign);
- } else {
- p.noalias() += R * (A.array() > 0).select(-s1.halfSide, s1.halfSide);
- }
-
- /// compute the contact point from the deepest point
- normal = -new_s2.n;
- p1 = p2 = p - new_s2.n * (distance * 0.5);
-
return true;
}
@@ -1468,6 +1451,9 @@ inline bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1,
const Transform3f& tf2,
FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
Vec3f& normal) {
+ static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+ HPP_FCL_ONLY_USED_FOR_DEBUG(eps);
+
Halfspace new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
@@ -1475,19 +1461,22 @@ inline bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1,
Vec3f dir_z = R.col(2);
+ normal = -new_s2.n;
FCL_REAL cosa = dir_z.dot(new_s2.n);
if (std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>()) {
// Capsule parallel to plane
+ // TODO: here we are returning p1 as the middle of the capsule's segment (+
+ // radius) but we might want to return both ends of the capsule instead. For
+ // that we need to implement the possibility of returning multiple contact
+ // points.
FCL_REAL signed_dist = new_s2.signedDistance(T);
distance = signed_dist - s1.radius;
+ p1 = T - s1.radius * new_s2.n;
+ p2 = p1 - distance * new_s2.n;
+ assert(new_s2.distance(p2) < 3 * eps);
if (distance > 0) {
- p1 = T - s1.radius * new_s2.n;
- p2 = p1 - distance * new_s2.n;
return false;
}
-
- normal = -new_s2.n;
- p1 = p2 = T + new_s2.n * (-0.5 * distance - s1.radius);
return true;
} else {
int sign = (cosa > 0) ? -1 : 1;
@@ -1497,15 +1486,12 @@ inline bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1,
FCL_REAL signed_dist = new_s2.signedDistance(p);
distance = signed_dist - s1.radius;
+ p1 = p - s1.radius * new_s2.n;
+ p2 = p1 - distance * new_s2.n;
+ assert(new_s2.distance(p2) < 3 * eps);
if (distance > 0) {
- p1 = T - s1.radius * new_s2.n;
- p2 = p1 - distance * new_s2.n;
return false;
}
- normal = -new_s2.n;
- // deepest point
- Vec3f c = p - new_s2.n * s1.radius;
- p1 = p2 = c - (0.5 * distance) * new_s2.n;
return true;
}
}
@@ -1524,17 +1510,19 @@ inline bool cylinderHalfspaceIntersect(const Cylinder& s1,
Vec3f dir_z = R.col(2);
FCL_REAL cosa = dir_z.dot(new_s2.n);
- if (cosa < halfspaceIntersectTolerance<FCL_REAL>()) {
+ normal = -new_s2.n;
+ if (std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>()) {
+ // Cylinder is parallel to plane.
+ // TODO: we might want to return more than one contact point in this case.
FCL_REAL signed_dist = new_s2.signedDistance(T);
distance = signed_dist - s1.radius;
+ p1 = T - s1.radius * new_s2.n;
+ p2 = p1 - distance * new_s2.n;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
if (distance > 0) {
- // TODO: compute closest points
- p1 = p2 = Vec3f(0, 0, 0);
return false;
}
-
- normal = -new_s2.n;
- p1 = p2 = T - (0.5 * distance + s1.radius) * new_s2.n;
return true;
} else {
Vec3f C = dir_z * cosa - new_s2.n;
@@ -1549,15 +1537,14 @@ inline bool cylinderHalfspaceIntersect(const Cylinder& s1,
int sign = (cosa > 0) ? -1 : 1;
// deepest point
- Vec3f p = T + dir_z * (s1.halfLength * sign) + C;
- distance = new_s2.signedDistance(p);
+ p1 = T + dir_z * (s1.halfLength * sign) + C;
+ distance = new_s2.signedDistance(p1);
+ p2 = p1 - distance * new_s2.n;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
if (distance > 0) {
- // TODO: compute closest points
- p1 = p2 = Vec3f(0, 0, 0);
return false;
} else {
- normal = -new_s2.n;
- p1 = p2 = p - (0.5 * distance) * new_s2.n;
return true;
}
}
@@ -1575,16 +1562,18 @@ inline bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1,
Vec3f dir_z = R.col(2);
FCL_REAL cosa = dir_z.dot(new_s2.n);
- if (cosa < halfspaceIntersectTolerance<FCL_REAL>()) {
+ normal = -new_s2.n;
+ if (std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>()) {
+ // cone axis is parallel to plane
FCL_REAL signed_dist = new_s2.signedDistance(T);
distance = signed_dist - s1.radius;
+ p1 = T - dir_z * (s1.halfLength) - new_s2.n * s1.radius;
+ p2 = p1 - distance * new_s2.n;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
if (distance > 0) {
- p1 = p2 = Vec3f(0, 0, 0);
return false;
} else {
- normal = -new_s2.n;
- p1 = p2 =
- T - dir_z * (s1.halfLength) - new_s2.n * (0.5 * distance + s1.radius);
return true;
}
} else {
@@ -1604,12 +1593,17 @@ inline bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1,
FCL_REAL d1 = new_s2.signedDistance(a1);
FCL_REAL d2 = new_s2.signedDistance(a2);
+ distance = std::min(d1, d2);
+ // TODO: when d1 == d2, we may want to return more than one contact point.
+ // There is an infinite amount but we may want to return a1 and a2 for
+ // example.
+ p1 = ((d1 < d2) ? a1 : a2);
+ p2 = p1 - distance * new_s2.n;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
if (d1 > 0 && d2 > 0)
return false;
else {
- distance = std::min(d1, d2);
- normal = -new_s2.n;
- p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 * distance) * new_s2.n;
return true;
}
}
@@ -1624,8 +1618,9 @@ inline bool convexHalfspaceIntersect(
Vec3f v;
FCL_REAL depth = (std::numeric_limits<FCL_REAL>::max)();
+ const std::vector<Vec3f>& points_ = *(s1.points);
for (unsigned int i = 0; i < s1.num_points; ++i) {
- Vec3f p = tf1.transform(s1.points[i]);
+ Vec3f p = tf1.transform(points_[i]);
FCL_REAL d = new_s2.signedDistance(p);
if (d < depth) {
@@ -1673,18 +1668,17 @@ inline bool halfspaceTriangleIntersect(const Halfspace& s1,
depth = d;
v = p;
}
+
// v is the vertex with minimal projection abscissa (depth) on normal to
// plane,
distance = depth;
+ normal = new_s1.n;
+ p1 = v - depth * normal;
+ p2 = v;
if (depth <= 0) {
- normal = new_s1.n;
- p1 = p2 = v - (0.5 * depth) * new_s1.n;
return true;
- } else {
- p1 = v - depth * new_s1.n;
- p2 = v;
- return false;
}
+ return false;
}
/// @brief return whether plane collides with halfspace
@@ -1810,13 +1804,15 @@ inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
Vec3f& normal) {
Vec3f n_w = tf1.getRotation() * h.n;
+ FCL_REAL d_w = h.d + n_w.dot(tf1.getTranslation());
Vec3f n_2(tf2.getRotation().transpose() * n_w);
int hint = 0;
p2 = getSupport(&s, -n_2, true, hint);
- p2 = tf2.transform(p2);
+ p2.noalias() = tf2.transform(p2);
- dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d;
+ dist = p2.dot(n_w) - d_w;
p1 = p2 - dist * n_w;
+ assert(std::abs(p1.dot(n_w) - d_w) <= 1e-8);
normal = n_w;
return dist <= 0;
@@ -1848,23 +1844,16 @@ inline bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1,
const Vec3f& center = tf1.getTranslation();
FCL_REAL signed_dist = new_s2.signedDistance(center);
distance = std::abs(signed_dist) - s1.radius;
- if (distance <= 0) {
- if (signed_dist > 0)
- normal = -new_s2.n;
- else
- normal = new_s2.n;
- p1 = p2 = center - new_s2.n * signed_dist;
- return true;
- } else {
- if (signed_dist > 0) {
- p1 = center - s1.radius * new_s2.n;
- p2 = center - signed_dist * new_s2.n;
- } else {
- p1 = center + s1.radius * new_s2.n;
- p2 = center + signed_dist * new_s2.n;
- }
- return false;
- }
+ if (signed_dist > 0)
+ normal = -new_s2.n;
+ else
+ normal = new_s2.n;
+ p1 = center + s1.radius * normal;
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+ if (distance > 0) return false;
+ return false;
}
/// @brief box half space, a, b, c = +/- edge size
@@ -1893,66 +1882,39 @@ inline bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1,
const Vec3f A(Q.cwiseProduct(s1.halfSide));
const FCL_REAL signed_dist = new_s2.signedDistance(T);
- distance = std::abs(signed_dist) - A.lpNorm<1>();
- if (distance > 0) {
- // Is the box above or below the plane
- const bool positive = signed_dist > 0;
- // Set p1 at the center of the box
- p1 = T;
- for (Vec3f::Index i = 0; i < 3; ++i) {
- // scalar product between box axis and plane normal
- FCL_REAL alpha((positive ? 1 : -1) * R.col(i).dot(new_s2.n));
- if (alpha > eps) {
- p1 -= R.col(i) * s1.halfSide[i];
- } else if (alpha < -eps) {
- p1 += R.col(i) * s1.halfSide[i];
- }
- }
- p2 = p1 - (positive ? distance : -distance) * new_s2.n;
- assert(new_s2.distance(p2) < 3 * eps);
- return false;
- }
-
- // find the deepest point
- Vec3f p = T;
-
- // when center is on the positive side of the plane, use a, b, c
- // make (R^T n) (a v1 + b v2 + c v3) the minimum
- // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum
- int sign = (signed_dist > 0) ? 1 : -1;
-
- if (std::abs(Q[0] - 1) < planeIntersectTolerance<FCL_REAL>() ||
- std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>()) {
- int sign2 = (A[0] > 0) ? -sign : sign;
- p.noalias() += R.col(0) * (s1.halfSide[0] * sign2);
- } else if (std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() ||
- std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>()) {
- int sign2 = (A[1] > 0) ? -sign : sign;
- p.noalias() += R.col(1) * (s1.halfSide[1] * sign2);
- } else if (std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() ||
- std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>()) {
- int sign2 = (A[2] > 0) ? -sign : sign;
- p.noalias() += R.col(2) * (s1.halfSide[2] * sign2);
- } else {
- Vec3f tmp(sign * R * s1.halfSide);
- p.noalias() += (A.array() > 0).select(-tmp, tmp);
- }
-
- // compute the contact point by project the deepest point onto the plane
if (signed_dist > 0)
normal = -new_s2.n;
else
normal = new_s2.n;
- p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p);
-
+ distance = std::abs(signed_dist) - A.lpNorm<1>();
+ // Is the box above or below the plane
+ const bool positive = signed_dist > 0;
+ // TODO: if the plane is parallel to a face of the box,
+ // we should not return only one corner as witness point p1.
+ // Compute p1, point of the box deepest in the plane
+ p1 = T;
+ for (Vec3f::Index i = 0; i < 3; ++i) {
+ // scalar product between box axis and plane normal
+ // we also take into account which side of the plane the box is
+ FCL_REAL alpha((positive ? 1 : -1) * R.col(i).dot(new_s2.n));
+ if (alpha > eps) {
+ p1 -= R.col(i) * s1.halfSide[i];
+ } else if (alpha < -eps) {
+ p1 += R.col(i) * s1.halfSide[i];
+ }
+ }
+ p2 = p1 - (positive ? distance : -distance) * new_s2.n;
+ assert(new_s2.distance(p2) < 3 * eps);
+ if (distance > 0) {
+ return false;
+ }
return true;
}
/// Taken from book Real Time Collision Detection, from Christer Ericson
-/// @param pb the closest point to the sphere center on the box surface
-/// @param ps when colliding, matches pb, which is inside the sphere.
-/// when not colliding, the closest point on the sphere
-/// @param normal direction of motion of the box
+/// @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 true if the distance is negative (the shape overlaps).
inline bool boxSphereDistance(const Box& b, const Transform3f& tfb,
const Sphere& s, const Transform3f& tfs,
@@ -1997,11 +1959,12 @@ inline bool boxSphereDistance(const Box& b, const Transform3f& tfb,
normal = -Rb.col(axis);
dist = -min_d - s.radius;
}
+ ps = os - s.radius * normal;
if (!outside || dist <= 0) {
- ps = pb;
+ // project point pb onto the box's surface
+ pb = ps - dist * normal;
return true;
} else {
- ps = os - s.radius * normal;
return false;
}
}
@@ -2017,85 +1980,79 @@ inline bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
const Vec3f& T1 = tf1.getTranslation();
Vec3f dir_z = R1.col(2);
+ FCL_REAL cosa = dir_z.dot(new_s2.n);
- // ends of capsule inner segment
- Vec3f a1 = T1 + dir_z * s1.halfLength;
- Vec3f a2 = T1 - dir_z * s1.halfLength;
-
- FCL_REAL d1 = new_s2.signedDistance(a1);
- FCL_REAL d2 = new_s2.signedDistance(a2);
-
- FCL_REAL abs_d1 = std::abs(d1);
- FCL_REAL abs_d2 = std::abs(d2);
-
- // two end points on different side of the plane
- // the contact point is the intersect of axis with the plane
- // the normal is the direction to avoid intersection
- // the depth is the minimum distance to resolve the collision
- if (d1 * d2 < -planeIntersectTolerance<FCL_REAL>()) {
- if (abs_d1 < abs_d2) {
- distance = -abs_d1 - s1.radius;
- p1 = p2 =
- a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2));
- if (d1 < 0)
- normal = -new_s2.n;
- else
- normal = new_s2.n;
- } else {
- distance = -abs_d2 - s1.radius;
- p1 = p2 =
- a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2));
- if (d2 < 0)
- normal = -new_s2.n;
- else
- normal = new_s2.n;
- }
- assert(!p1.hasNaN() && !p2.hasNaN());
- return true;
- }
-
- if (abs_d1 > s1.radius && abs_d2 > s1.radius) {
- // Here both capsule ends are on the same side of the plane
- if (d1 > 0)
+ if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) {
+ // The capsule axis is parallel to the plane.
+ FCL_REAL d = new_s2.signedDistance(T1);
+ distance = std::abs(d) - s1.radius;
+ if (d < 0)
normal = new_s2.n;
else
normal = -new_s2.n;
- if (abs_d1 < abs_d2) {
- distance = abs_d1 - s1.radius;
- p1 = a1 - s1.radius * normal;
- p2 = p1 - distance * normal;
- } else {
- distance = abs_d2 - s1.radius;
- p1 = a2 - s1.radius * normal;
- p2 = p1 - distance * normal;
+ p1 = T1 + s1.radius * normal;
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+ if (distance > 0)
+ return false;
+ else {
+ return true;
}
- assert(!p1.hasNaN() && !p2.hasNaN());
- return false;
} else {
- // Both capsule ends are on the same side of the plane, but one
- // is closer than the capsule radius, hence collision
- distance = std::min(abs_d1, abs_d2) - s1.radius;
-
- if (abs_d1 <= s1.radius && abs_d2 <= s1.radius) {
- Vec3f c1 = a1 - new_s2.n * d1;
- Vec3f c2 = a2 - new_s2.n * d2;
- p1 = p2 = (c1 + c2) * 0.5;
- } else if (abs_d1 <= s1.radius) {
- Vec3f c = a1 - new_s2.n * d1;
- p1 = p2 = c;
- } else if (abs_d2 <= s1.radius) {
- Vec3f c = a2 - new_s2.n * d2;
- p1 = p2 = c;
+ // ends of capsule inner segment
+ Vec3f a1 = T1 + dir_z * s1.halfLength;
+ Vec3f a2 = T1 - dir_z * s1.halfLength;
+
+ FCL_REAL d1 = new_s2.signedDistance(a1);
+ FCL_REAL d2 = new_s2.signedDistance(a2);
+
+ FCL_REAL abs_d1 = std::abs(d1);
+ FCL_REAL abs_d2 = std::abs(d2);
+
+ if (d1 * d2 <= 0) {
+ // Two end points of the capsule are on different sides of the plane.
+ if (abs_d1 < abs_d2) {
+ distance = -abs_d1 - s1.radius;
+ if (d1 > 0)
+ normal = new_s2.n;
+ else
+ normal = -new_s2.n;
+ p1 = a1 + s1.radius * normal;
+ } else {
+ distance = -abs_d2 - s1.radius;
+ if (d2 > 0)
+ normal = new_s2.n;
+ else
+ normal = -new_s2.n;
+ p1 = a2 + s1.radius * normal;
+ }
+ assert(distance <= 0);
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+ assert(!p1.hasNaN() && !p2.hasNaN());
+ return true;
} else {
- assert(false);
- }
+ // Both capsule ends are on the same side of the plane.
+ distance = std::min(abs_d1, abs_d2) - s1.radius;
+ if (d1 < 0)
+ normal = new_s2.n;
+ else
+ normal = -new_s2.n;
- if (d1 < 0)
- normal = new_s2.n;
- else
- normal = -new_s2.n;
- assert(!p1.hasNaN() && !p2.hasNaN());
- return true;
+ if (abs_d1 <= abs_d2) {
+ p1 = a1 + s1.radius * normal;
+ } else {
+ p1 = a2 + s1.radius * normal;
+ }
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+ assert(!p1.hasNaN() && !p2.hasNaN());
+ if (distance > 0) return true;
+ return false;
+ }
}
assert(false);
}
@@ -2121,14 +2078,17 @@ inline bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) {
FCL_REAL d = new_s2.signedDistance(T);
distance = std::abs(d) - s1.radius;
+ if (d < 0)
+ normal = new_s2.n;
+ else
+ normal = -new_s2.n;
+ p1 = T + s1.radius * normal;
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
if (distance > 0)
return false;
else {
- if (d < 0)
- normal = new_s2.n;
- else
- normal = -new_s2.n;
- p1 = p2 = T - new_s2.n * d;
return true;
}
} else {
@@ -2156,29 +2116,31 @@ inline bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
FCL_REAL d1 = new_s2.signedDistance(c1);
FCL_REAL d2 = new_s2.signedDistance(c2);
-
- if (d1 * d2 <= 0) {
- FCL_REAL abs_d1 = std::abs(d1);
- FCL_REAL abs_d2 = std::abs(d2);
-
- if (abs_d1 > abs_d2) {
- distance = -abs_d2;
- p1 = p2 = c2 - new_s2.n * d2;
- if (d2 < 0)
- normal = -new_s2.n;
- else
- normal = new_s2.n;
- } else {
- distance = -abs_d1;
- p1 = p2 = c1 - new_s2.n * d1;
- if (d1 < 0)
- normal = -new_s2.n;
- else
- normal = new_s2.n;
- }
- return true;
- } else
- return false;
+ FCL_REAL abs_d1 = std::abs(d1);
+ FCL_REAL abs_d2 = std::abs(d2);
+ bool intersect = (d1 * d2 <= 0);
+ FCL_REAL sign = (intersect ? -1. : 1.);
+
+ if (abs_d1 > abs_d2) {
+ distance = (intersect ? -1. : 1.) * abs_d2;
+ p1 = c2;
+ if (d2 < 0)
+ normal = sign * new_s2.n;
+ else
+ normal = -sign * new_s2.n;
+ } else {
+ distance = (intersect ? -1. : 1.) * abs_d1;
+ p1 = c1;
+ if (d1 < 0)
+ normal = sign * new_s2.n;
+ else
+ normal = -sign * new_s2.n;
+ }
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+ if (distance > 0) return false;
+ return true;
}
}
@@ -2197,16 +2159,17 @@ inline bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) {
FCL_REAL d = new_s2.signedDistance(T);
distance = std::abs(d) - s1.radius;
+ if (d > 0)
+ normal = -new_s2.n;
+ else
+ normal = new_s2.n;
+ p1 = T - dir_z * s1.halfLength + s1.radius * normal;
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
if (distance > 0) {
- p1 = p2 = Vec3f(0, 0, 0);
return false;
} else {
- if (d < 0)
- normal = new_s2.n;
- else
- normal = -new_s2.n;
- p1 = p2 = T - dir_z * (s1.halfLength) +
- dir_z * (-distance / s1.radius * s1.halfLength) - new_s2.n * d;
return true;
}
} else {
@@ -2220,6 +2183,10 @@ inline bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
C *= s;
}
+ // We are looking for the point of the cone deepest into the plane,
+ // on one side or the other.
+ // There are 3 candidates: the tip of the cone and the 2 points of the
+ // cone's circle opposite to each other aligned with axis C.
Vec3f c[3];
c[0] = T + dir_z * (s1.halfLength);
c[1] = T - dir_z * (s1.halfLength) + C;
@@ -2231,65 +2198,64 @@ inline bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
d[2] = new_s2.signedDistance(c[2]);
if ((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) ||
- (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
+ (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) {
+ // All three candidate points on the cone are on the same side of the
+ // plane.
+ bool positive = (d[0] >= 0);
+ FCL_REAL sign = (positive ? 1. : -1.);
+ normal = -sign * new_s2.n;
+
+ distance = sign * d[0];
+ // p1 is the point of the cone closest to the plane.
+ int index_closest = 0;
+ for (int i = 1; i < 3; ++i) {
+ if (distance >= sign * d[i]) {
+ distance = sign * d[i];
+ index_closest = i;
+ }
+ }
+ assert(distance >= 0);
+ p1 = c[index_closest];
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
return false;
- else {
+ } else {
+ // The cone is overlapping the plane.
+ // We look for the 2 points on each side of the plane which have the
+ // deepest penetration.
+ // Out of these 2 points, p1 is the one with the smallest penetration.
bool positive[3];
for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] >= 0);
- int n_positive = 0;
FCL_REAL d_positive = 0, d_negative = 0;
- for (std::size_t i = 0; i < 3; ++i) {
+ int index_positive(0), index_negative(0);
+ for (int i = 0; i < 3; ++i) {
if (positive[i]) {
- n_positive++;
- if (d_positive <= d[i]) d_positive = d[i];
+ if (d_positive <= d[i]) {
+ d_positive = d[i];
+ index_positive = i;
+ }
} else {
- if (d_negative <= -d[i]) d_negative = -d[i];
+ if (d_negative <= -d[i]) {
+ d_negative = -d[i];
+ index_negative = i;
+ }
}
}
distance = -std::min(d_positive, d_negative);
- if (d_positive > d_negative)
+ assert(distance <= 0);
+ if (d_positive > d_negative) {
normal = -new_s2.n;
- else
- normal = new_s2.n;
- Vec3f p[2];
- Vec3f q;
-
- FCL_REAL p_d[2];
- FCL_REAL q_d(0);
-
- if (n_positive == 2) {
- for (std::size_t i = 0, j = 0; i < 3; ++i) {
- if (positive[i]) {
- p[j] = c[i];
- p_d[j] = d[i];
- j++;
- } else {
- q = c[i];
- q_d = d[i];
- }
- }
-
- Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
- Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
- p1 = p2 = (t1 + t2) * 0.5;
+ p1 = c[index_negative];
} else {
- for (std::size_t i = 0, j = 0; i < 3; ++i) {
- if (!positive[i]) {
- p[j] = c[i];
- p_d[j] = d[i];
- j++;
- } else {
- q = c[i];
- q_d = d[i];
- }
- }
-
- Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
- Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
- p1 = p2 = (t1 + t2) * 0.5;
+ normal = new_s2.n;
+ p1 = c[index_positive];
}
+ p2 = p1 + distance * normal;
+ assert(new_s2.distance(p2) <=
+ 3 * sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
}
return true;
}
@@ -2305,8 +2271,9 @@ inline bool convexPlaneIntersect(const ConvexBase& s1, const Transform3f& tf1,
FCL_REAL d_min = (std::numeric_limits<FCL_REAL>::max)(),
d_max = -(std::numeric_limits<FCL_REAL>::max)();
+ const std::vector<Vec3f>& points_ = *(s1.points);
for (unsigned int i = 0; i < s1.num_points; ++i) {
- Vec3f p = tf1.transform(s1.points[i]);
+ Vec3f p = tf1.transform(points_[i]);
FCL_REAL d = new_s2.signedDistance(p);
@@ -2507,6 +2474,30 @@ inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2,
return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2,
globalQ3, normal);
}
+
+inline bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
+ Vec3f& normal) {
+ Vec3f n_w = tf2.getRotation() * s2.n;
+ FCL_REAL d_w = s2.d + n_w.dot(tf2.getTranslation());
+
+ const Vec3f& center = tf1.getTranslation();
+ FCL_REAL signed_dist = n_w.dot(center) - d_w;
+ if (signed_dist > 0)
+ normal = -n_w;
+ else {
+ normal = n_w;
+ d_w = s2.d - n_w.dot(tf2.getTranslation());
+ }
+ int hint = 0;
+ p1 = getSupport(&s1, tf1.getRotation().transpose() * normal, true, hint);
+ p1.noalias() = tf1.transform(p1);
+ distance = -normal.dot(p1) - d_w;
+ p2 = p1 + distance * normal;
+ assert(std::abs(-normal.dot(p2) - d_w) <= 1e-8);
+ return distance <= 0;
+}
} // namespace details
} // namespace fcl
} // namespace hpp
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 4d4b1374c97332879cb4f70091c3dfc24cdff783..c40fd5364b21795ad42bb48e2af0351869a31962 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -3,6 +3,7 @@
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * Copyright (c) 2021-2022, INRIA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -35,6 +36,7 @@
/** \author Jia Pan */
+#include "hpp/fcl/shape/geometric_shapes.h"
#include <hpp/fcl/narrowphase/gjk.h>
#include <hpp/fcl/internal/intersect.h>
#include <hpp/fcl/internal/tools.h>
@@ -171,11 +173,11 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
MinkowskiDiff::ShapeData* data) {
assert(data != NULL);
- const Vec3f* pts = convex->points;
- const ConvexBase::Neighbors* nn = convex->neighbors;
+ const std::vector<Vec3f>& pts = *(convex->points);
+ const std::vector<ConvexBase::Neighbors>& nn = *(convex->neighbors);
if (hint < 0 || hint >= (int)convex->num_points) hint = 0;
- FCL_REAL maxdot = pts[hint].dot(dir);
+ FCL_REAL maxdot = pts[static_cast<size_t>(hint)].dot(dir);
std::vector<int8_t>& visited = data->visited;
visited.assign(convex->num_points, false);
visited[static_cast<std::size_t>(hint)] = true;
@@ -183,7 +185,7 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
// equal. Yet, the neighbors must be visited.
bool found = true, loose_check = true;
while (found) {
- const ConvexBase::Neighbors& n = nn[hint];
+ const ConvexBase::Neighbors& n = nn[static_cast<size_t>(hint)];
found = false;
for (int in = 0; in < n.count(); ++in) {
const unsigned int ip = n[in];
@@ -204,24 +206,25 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
}
}
- support = pts[hint];
+ support = pts[static_cast<size_t>(hint)];
}
void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir,
Vec3f& support, int& hint,
MinkowskiDiff::ShapeData*) {
- const Vec3f* pts = convex->points;
+ const std::vector<Vec3f>& pts = *(convex->points);
hint = 0;
FCL_REAL maxdot = pts[0].dot(dir);
for (int i = 1; i < (int)convex->num_points; ++i) {
- FCL_REAL dot = pts[i].dot(dir);
+ FCL_REAL dot = pts[static_cast<size_t>(i)].dot(dir);
if (dot > maxdot) {
maxdot = dot;
hint = i;
}
}
- support = pts[hint];
+
+ support = pts[static_cast<size_t>(hint)];
}
void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
@@ -257,6 +260,11 @@ inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
: dir, \
support, hint, NULL)
+inline void getSphereSupport(const Sphere* sphere, const Vec3f& dir,
+ Vec3f& support) {
+ support = sphere->radius * (dir.normalized());
+}
+
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
int& hint) {
Vec3f support;
@@ -268,7 +276,7 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
CALL_GET_SHAPE_SUPPORT(Box);
break;
case GEOM_SPHERE:
- CALL_GET_SHAPE_SUPPORT(Sphere);
+ getSphereSupport(static_cast<const Sphere*>(shape), dir, support);
break;
case GEOM_ELLIPSOID:
CALL_GET_SHAPE_SUPPORT(Ellipsoid);
@@ -394,7 +402,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
return getSupportFuncTpl<Shape0, SmallConvex, false>;
}
default:
- throw std::logic_error("Unsupported geometric shape");
+ HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
}
}
@@ -443,7 +451,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
s1, identity, inflation, linear_log_convex_threshold);
break;
default:
- throw std::logic_error("Unsupported geometric shape");
+ HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
}
}
@@ -474,7 +482,7 @@ bool getNormalizeSupportDirection(const ShapeBase* shape) {
return (bool)shape_traits<ConvexBase>::NeedNesterovNormalizeHeuristic;
break;
default:
- throw std::logic_error("Unsupported geometric shape");
+ HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
}
}
@@ -523,6 +531,8 @@ void GJK::initialize() {
gjk_variant = GJKVariant::DefaultGJK;
convergence_criterion = GJKConvergenceCriterion::VDB;
convergence_criterion_type = GJKConvergenceCriterionType::Relative;
+ iterations = 0;
+ iterations_momentum_stop = 0;
}
Vec3f GJK::getGuessFromSimplex() const { return ray; }
@@ -576,7 +586,8 @@ bool getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
vs[2]->w, vs[3]->w);
break;
default:
- throw std::logic_error("The simplex rank must be in [ 1, 4 ]");
+ HPP_FCL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]",
+ std::logic_error);
}
w0.setZero();
w1.setZero();
@@ -650,6 +661,9 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
// Momentum
GJKVariant current_gjk_variant = gjk_variant;
+ // If at the end of gjk, iterations_stop_momentum has a value of -1,
+ // this means momentum has not been stopped.
+ if (current_gjk_variant != DefaultGJK) iterations_momentum_stop = -1;
Vec3f w = ray;
Vec3f dir = ray;
Vec3f y;
@@ -700,8 +714,13 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
}
break;
+ case PolyakAcceleration:
+ momentum = 1 / (FCL_REAL(iterations) + 1);
+ dir = momentum * dir + (1 - momentum) * ray;
+ break;
+
default:
- throw std::logic_error("Invalid momentum variant.");
+ HPP_FCL_THROW_PRETTY("Invalid momentum variant.", std::logic_error);
}
appendVertex(curr_simplex, -dir, false,
@@ -725,7 +744,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
if (frank_wolfe_duality_gap - tolerance <= 0) {
removeVertex(simplices[current]);
current_gjk_variant = DefaultGJK; // move back to classic GJK
- continue; // continue to next iteration
+ iterations_momentum_stop = static_cast<int>(iterations);
+ continue; // continue to next iteration
}
}
@@ -770,7 +790,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
inside = projectTetrahedraOrigin(curr_simplex, next_simplex);
break;
default:
- throw std::logic_error("Invalid simplex rank");
+ HPP_FCL_THROW_PRETTY("Invalid simplex rank", std::logic_error);
}
assert(nfree + next_simplex.rank == 4);
current = next;
@@ -807,13 +827,15 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
diff = rl - alpha;
switch (convergence_criterion_type) {
case Absolute:
- throw std::logic_error("VDB convergence criterion is relative.");
+ HPP_FCL_THROW_PRETTY("VDB convergence criterion is relative.",
+ std::logic_error);
break;
case Relative:
check_passed = (diff - tolerance * rl) <= 0;
break;
default:
- throw std::logic_error("Invalid convergence criterion type.");
+ HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.",
+ std::logic_error);
}
break;
@@ -828,7 +850,8 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
check_passed = ((diff / tolerance * rl) - tolerance * rl) <= 0;
break;
default:
- throw std::logic_error("Invalid convergence criterion type.");
+ HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.",
+ std::logic_error);
}
break;
@@ -845,12 +868,13 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
check_passed = ((diff / tolerance * rl) - tolerance * rl) <= 0;
break;
default:
- throw std::logic_error("Invalid convergence criterion type.");
+ HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.",
+ std::logic_error);
}
break;
default:
- throw std::logic_error("Invalid convergence criterion.");
+ HPP_FCL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error);
}
return check_passed;
}
@@ -1446,6 +1470,7 @@ void EPA::initialize() {
nextsv = 0;
for (size_t i = 0; i < max_face_num; ++i)
stock.append(&fc_store[max_face_num - i - 1]);
+ iterations = 0;
}
bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b,
@@ -1561,7 +1586,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
// minimum distance to origin) to split
SimplexF outer = *best;
size_t pass = 0;
- size_t iterations = 0;
+ iterations = 0;
// set the face connectivity
bind(tetrahedron[0], 0, tetrahedron[1], 0);
@@ -1622,7 +1647,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
// the origin.
status = FallBack;
// TODO: define a better normal
- assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance()));
+ assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.tolerance));
normal = -guess;
FCL_REAL nl = normal.norm();
if (nl > 0)
diff --git a/src/octree.cpp b/src/octree.cpp
index 3d2ff6ea18a5911c2769ff22264c7cb44c4a20b9..7e01605965be4e3df605a279c89df4c4f124f1f2 100644
--- a/src/octree.cpp
+++ b/src/octree.cpp
@@ -1,7 +1,7 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2020, INRIA
+ * Copyright (c) 2020-2023, INRIA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -33,6 +33,7 @@
*/
#include <hpp/fcl/octree.h>
+#include <array>
namespace hpp {
namespace fcl {
@@ -54,13 +55,13 @@ struct Neighbors {
void hasNeighboordPlusZ() { value |= 0x20; }
}; // struct neighbors
-void computeNeighbors(const std::vector<boost::array<FCL_REAL, 6> >& boxes,
+void computeNeighbors(const std::vector<Vec6f>& boxes,
std::vector<Neighbors>& neighbors) {
- typedef std::vector<boost::array<FCL_REAL, 6> > VectorArray6;
+ typedef std::vector<Vec6f> VectorVec6f;
FCL_REAL fixedSize = -1;
FCL_REAL e(1e-8);
for (std::size_t i = 0; i < boxes.size(); ++i) {
- const boost::array<FCL_REAL, 6>& box(boxes[i]);
+ const Vec6f& box(boxes[i]);
Neighbors& n(neighbors[i]);
FCL_REAL x(box[0]);
FCL_REAL y(box[1]);
@@ -71,9 +72,9 @@ void computeNeighbors(const std::vector<boost::array<FCL_REAL, 6> >& boxes,
else
assert(s == fixedSize);
- for (VectorArray6::const_iterator it = boxes.begin(); it != boxes.end();
+ for (VectorVec6f::const_iterator it = boxes.begin(); it != boxes.end();
++it) {
- const boost::array<FCL_REAL, 6>& otherBox = *it;
+ const Vec6f& otherBox = *it;
FCL_REAL xo(otherBox[0]);
FCL_REAL yo(otherBox[1]);
FCL_REAL zo(otherBox[2]);
@@ -105,7 +106,7 @@ void computeNeighbors(const std::vector<boost::array<FCL_REAL, 6> >& boxes,
} // namespace internal
void OcTree::exportAsObjFile(const std::string& filename) const {
- std::vector<boost::array<FCL_REAL, 6> > boxes(this->toBoxes());
+ std::vector<Vec6f> boxes(this->toBoxes());
std::vector<internal::Neighbors> neighbors(boxes.size());
internal::computeNeighbors(boxes, neighbors);
// compute list of vertices and faces
@@ -113,12 +114,12 @@ void OcTree::exportAsObjFile(const std::string& filename) const {
typedef std::vector<Vec3f> VectorVec3f;
std::vector<Vec3f> vertices;
- typedef boost::array<std::size_t, 4> Array4;
+ typedef std::array<std::size_t, 4> Array4;
typedef std::vector<Array4> VectorArray4;
std::vector<Array4> faces;
for (std::size_t i = 0; i < boxes.size(); ++i) {
- const boost::array<FCL_REAL, 6>& box(boxes[i]);
+ const Vec6f& box(boxes[i]);
internal::Neighbors& n(neighbors[i]);
FCL_REAL x(box[0]);
@@ -165,8 +166,10 @@ void OcTree::exportAsObjFile(const std::string& filename) const {
std::ofstream os;
os.open(filename);
if (!os.is_open())
- throw std::runtime_error(std::string("failed to open file \"") + filename +
- std::string("\""));
+ HPP_FCL_THROW_PRETTY(
+ (std::string("failed to open file \"") + filename + std::string("\""))
+ .c_str(),
+ std::runtime_error);
// write vertices
os << "# list of vertices\n";
for (VectorVec3f::const_iterator it = vertices.begin(); it != vertices.end();
@@ -191,9 +194,7 @@ OcTreePtr_t makeOctree(
shared_ptr<octomap::OcTree> 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);
- octomap::point3d p(static_cast<float>(row[0]), static_cast<float>(row[1]),
- static_cast<float>(row[2]));
- octree->updateNode(p, true);
+ octree->updateNode(row[0], row[1], row[2], true, true);
}
octree->updateInnerOccupancy();
diff --git a/src/serialization/serialization.cpp b/src/serialization/serialization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ef1d28a220473675d6981e7c07fd51a65770235
--- /dev/null
+++ b/src/serialization/serialization.cpp
@@ -0,0 +1,87 @@
+/*
+ * 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 "hpp/fcl/serialization/fwd.h"
+
+using namespace hpp::fcl;
+
+#include "hpp/fcl/serialization/geometric_shapes.h"
+#include "hpp/fcl/serialization/convex.h"
+#include "hpp/fcl/serialization/hfield.h"
+#include "hpp/fcl/serialization/BVH_model.h"
+#ifdef HPP_FCL_HAS_OCTOMAP
+#include "hpp/fcl/serialization/octree.h"
+#endif
+
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(ShapeBase)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionGeometry)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(TriangleP)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Box)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Sphere)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Ellipsoid)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Capsule)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Cone)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Cylinder)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Halfspace)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Plane)
+
+#define EXPORT_AND_CAST(Derived, Base) \
+ HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base) \
+ HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Derived) \
+ /**/
+
+HPP_FCL_SERIALIZATION_CAST_REGISTER(ConvexBase, CollisionGeometry)
+EXPORT_AND_CAST(Convex<Triangle>, ConvexBase)
+EXPORT_AND_CAST(Convex<Quadrilateral>, ConvexBase)
+
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField<AABB>)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField<OBB>)
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField<OBBRSS>)
+
+HPP_FCL_SERIALIZATION_CAST_REGISTER(BVHModelBase, CollisionGeometry)
+
+EXPORT_AND_CAST(BVHModel<AABB>, BVHModelBase)
+EXPORT_AND_CAST(BVHModel<OBB>, BVHModelBase)
+EXPORT_AND_CAST(BVHModel<RSS>, BVHModelBase)
+EXPORT_AND_CAST(BVHModel<OBBRSS>, BVHModelBase)
+EXPORT_AND_CAST(BVHModel<kIOS>, BVHModelBase)
+EXPORT_AND_CAST(BVHModel<KDOP<16>>, BVHModelBase)
+EXPORT_AND_CAST(BVHModel<KDOP<18>>, BVHModelBase)
+EXPORT_AND_CAST(BVHModel<KDOP<24>>, BVHModelBase)
+
+#ifdef HPP_FCL_HAS_OCTOMAP
+HPP_FCL_SERIALIZATION_DEFINE_EXPORT(OcTree)
+#endif
diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp
index a2a05a974b040f44bc30b188a50ed0efe28e9397..c50d1e7c6e9e6569d3c3649e5643642abd3d0bd7 100644
--- a/src/shape/convex.cpp
+++ b/src/shape/convex.cpp
@@ -13,7 +13,6 @@ using orgQhull::Qhull;
using orgQhull::QhullFacet;
using orgQhull::QhullPoint;
using orgQhull::QhullRidgeSet;
-using orgQhull::QhullVertex;
using orgQhull::QhullVertexList;
using orgQhull::QhullVertexSet;
#endif
@@ -25,9 +24,9 @@ namespace fcl {
// the vector `triangle barycentre - convex_tri.center` is positive.
void reorderTriangle(const Convex<Triangle>* convex_tri, Triangle& tri) {
Vec3f p0, p1, p2;
- p0 = convex_tri->points[tri[0]];
- p1 = convex_tri->points[tri[1]];
- p2 = convex_tri->points[tri[2]];
+ p0 = (*(convex_tri->points))[tri[0]];
+ p1 = (*(convex_tri->points))[tri[1]];
+ p2 = (*(convex_tri->points))[tri[2]];
Vec3f barycentre_tri, center_barycenter;
barycentre_tri = (p0 + p1 + p2) / 3;
@@ -43,14 +42,25 @@ void reorderTriangle(const Convex<Triangle>* convex_tri, Triangle& tri) {
}
}
+ConvexBase* ConvexBase::convexHull(std::shared_ptr<std::vector<Vec3f>>& pts,
+ unsigned int num_points, bool keepTriangles,
+ const char* qhullCommand) {
+ HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
+ HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+ return ConvexBase::convexHull(pts->data(), num_points, keepTriangles,
+ qhullCommand);
+ HPP_FCL_COMPILER_DIAGNOSTIC_POP
+}
+
ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
bool keepTriangles,
const char* qhullCommand) {
#ifdef HPP_FCL_HAS_QHULL
if (num_points <= 3) {
- throw std::invalid_argument(
+ HPP_FCL_THROW_PRETTY(
"You shouldn't use this function with less than"
- " 4 points.");
+ " 4 points.",
+ std::invalid_argument);
}
assert(pts[0].data() + 3 == pts[1].data());
@@ -61,7 +71,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
if (qh.qhullStatus() != qh_ERRnone) {
if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl;
- throw std::logic_error("Qhull failed");
+ HPP_FCL_THROW_PRETTY("Qhull failed", std::logic_error);
}
typedef std::size_t index_type;
@@ -71,15 +81,16 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
std::vector<int> pts_to_vertices(num_points, -1);
// Initialize the vertices
- int nvertex = (qh.vertexCount());
- Vec3f* vertices = new Vec3f[size_t(nvertex)];
+ size_t nvertex = static_cast<size_t>(qh.vertexCount());
+ std::shared_ptr<std::vector<Vec3f>> vertices(
+ new std::vector<Vec3f>(size_t(nvertex)));
QhullVertexList vertexList(qh.vertexList());
- int i_vertex = 0;
+ 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] = Vec3f(pt[0], pt[1], pt[2]);
+ (*vertices)[i_vertex] = Vec3f(pt[0], pt[1], pt[2]);
++i_vertex;
}
assert(i_vertex == nvertex);
@@ -90,14 +101,15 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
convex = convex_tri = new Convex<Triangle>();
else
convex = new ConvexBase;
- convex->initialize(true, vertices, static_cast<unsigned int>(nvertex));
+ convex->initialize(vertices, static_cast<unsigned int>(nvertex));
// Build the neighbors
- convex->neighbors = new Neighbors[size_t(nvertex)];
- std::vector<std::set<index_type> > nneighbors(static_cast<size_t>(nvertex));
+ convex->neighbors.reset(new std::vector<Neighbors>(size_t(nvertex)));
+ std::vector<std::set<index_type>> nneighbors(static_cast<size_t>(nvertex));
if (keepTriangles) {
convex_tri->num_polygons = static_cast<unsigned int>(qh.facetCount());
- convex_tri->polygons = new Triangle[convex_tri->num_polygons];
+ convex_tri->polygons.reset(
+ new std::vector<Triangle>(convex_tri->num_polygons));
convex_tri->computeCenter();
}
@@ -121,7 +133,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
f_vertices[2].point().id())]));
if (keepTriangles) {
reorderTriangle(convex_tri, tri);
- convex_tri->polygons[i_polygon++] = tri;
+ (*convex_tri->polygons)[i_polygon++] = tri;
}
for (size_t j = 0; j < n; ++j) {
size_t i = (j == 0) ? n - 1 : j - 1;
@@ -132,9 +144,10 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
}
} else {
if (keepTriangles) { // TODO I think there is a memory leak here.
- throw std::invalid_argument(
+ HPP_FCL_THROW_PRETTY(
"You requested to keep triangles so you "
- "must pass option \"Qt\" to qhull via the qhull command argument.");
+ "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
@@ -158,30 +171,78 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
}
}
}
- assert(!keepTriangles || i_polygon == qh.facetCount());
+ assert(!keepTriangles || static_cast<int>(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_ = new unsigned int[c_nneighbors];
- unsigned int* p_nneighbors = convex->nneighbors_;
+ convex->nneighbors_.reset(new std::vector<unsigned int>(c_nneighbors));
+ unsigned int* p_nneighbors = convex->nneighbors_->data();
+ std::vector<Neighbors>& neighbors_ = *(convex->neighbors);
for (size_t i = 0; i < static_cast<size_t>(nvertex); ++i) {
- Neighbors& n = convex->neighbors[i];
+ Neighbors& n = neighbors_[i];
if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
- throw std::logic_error("Too many neighbors.");
+ HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error);
n.count_ = (unsigned char)nneighbors[i].size();
n.n_ = p_nneighbors;
p_nneighbors =
std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
}
- assert(p_nneighbors == convex->nneighbors_ + c_nneighbors);
+ assert(p_nneighbors == convex->nneighbors_->data() + c_nneighbors);
return convex;
#else
- throw std::logic_error(
- "Library built without qhull. Cannot build object of this type.");
+ HPP_FCL_THROW_PRETTY(
+ "Library built without qhull. Cannot build object of this type.",
+ std::logic_error);
HPP_FCL_UNUSED_VARIABLE(pts);
HPP_FCL_UNUSED_VARIABLE(num_points);
HPP_FCL_UNUSED_VARIABLE(keepTriangles);
HPP_FCL_UNUSED_VARIABLE(qhullCommand);
#endif
}
+
+#ifdef HPP_FCL_HAS_QHULL
+void ConvexBase::buildDoubleDescription() {
+ if (num_points <= 3) {
+ HPP_FCL_THROW_PRETTY(
+ "You shouldn't use this function with a convex less than"
+ " 4 points.",
+ std::invalid_argument);
+ }
+
+ Qhull qh;
+ const char* command = "Qt";
+ qh.runQhull("", 3, static_cast<int>(num_points), (*points)[0].data(),
+ command);
+
+ if (qh.qhullStatus() != qh_ERRnone) {
+ if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl;
+ HPP_FCL_THROW_PRETTY("Qhull failed", std::logic_error);
+ }
+
+ buildDoubleDescriptionFromQHullResult(qh);
+}
+
+void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) {
+ num_normals_and_offsets = static_cast<unsigned int>(qh.facetCount());
+ normals.reset(new std::vector<Vec3f>(num_normals_and_offsets));
+ std::vector<Vec3f>& normals_ = *normals;
+ offsets.reset(new std::vector<double>(num_normals_and_offsets));
+ std::vector<double>& 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] = Vec3f(plane.coordinates()[0], plane.coordinates()[1],
+ plane.coordinates()[2]);
+ offsets_[i_normal] = plane.offset();
+ i_normal++;
+ }
+ assert(static_cast<int>(i_normal) == qh.facetCount());
+}
+#endif
+
} // namespace fcl
} // namespace hpp
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index 8c05a2d027bb5ad5ce876ee50bd42e412b3084e0..4ac11a01730ff8b0fdcfa105e0c25fc20b2f585a 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/src/shape/geometric_shapes.cpp
@@ -41,57 +41,74 @@
namespace hpp {
namespace fcl {
-void ConvexBase::initialize(bool own_storage, Vec3f* points_,
+void ConvexBase::initialize(std::shared_ptr<std::vector<Vec3f>> points_,
unsigned int num_points_) {
points = points_;
num_points = num_points_;
- own_storage_ = own_storage;
+ num_normals_and_offsets = 0;
+ normals.reset();
+ offsets.reset();
computeCenter();
}
-void ConvexBase::set(bool own_storage_, Vec3f* points_,
+void ConvexBase::set(std::shared_ptr<std::vector<Vec3f>> points_,
unsigned int num_points_) {
- if (own_storage_ && points) delete[] points;
- initialize(own_storage_, points_, num_points_);
+ initialize(points_, num_points_);
}
ConvexBase::ConvexBase(const ConvexBase& other)
: ShapeBase(other),
num_points(other.num_points),
- center(other.center),
- own_storage_(other.own_storage_) {
- if (neighbors) delete[] neighbors;
- if (nneighbors_) delete[] nneighbors_;
- if (own_storage_) {
- if (own_storage_ && points) delete[] points;
-
- points = new Vec3f[num_points];
- std::copy(other.points, other.points + num_points, points);
+ num_normals_and_offsets(other.num_normals_and_offsets),
+ center(other.center) {
+ if (other.points.get() && other.points->size() > 0) {
+ // Deep copy of other points
+ points.reset(new std::vector<Vec3f>(*other.points));
} else
- points = other.points;
+ points.reset();
+
+ if (other.nneighbors_.get() && other.nneighbors_->size() > 0) {
+ // Deep copy the list of all the neighbors of all the points
+ nneighbors_.reset(new std::vector<unsigned int>(*(other.nneighbors_)));
+ if (other.neighbors.get() && other.neighbors->size() > 0) {
+ // Fill each neighbors for each point in the Convex object.
+ neighbors.reset(new std::vector<Neighbors>(other.neighbors->size()));
+ assert(neighbors->size() == points->size());
+ unsigned int* p_nneighbors = nneighbors_->data();
+
+ std::vector<Neighbors>& neighbors_ = *neighbors;
+ const std::vector<Neighbors>& other_neighbors_ = *(other.neighbors);
+ for (size_t i = 0; i < neighbors->size(); ++i) {
+ Neighbors& n = neighbors_[i];
+ n.count_ = other_neighbors_[i].count_;
+ n.n_ = p_nneighbors;
+ p_nneighbors += n.count_;
+ }
+ } else
+ neighbors.reset();
+ } else
+ nneighbors_.reset();
- neighbors = new Neighbors[num_points];
- std::copy(other.neighbors, other.neighbors + num_points, neighbors);
+ if (other.normals.get() && other.normals->size() > 0) {
+ normals.reset(new std::vector<Vec3f>(*(other.normals)));
+ } else
+ normals.reset();
- std::size_t c_nneighbors = 0;
- for (std::size_t i = 0; i < num_points; ++i)
- c_nneighbors += neighbors[i].count();
- nneighbors_ = new unsigned int[c_nneighbors];
- std::copy(other.nneighbors_, other.nneighbors_ + c_nneighbors, nneighbors_);
+ if (other.offsets.get() && other.offsets->size() > 0) {
+ offsets.reset(new std::vector<double>(*(other.offsets)));
+ } else
+ offsets.reset();
ShapeBase::operator=(*this);
}
-ConvexBase::~ConvexBase() {
- if (neighbors) delete[] neighbors;
- if (nneighbors_) delete[] nneighbors_;
- if (own_storage_ && points) delete[] points;
-}
+ConvexBase::~ConvexBase() {}
void ConvexBase::computeCenter() {
center.setZero();
+ const std::vector<Vec3f>& points_ = *points;
for (std::size_t i = 0; i < num_points; ++i)
- center += points[i]; // TODO(jcarpent): vectorization
+ center += points_[i]; // TODO(jcarpent): vectorization
center /= num_points;
}
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 181487cde33a25bf19ae33b384c1dd660fb12dbd..2f7d0992322fcbd7808e27280c37afcde298ea60 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -226,8 +226,9 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder,
std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
const Transform3f& tf) {
std::vector<Vec3f> result(convex.num_points);
+ const std::vector<Vec3f>& points_ = *(convex.points);
for (std::size_t i = 0; i < convex.num_points; ++i) {
- result[i] = tf.transform(convex.points[i]);
+ result[i] = tf.transform(points_[i]);
}
return result;
@@ -354,8 +355,9 @@ void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
const Vec3f& T = tf.getTranslation();
AABB bv_;
+ const std::vector<Vec3f>& points_ = *(s.points);
for (std::size_t i = 0; i < s.num_points; ++i) {
- Vec3f new_p = R * s.points[i] + T;
+ Vec3f new_p = R * points_[i] + T;
bv_ += new_p;
}
@@ -492,7 +494,7 @@ void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- fit(s.points, s.num_points, bv);
+ fit(s.points->data(), s.num_points, bv);
bv.axes.applyOnTheLeft(R);
diff --git a/src/traits_traversal.h b/src/traits_traversal.h
index a8d20c68b51ae26e61f31f1c8cdcf47b63ab6557..2df829ef741b26cbad57e3dfb6d94803d8137ff2 100644
--- a/src/traits_traversal.h
+++ b/src/traits_traversal.h
@@ -48,6 +48,16 @@ struct HPP_FCL_LOCAL TraversalTraitsCollision<BVHModel<T_BVH>, OcTree> {
typedef MeshOcTreeCollisionTraversalNode<T_BVH> CollisionTraversal_t;
};
+template <typename T_HF>
+struct HPP_FCL_LOCAL TraversalTraitsCollision<OcTree, HeightField<T_HF> > {
+ typedef OcTreeHeightFieldCollisionTraversalNode<T_HF> CollisionTraversal_t;
+};
+
+template <typename T_HF>
+struct HPP_FCL_LOCAL TraversalTraitsCollision<HeightField<T_HF>, OcTree> {
+ typedef HeightFieldOcTreeCollisionTraversalNode<T_HF> CollisionTraversal_t;
+};
+
#endif
// TraversalTraitsDistance for distance_func_matrix.cpp
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index d708a4ee3838b54f23b1f405ff8850308f5ea622..96e26d8091c162cb32576f5bf0947f237778e13c 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -28,6 +28,7 @@ add_fcl_test(math math.cpp)
add_fcl_test(collision collision.cpp)
add_fcl_test(distance distance.cpp)
+add_fcl_test(normal_and_nearest_points normal_and_nearest_points.cpp)
add_fcl_test(distance_lower_bound distance_lower_bound.cpp)
add_fcl_test(security_margin security_margin.cpp)
add_fcl_test(geometric_shapes geometric_shapes.cpp)
@@ -52,7 +53,7 @@ add_fcl_test(hfields hfields.cpp)
add_fcl_test(profiling profiling.cpp)
add_fcl_test(gjk gjk.cpp)
-add_fcl_test(nesterov_gjk nesterov_gjk.cpp)
+add_fcl_test(accelerated_gjk accelerated_gjk.cpp)
add_fcl_test(gjk_convergence_criterion gjk_convergence_criterion.cpp)
if(HPP_FCL_HAS_OCTOMAP)
add_fcl_test(octree octree.cpp)
diff --git a/test/nesterov_gjk.cpp b/test/accelerated_gjk.cpp
similarity index 73%
rename from test/nesterov_gjk.cpp
rename to test/accelerated_gjk.cpp
index a75aeb157d50145c64e209538ba0532ca6f80ac7..6d2c80964dc1b566a2e907df4564a3e7be857694 100644
--- a/test/nesterov_gjk.cpp
+++ b/test/accelerated_gjk.cpp
@@ -77,6 +77,12 @@ BOOST_AUTO_TEST_CASE(set_gjk_variant) {
BOOST_CHECK(solver.gjk_variant == GJKVariant::NesterovAcceleration);
BOOST_CHECK(gjk.gjk_variant == GJKVariant::NesterovAcceleration);
+
+ solver.gjk_variant = GJKVariant::PolyakAcceleration;
+ gjk.gjk_variant = GJKVariant::PolyakAcceleration;
+
+ BOOST_CHECK(solver.gjk_variant == GJKVariant::PolyakAcceleration);
+ BOOST_CHECK(gjk.gjk_variant == GJKVariant::PolyakAcceleration);
}
BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) {
@@ -97,13 +103,15 @@ BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) {
BOOST_CHECK(mink_diff3.normalize_support_direction == true);
}
-void test_nesterov_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
+void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
// Solvers
unsigned int max_iterations = 128;
FCL_REAL tolerance = 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;
@@ -132,6 +140,9 @@ void test_nesterov_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
BOOST_CHECK(res_gjk_1 == res_gjk_2);
EIGEN_VECTOR_IS_APPROX(ray_gjk, gjk.ray, 1e-8);
+ // --------------
+ // -- Nesterov --
+ // --------------
GJK::Status res_nesterov_gjk_1 =
gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess);
Vec3f ray_nesterov = gjk_nesterov.ray;
@@ -147,8 +158,29 @@ void test_nesterov_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
// Make sure GJK and Nesterov accelerated GJK converges in a reasonable
// amount of iterations
- BOOST_CHECK(gjk.getIterations() < max_iterations);
- BOOST_CHECK(gjk_nesterov.getIterations() < max_iterations);
+ BOOST_CHECK(gjk.iterations < max_iterations);
+ BOOST_CHECK(gjk_nesterov.iterations < max_iterations);
+
+ // ------------
+ // -- Polyak --
+ // ------------
+ GJK::Status res_polyak_gjk_1 =
+ gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess);
+ Vec3f ray_polyak = gjk_polyak.ray;
+ GJK::Status res_polyak_gjk_2 =
+ gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess);
+ BOOST_CHECK(res_polyak_gjk_1 == res_polyak_gjk_2);
+ EIGEN_VECTOR_IS_APPROX(ray_polyak, gjk_polyak.ray, 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()), 1e-4);
+
+ // Make sure GJK and Polyak accelerated GJK converges in a reasonable
+ // amount of iterations
+ BOOST_CHECK(gjk.iterations < max_iterations);
+ BOOST_CHECK(gjk_polyak.iterations < max_iterations);
}
}
@@ -156,8 +188,8 @@ BOOST_AUTO_TEST_CASE(ellipsoid_ellipsoid) {
Ellipsoid ellipsoid0 = Ellipsoid(0.3, 0.4, 0.5);
Ellipsoid ellipsoid1 = Ellipsoid(1.5, 1.4, 1.3);
- test_nesterov_gjk(ellipsoid0, ellipsoid1);
- test_nesterov_gjk(ellipsoid0, ellipsoid1);
+ test_accelerated_gjk(ellipsoid0, ellipsoid1);
+ test_accelerated_gjk(ellipsoid0, ellipsoid1);
}
BOOST_AUTO_TEST_CASE(ellipsoid_capsule) {
@@ -166,10 +198,10 @@ BOOST_AUTO_TEST_CASE(ellipsoid_capsule) {
Capsule capsule0 = Capsule(0.1, 0.3);
Capsule capsule1 = Capsule(1.1, 1.3);
- test_nesterov_gjk(ellipsoid0, capsule0);
- test_nesterov_gjk(ellipsoid0, capsule1);
- test_nesterov_gjk(ellipsoid1, capsule0);
- test_nesterov_gjk(ellipsoid1, capsule1);
+ 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) {
@@ -178,10 +210,10 @@ BOOST_AUTO_TEST_CASE(ellipsoid_box) {
Box box0 = Box(0.1, 0.2, 0.3);
Box box1 = Box(1.1, 1.2, 1.3);
- test_nesterov_gjk(ellipsoid0, box0);
- test_nesterov_gjk(ellipsoid0, box1);
- test_nesterov_gjk(ellipsoid1, box0);
- test_nesterov_gjk(ellipsoid1, box1);
+ 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) {
@@ -190,10 +222,10 @@ BOOST_AUTO_TEST_CASE(ellipsoid_mesh) {
Convex<Triangle> cvx0 = constructPolytopeFromEllipsoid(ellipsoid0);
Convex<Triangle> cvx1 = constructPolytopeFromEllipsoid(ellipsoid1);
- test_nesterov_gjk(ellipsoid0, cvx0);
- test_nesterov_gjk(ellipsoid0, cvx1);
- test_nesterov_gjk(ellipsoid1, cvx0);
- test_nesterov_gjk(ellipsoid1, cvx1);
+ 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) {
@@ -204,27 +236,27 @@ BOOST_AUTO_TEST_CASE(capsule_mesh) {
Capsule capsule0 = Capsule(0.1, 0.3);
Capsule capsule1 = Capsule(1.1, 1.3);
- test_nesterov_gjk(capsule0, cvx0);
- test_nesterov_gjk(capsule0, cvx1);
- test_nesterov_gjk(capsule1, cvx0);
- test_nesterov_gjk(capsule1, cvx1);
+ 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(0.1, 0.3);
Capsule capsule1 = Capsule(1.1, 1.3);
- test_nesterov_gjk(capsule0, capsule0);
- test_nesterov_gjk(capsule1, capsule1);
- test_nesterov_gjk(capsule0, capsule1);
+ test_accelerated_gjk(capsule0, capsule0);
+ test_accelerated_gjk(capsule1, capsule1);
+ test_accelerated_gjk(capsule0, capsule1);
}
BOOST_AUTO_TEST_CASE(box_box) {
Box box0 = Box(0.1, 0.2, 0.3);
Box box1 = Box(1.1, 1.2, 1.3);
- test_nesterov_gjk(box0, box0);
- test_nesterov_gjk(box0, box1);
- test_nesterov_gjk(box1, box1);
+ test_accelerated_gjk(box0, box0);
+ test_accelerated_gjk(box0, box1);
+ test_accelerated_gjk(box1, box1);
}
BOOST_AUTO_TEST_CASE(box_mesh) {
@@ -235,10 +267,10 @@ BOOST_AUTO_TEST_CASE(box_mesh) {
Convex<Triangle> cvx0 = constructPolytopeFromEllipsoid(ellipsoid0);
Convex<Triangle> cvx1 = constructPolytopeFromEllipsoid(ellipsoid1);
- test_nesterov_gjk(box0, cvx0);
- test_nesterov_gjk(box0, cvx1);
- test_nesterov_gjk(box1, cvx0);
- test_nesterov_gjk(box1, cvx1);
+ 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) {
@@ -247,7 +279,7 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) {
Convex<Triangle> cvx0 = constructPolytopeFromEllipsoid(ellipsoid0);
Convex<Triangle> cvx1 = constructPolytopeFromEllipsoid(ellipsoid1);
- test_nesterov_gjk(cvx0, cvx0);
- test_nesterov_gjk(cvx0, cvx1);
- test_nesterov_gjk(cvx1, cvx1);
+ test_accelerated_gjk(cvx0, cvx0);
+ test_accelerated_gjk(cvx0, cvx1);
+ test_accelerated_gjk(cvx1, cvx1);
}
diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp
index bb84030545a83cbb4bdab34a89e5d0f36c975a32..10c54a39ace52694a1754dacfd4b7d893b7305a3 100644
--- a/test/bvh_models.cpp
+++ b/test/bvh_models.cpp
@@ -206,7 +206,7 @@ void testBVHModelTriangles() {
pose.setTranslation(Vec3f(0, 0, 0));
FCL_REAL sqrt2_2 = std::sqrt(2) / 2;
- pose.setQuatRotation(Quaternion3f(sqrt2_2, sqrt2_2, 0, 0));
+ pose.setQuatRotation(Quatf(sqrt2_2, sqrt2_2, 0, 0));
cropped.reset(BVHExtract(*model, pose, aabb));
BOOST_REQUIRE(cropped);
BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
@@ -214,7 +214,7 @@ void testBVHModelTriangles() {
BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
pose.setTranslation(-Vec3f(1, 1, 1));
- pose.setQuatRotation(Quaternion3f::Identity());
+ pose.setQuatRotation(Quatf::Identity());
cropped.reset(BVHExtract(*model, pose, aabb));
BOOST_CHECK(!cropped);
diff --git a/test/convex.cpp b/test/convex.cpp
index d55cab48a01876563ba91dbe961f9203ee7f8f82..ff789b98f9e641d77d8f63972f0635f254dc694a 100644
--- a/test/convex.cpp
+++ b/test/convex.cpp
@@ -46,26 +46,21 @@
using namespace hpp::fcl;
Convex<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) {
- Vec3f* pts = new Vec3f[8];
- pts[0] = Vec3f(l, w, d);
- pts[1] = Vec3f(l, w, -d);
- pts[2] = Vec3f(l, -w, d);
- pts[3] = Vec3f(l, -w, -d);
- pts[4] = Vec3f(-l, w, d);
- pts[5] = Vec3f(-l, w, -d);
- pts[6] = Vec3f(-l, -w, d);
- pts[7] = Vec3f(-l, -w, -d);
-
- Quadrilateral* polygons = new Quadrilateral[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 Convex<Quadrilateral>(true,
- pts, // points
+ std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>(
+ {Vec3f(l, w, d), Vec3f(l, w, -d), Vec3f(l, -w, d), Vec3f(l, -w, -d),
+ Vec3f(-l, w, d), Vec3f(-l, w, -d), Vec3f(-l, -w, d),
+ Vec3f(-l, -w, -d)}));
+
+ std::shared_ptr<std::vector<Quadrilateral>> polygons(
+ new std::vector<Quadrilateral>(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 Convex<Quadrilateral>(pts, // points
8, // num points
polygons,
6 // number of polygons
@@ -77,40 +72,40 @@ BOOST_AUTO_TEST_CASE(convex) {
Convex<Quadrilateral> box(buildBox(l, w, d));
// Check neighbors
- for (int i = 0; i < 8; ++i) {
- BOOST_CHECK_EQUAL(box.neighbors[i].count(), 3);
+ for (size_t i = 0; i < 8; ++i) {
+ BOOST_CHECK_EQUAL((*box.neighbors)[i].count(), 3);
}
- BOOST_CHECK_EQUAL(box.neighbors[0][0], 1);
- BOOST_CHECK_EQUAL(box.neighbors[0][1], 2);
- BOOST_CHECK_EQUAL(box.neighbors[0][2], 4);
+ BOOST_CHECK_EQUAL((*box.neighbors)[0][0], 1);
+ BOOST_CHECK_EQUAL((*box.neighbors)[0][1], 2);
+ BOOST_CHECK_EQUAL((*box.neighbors)[0][2], 4);
- BOOST_CHECK_EQUAL(box.neighbors[1][0], 0);
- BOOST_CHECK_EQUAL(box.neighbors[1][1], 3);
- BOOST_CHECK_EQUAL(box.neighbors[1][2], 5);
+ BOOST_CHECK_EQUAL((*box.neighbors)[1][0], 0);
+ BOOST_CHECK_EQUAL((*box.neighbors)[1][1], 3);
+ BOOST_CHECK_EQUAL((*box.neighbors)[1][2], 5);
- BOOST_CHECK_EQUAL(box.neighbors[2][0], 0);
- BOOST_CHECK_EQUAL(box.neighbors[2][1], 3);
- BOOST_CHECK_EQUAL(box.neighbors[2][2], 6);
+ BOOST_CHECK_EQUAL((*box.neighbors)[2][0], 0);
+ BOOST_CHECK_EQUAL((*box.neighbors)[2][1], 3);
+ BOOST_CHECK_EQUAL((*box.neighbors)[2][2], 6);
- BOOST_CHECK_EQUAL(box.neighbors[3][0], 1);
- BOOST_CHECK_EQUAL(box.neighbors[3][1], 2);
- BOOST_CHECK_EQUAL(box.neighbors[3][2], 7);
+ BOOST_CHECK_EQUAL((*box.neighbors)[3][0], 1);
+ BOOST_CHECK_EQUAL((*box.neighbors)[3][1], 2);
+ BOOST_CHECK_EQUAL((*box.neighbors)[3][2], 7);
- BOOST_CHECK_EQUAL(box.neighbors[4][0], 0);
- BOOST_CHECK_EQUAL(box.neighbors[4][1], 5);
- BOOST_CHECK_EQUAL(box.neighbors[4][2], 6);
+ BOOST_CHECK_EQUAL((*box.neighbors)[4][0], 0);
+ BOOST_CHECK_EQUAL((*box.neighbors)[4][1], 5);
+ BOOST_CHECK_EQUAL((*box.neighbors)[4][2], 6);
- BOOST_CHECK_EQUAL(box.neighbors[5][0], 1);
- BOOST_CHECK_EQUAL(box.neighbors[5][1], 4);
- BOOST_CHECK_EQUAL(box.neighbors[5][2], 7);
+ BOOST_CHECK_EQUAL((*box.neighbors)[5][0], 1);
+ BOOST_CHECK_EQUAL((*box.neighbors)[5][1], 4);
+ BOOST_CHECK_EQUAL((*box.neighbors)[5][2], 7);
- BOOST_CHECK_EQUAL(box.neighbors[6][0], 2);
- BOOST_CHECK_EQUAL(box.neighbors[6][1], 4);
- BOOST_CHECK_EQUAL(box.neighbors[6][2], 7);
+ BOOST_CHECK_EQUAL((*box.neighbors)[6][0], 2);
+ BOOST_CHECK_EQUAL((*box.neighbors)[6][1], 4);
+ BOOST_CHECK_EQUAL((*box.neighbors)[6][2], 7);
- BOOST_CHECK_EQUAL(box.neighbors[7][0], 3);
- BOOST_CHECK_EQUAL(box.neighbors[7][1], 5);
- BOOST_CHECK_EQUAL(box.neighbors[7][2], 6);
+ BOOST_CHECK_EQUAL((*box.neighbors)[7][0], 3);
+ BOOST_CHECK_EQUAL((*box.neighbors)[7][1], 5);
+ BOOST_CHECK_EQUAL((*box.neighbors)[7][2], 6);
}
template <typename Sa, typename Sb>
@@ -201,42 +196,38 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) {
#ifdef HPP_FCL_HAS_QHULL
BOOST_AUTO_TEST_CASE(convex_hull_throw) {
- std::vector<Vec3f> points({
- Vec3f(1, 1, 1),
- Vec3f(0, 0, 0),
- Vec3f(1, 0, 0),
- });
+ std::shared_ptr<std::vector<Vec3f>> points(
+ new std::vector<Vec3f>({Vec3f(1, 1, 1), Vec3f(0, 0, 0), Vec3f(1, 0, 0)}));
- BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 0, false, NULL),
+ BOOST_CHECK_THROW(ConvexBase::convexHull(points, 0, false, NULL),
std::invalid_argument);
- BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 1, false, NULL),
+ BOOST_CHECK_THROW(ConvexBase::convexHull(points, 1, false, NULL),
std::invalid_argument);
- BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 2, false, NULL),
+ BOOST_CHECK_THROW(ConvexBase::convexHull(points, 2, false, NULL),
std::invalid_argument);
- BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 3, false, NULL),
+ BOOST_CHECK_THROW(ConvexBase::convexHull(points, 3, false, NULL),
std::invalid_argument);
}
BOOST_AUTO_TEST_CASE(convex_hull_quad) {
- std::vector<Vec3f> points({
+ std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
Vec3f(1, 1, 1),
Vec3f(0, 0, 0),
Vec3f(1, 0, 0),
Vec3f(0, 0, 1),
- });
+ }));
- ConvexBase* convexHull = ConvexBase::convexHull(
- points.data(), (unsigned int)points.size(), false, NULL);
+ ConvexBase* convexHull = ConvexBase::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);
+ 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::vector<Vec3f> points({
+ std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
Vec3f(1, 1, 1),
Vec3f(1, 1, -1),
Vec3f(1, -1, 1),
@@ -247,29 +238,79 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
Vec3f(-1, -1, -1),
Vec3f(0, 0, 0),
Vec3f(0, 0, 0.99),
- });
+ }));
- ConvexBase* convexHull = ConvexBase::convexHull(
- points.data(), (unsigned int)points.size(), false, NULL);
+ ConvexBase* convexHull = ConvexBase::convexHull(points, 9, false, NULL);
BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
- for (int i = 0; i < 8; ++i) {
- BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1, 1, 1));
- BOOST_CHECK_EQUAL(convexHull->neighbors[i].count(), 3);
+ {
+ const std::vector<Vec3f>& cvxhull_points = *(convexHull->points);
+ for (size_t i = 0; i < 8; ++i) {
+ BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1));
+ BOOST_CHECK_EQUAL((*(convexHull->neighbors))[i].count(), 3);
+ }
}
delete convexHull;
- convexHull = ConvexBase::convexHull(points.data(),
- (unsigned int)points.size(), true, NULL);
+ convexHull = ConvexBase::convexHull(points, 9, true, NULL);
Convex<Triangle>* convex_tri = dynamic_cast<Convex<Triangle>*>(convexHull);
BOOST_CHECK(convex_tri != NULL);
BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
- for (int i = 0; i < 8; ++i) {
- BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1, 1, 1));
- BOOST_CHECK(convexHull->neighbors[i].count() >= 3);
- BOOST_CHECK(convexHull->neighbors[i].count() <= 6);
+ {
+ const std::vector<Vec3f>& cvxhull_points = *(convexHull->points);
+ for (size_t i = 0; i < 8; ++i) {
+ BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(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) {
+ Convex<Triangle>* convexHullTriCopy;
+ {
+ std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
+ Vec3f(1, 1, 1),
+ Vec3f(1, 1, -1),
+ Vec3f(1, -1, 1),
+ Vec3f(1, -1, -1),
+ Vec3f(-1, 1, 1),
+ Vec3f(-1, 1, -1),
+ Vec3f(-1, -1, 1),
+ Vec3f(-1, -1, -1),
+ Vec3f(0, 0, 0),
+ }));
+
+ Convex<Triangle>* convexHullTri = dynamic_cast<Convex<Triangle>*>(
+ ConvexBase::convexHull(points, 9, true, NULL));
+ convexHullTriCopy = new Convex<Triangle>(*convexHullTri);
+ BOOST_CHECK(*convexHullTri == *convexHullTriCopy);
+ }
+ Convex<Triangle>* convexHullTriCopyOfCopy =
+ new Convex<Triangle>(*convexHullTriCopy);
+ BOOST_CHECK(*convexHullTriCopyOfCopy == *convexHullTriCopy);
+}
+
+BOOST_AUTO_TEST_CASE(convex_clone) {
+ std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
+ Vec3f(1, 1, 1),
+ Vec3f(1, 1, -1),
+ Vec3f(1, -1, 1),
+ Vec3f(1, -1, -1),
+ Vec3f(-1, 1, 1),
+ Vec3f(-1, 1, -1),
+ Vec3f(-1, -1, 1),
+ Vec3f(-1, -1, -1),
+ Vec3f(0, 0, 0),
+ }));
+
+ Convex<Triangle>* convexHullTri = dynamic_cast<Convex<Triangle>*>(
+ ConvexBase::convexHull(points, 9, true, NULL));
+ Convex<Triangle>* convexHullTriCopy;
+ convexHullTriCopy = convexHullTri->clone();
+ BOOST_CHECK(*convexHullTri == *convexHullTriCopy);
+}
+
#endif
diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp
index d909e6e2122a533374fb81b3ae60b686ae16c115..213bf9b34b1f53483295be9c4e1759f2a4b9cb9f 100644
--- a/test/geometric_shapes.cpp
+++ b/test/geometric_shapes.cpp
@@ -239,12 +239,12 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylinderbox) {
Box s2(1.6, 0.6, 0.025);
Transform3f tf1(
- Quaternion3f(0.5279170511703305, -0.50981118132505521,
- -0.67596178682051911, 0.0668715876735793),
+ Quatf(0.5279170511703305, -0.50981118132505521, -0.67596178682051911,
+ 0.0668715876735793),
Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
Transform3f tf2(
- Quaternion3f(0.70738826916719977, 0, 0, 0.70682518110536596),
+ Quatf(0.70738826916719977, 0, 0, 0.70682518110536596),
Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
GJKSolver solver;
@@ -285,8 +285,8 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylinderbox) {
s1 = Cylinder(0.06, 0.1);
tf1.setTranslation(
Vec3f(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293));
- tf1.setQuatRotation(Quaternion3f(0.52613359459338371, 0.32189408354839893,
- 0.70415587451837913, -0.35175580165512249));
+ tf1.setQuatRotation(Quatf(0.52613359459338371, 0.32189408354839893,
+ 0.70415587451837913, -0.35175580165512249));
res = solver.shapeDistance(s1, tf1, s2, tf2, distance, p1, p2, normal);
}
@@ -450,7 +450,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) {
// FCL_REAL depth;
Vec3f normal;
- Quaternion3f q;
+ Quatf q;
q = AngleAxis((FCL_REAL)3.140 / 6, UnitZ);
tf1 = Transform3f();
@@ -996,7 +996,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << -5, 0, 0;
- depth = 10;
+ depth = -10;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1004,7 +1004,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(-5, 0, 0));
- depth = 10;
+ depth = -10;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1012,7 +1012,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(5, 0, 0));
contact << -2.5, 0, 0;
- depth = 15;
+ depth = -15;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1020,7 +1020,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(5, 0, 0));
contact = transform.transform(Vec3f(-2.5, 0, 0));
- depth = 15;
+ depth = -15;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1028,7 +1028,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-5, 0, 0));
contact << -7.5, 0, 0;
- depth = 5;
+ depth = -5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1036,7 +1036,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
contact = transform.transform(Vec3f(-7.5, 0, 0));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1054,7 +1054,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(10.1, 0, 0));
contact << 0.05, 0, 0;
- depth = 20.1;
+ depth = -20.1;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1062,7 +1062,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
contact = transform.transform(Vec3f(0.05, 0, 0));
- depth = 20.1;
+ depth = -20.1;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1081,51 +1081,77 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) {
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
+ Vec3f p1, p2;
- tf1 = Transform3f();
+ FCL_REAL eps = 1e-6;
+ tf1 = Transform3f(Vec3f(eps, 0, 0));
tf2 = Transform3f();
- contact.setZero();
- depth = 10;
+ 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;
+ testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
+
+ tf1 = transform * tf1;
+ tf2 = transform;
+ contact = transform.transform((p1 + p2) / 2);
+ normal =
+ transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ SET_LINE;
+ testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
+
+ eps = -1e-6;
+ tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf2 = Transform3f();
+ 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;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
- tf1 = transform;
+ tf1 = transform * tf1;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
- depth = 10;
+ contact = transform.transform((p1 + p2) / 2);
normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(5, 0, 0));
- contact << 5, 0, 0;
- depth = 5;
+ p1 << 10, 0, 0;
+ p2 << 5, 0, 0;
+ contact << (p1 + p2) / 2;
+ depth = -5;
normal << 1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(5, 0, 0));
- contact = transform.transform(Vec3f(5, 0, 0));
- depth = 5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -5;
normal = transform.getRotation() * Vec3f(1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-5, 0, 0));
- contact << -5, 0, 0;
- depth = 5;
+ p1 << -10, 0, 0;
+ p2 << -5, 0, 0;
+ contact << (p1 + p2) / 2;
+ depth = -5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
- contact = transform.transform(Vec3f(-5, 0, 0));
- depth = 5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1168,7 +1194,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << -1.25, 0, 0;
- depth = 2.5;
+ depth = -2.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1176,7 +1202,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(-1.25, 0, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1184,7 +1210,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(1.25, 0, 0));
contact << -0.625, 0, 0;
- depth = 3.75;
+ depth = -3.75;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1192,7 +1218,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
contact = transform.transform(Vec3f(-0.625, 0, 0));
- depth = 3.75;
+ depth = -3.75;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1200,7 +1226,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-1.25, 0, 0));
contact << -1.875, 0, 0;
- depth = 1.25;
+ depth = -1.25;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1208,7 +1234,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
contact = transform.transform(Vec3f(-1.875, 0, 0));
- depth = 1.25;
+ depth = -1.25;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1216,7 +1242,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(2.51, 0, 0));
contact << 0.005, 0, 0;
- depth = 5.01;
+ depth = -5.01;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1224,7 +1250,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
contact = transform.transform(Vec3f(0.005, 0, 0));
- depth = 5.01;
+ depth = -5.01;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1261,48 +1287,53 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) {
tf1 = Transform3f();
tf2 = Transform3f();
- contact << 0, 0, 0;
- depth = 2.5;
+ Vec3f p1(2.5, 0, 0);
+ Vec3f p2(0, 0, 0);
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(1.25, 0, 0));
- contact << 1.25, 0, 0;
- depth = 1.25;
+ p2 << 1.25, 0, 0;
+ contact << (p1 + p2) / 2;
+ depth = -1.25;
normal << 1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
- contact = transform.transform(Vec3f(1.25, 0, 0));
- depth = 1.25;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -1.25;
normal = transform.getRotation() * Vec3f(1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-1.25, 0, 0));
- contact << -1.25, 0, 0;
- depth = 1.25;
+ p1 << -2.5, 0, 0;
+ p2 << -1.25, 0, 0;
+ contact << (p1 + p2) / 2;
+ depth = -1.25;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
- contact = transform.transform(Vec3f(-1.25, 0, 0));
- depth = 1.25;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -1.25;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1350,7 +1381,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << -2.5, 0, 0;
- depth = 5;
+ depth = -5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1358,7 +1389,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(-2.5, 0, 0));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1366,7 +1397,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(2.5, 0, 0));
contact << -1.25, 0, 0;
- depth = 7.5;
+ depth = -7.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1374,7 +1405,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
contact = transform.transform(Vec3f(-1.25, 0, 0));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1382,7 +1413,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-2.5, 0, 0));
contact << -3.75, 0, 0;
- depth = 2.5;
+ depth = -2.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1390,7 +1421,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
contact = transform.transform(Vec3f(-3.75, 0, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1398,7 +1429,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(5.1, 0, 0));
contact << 0.05, 0, 0;
- depth = 10.1;
+ depth = -10.1;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1406,7 +1437,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
contact = transform.transform(Vec3f(0.05, 0, 0));
- depth = 10.1;
+ depth = -10.1;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1426,7 +1457,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, -2.5, 0;
- depth = 5;
+ depth = -5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1434,7 +1465,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, -2.5, 0));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1442,7 +1473,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 2.5, 0));
contact << 0, -1.25, 0;
- depth = 7.5;
+ depth = -7.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1450,7 +1481,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
contact = transform.transform(Vec3f(0, -1.25, 0));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1458,7 +1489,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, -2.5, 0));
contact << 0, -3.75, 0;
- depth = 2.5;
+ depth = -2.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1466,7 +1497,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
contact = transform.transform(Vec3f(0, -3.75, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1474,7 +1505,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 5.1, 0));
contact << 0, 0.05, 0;
- depth = 10.1;
+ depth = -10.1;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1482,7 +1513,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
contact = transform.transform(Vec3f(0, 0.05, 0));
- depth = 10.1;
+ depth = -10.1;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1502,7 +1533,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, 0, -5;
- depth = 10;
+ depth = -10;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1510,7 +1541,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, 0, -5));
- depth = 10;
+ depth = -10;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1518,7 +1549,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 2.5));
contact << 0, 0, -3.75;
- depth = 12.5;
+ depth = -12.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1526,7 +1557,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
contact = transform.transform(Vec3f(0, 0, -3.75));
- depth = 12.5;
+ depth = -12.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1534,7 +1565,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, -2.5));
contact << 0, 0, -6.25;
- depth = 7.5;
+ depth = -7.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1542,7 +1573,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
contact = transform.transform(Vec3f(0, 0, -6.25));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1550,7 +1581,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 10.1));
contact << 0, 0, 0.05;
- depth = 20.1;
+ depth = -20.1;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1558,7 +1589,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
contact = transform.transform(Vec3f(0, 0, 0.05));
- depth = 20.1;
+ depth = -20.1;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1591,7 +1622,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, 0, 0;
- depth = 5;
+ depth = -5;
normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true);
@@ -1599,7 +1630,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, 0, 0));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true);
@@ -1607,7 +1638,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(2.5, 0, 0));
contact << 2.5, 0, 0;
- depth = 2.5;
+ depth = -2.5;
normal << 1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
@@ -1615,7 +1646,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
contact = transform.transform(Vec3f(2.5, 0, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
@@ -1623,7 +1654,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-2.5, 0, 0));
contact << -2.5, 0, 0;
- depth = 2.5;
+ depth = -2.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
@@ -1631,7 +1662,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
contact = transform.transform(Vec3f(-2.5, 0, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
@@ -1661,7 +1692,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, 0, 0;
- depth = 5;
+ depth = -5;
normal << 0, 1, 0; // (0, 1, 0) or (0, -1, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true);
@@ -1669,7 +1700,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, 0, 0));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true);
@@ -1677,7 +1708,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 2.5, 0));
contact << 0, 2.5, 0;
- depth = 2.5;
+ depth = -2.5;
normal << 0, 1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
@@ -1685,7 +1716,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
contact = transform.transform(Vec3f(0, 2.5, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
@@ -1693,7 +1724,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, -2.5, 0));
contact << 0, -2.5, 0;
- depth = 2.5;
+ depth = -2.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
@@ -1701,7 +1732,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
contact = transform.transform(Vec3f(0, -2.5, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
@@ -1731,7 +1762,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, 0, 0;
- depth = 10;
+ depth = -10;
normal << 0, 0, 1; // (0, 0, 1) or (0, 0, -1)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true);
@@ -1739,7 +1770,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, 0, 0));
- depth = 10;
+ depth = -10;
normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true);
@@ -1747,7 +1778,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 2.5));
contact << 0, 0, 2.5;
- depth = 7.5;
+ depth = -7.5;
normal << 0, 0, 1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
@@ -1755,7 +1786,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
contact = transform.transform(Vec3f(0, 0, 2.5));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(0, 0, 1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
@@ -1763,7 +1794,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, -2.5));
contact << 0, 0, -2.5;
- depth = 7.5;
+ depth = -7.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
@@ -1771,7 +1802,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
contact = transform.transform(Vec3f(0, 0, -2.5));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
@@ -1814,7 +1845,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << -2.5, 0, 0;
- depth = 5;
+ depth = -5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1822,7 +1853,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(-2.5, 0, 0));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1830,7 +1861,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(2.5, 0, 0));
contact << -1.25, 0, 0;
- depth = 7.5;
+ depth = -7.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1838,7 +1869,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
contact = transform.transform(Vec3f(-1.25, 0, 0));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1846,7 +1877,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-2.5, 0, 0));
contact << -3.75, 0, 0;
- depth = 2.5;
+ depth = -2.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1854,7 +1885,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
contact = transform.transform(Vec3f(-3.75, 0, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1862,7 +1893,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(5.1, 0, 0));
contact << 0.05, 0, 0;
- depth = 10.1;
+ depth = -10.1;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1870,7 +1901,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
contact = transform.transform(Vec3f(0.05, 0, 0));
- depth = 10.1;
+ depth = -10.1;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1890,7 +1921,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, -2.5, 0;
- depth = 5;
+ depth = -5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1898,7 +1929,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, -2.5, 0));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1906,7 +1937,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 2.5, 0));
contact << 0, -1.25, 0;
- depth = 7.5;
+ depth = -7.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1914,7 +1945,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
contact = transform.transform(Vec3f(0, -1.25, 0));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1922,7 +1953,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, -2.5, 0));
contact << 0, -3.75, 0;
- depth = 2.5;
+ depth = -2.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1930,7 +1961,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
contact = transform.transform(Vec3f(0, -3.75, 0));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1938,7 +1969,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 5.1, 0));
contact << 0, 0.05, 0;
- depth = 10.1;
+ depth = -10.1;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1946,7 +1977,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
contact = transform.transform(Vec3f(0, 0.05, 0));
- depth = 10.1;
+ depth = -10.1;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1966,7 +1997,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, 0, -2.5;
- depth = 5;
+ depth = -5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1974,7 +2005,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, 0, -2.5));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1982,7 +2013,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 2.5));
contact << 0, 0, -1.25;
- depth = 7.5;
+ depth = -7.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1990,7 +2021,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
contact = transform.transform(Vec3f(0, 0, -1.25));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -1998,7 +2029,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, -2.5));
contact << 0, 0, -3.75;
- depth = 2.5;
+ depth = -2.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2006,7 +2037,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
contact = transform.transform(Vec3f(0, 0, -3.75));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2014,7 +2045,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 5.1));
contact << 0, 0, 0.05;
- depth = 10.1;
+ depth = -10.1;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2022,7 +2053,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
contact = transform.transform(Vec3f(0, 0, 0.05));
- depth = 10.1;
+ depth = -10.1;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2051,51 +2082,80 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) {
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
+ Vec3f p1, p2;
- tf1 = Transform3f();
+ FCL_REAL eps = 1e-6;
+ tf1 = Transform3f(Vec3f(eps, 0, 0));
tf2 = Transform3f();
- contact << 0, 0, 0;
- depth = 5;
- normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
+ 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;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
- tf1 = transform;
+ tf1 = transform * tf1;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
- depth = 5;
- normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -5 + eps;
+ normal =
+ transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ SET_LINE;
+ testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
+
+ eps = -1e-6;
+ tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf2 = Transform3f();
+ 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;
+ testShapeIntersection(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() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(2.5, 0, 0));
- contact << 2.5, 0, 0;
- depth = 2.5;
+ p1 << 5, 0, 0;
+ p2 << 2.5, 0, 0;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
- contact = transform.transform(Vec3f(2.5, 0, 0));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-2.5, 0, 0));
- contact << -2.5, 0, 0;
- depth = 2.5;
+ p1 << -5, 0, 0;
+ p2 << -2.5, 0, 0;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
- contact = transform.transform(Vec3f(-2.5, 0, 0));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2122,50 +2182,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) {
hs = Plane(Vec3f(0, 1, 0), 0);
- tf1 = Transform3f();
+ eps = 1e-6;
+ tf1 = Transform3f(Vec3f(0, eps, 0));
tf2 = Transform3f();
- contact << 0, 0, 0;
- depth = 5;
+ 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;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
- tf1 = transform;
+ tf1 = transform * tf1;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
- depth = 5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -5 + eps;
+ normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ SET_LINE;
+ testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
+
+ eps = -1e-6;
+ tf1 = Transform3f(Vec3f(0, eps, 0));
+ tf2 = Transform3f();
+ 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;
+ testShapeIntersection(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() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 2.5, 0));
- contact << 0, 2.5, 0;
- depth = 2.5;
+ p1 << 0, 5, 0;
+ p2 << 0, 2.5, 0;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 0, 1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
- contact = transform.transform(Vec3f(0, 2.5, 0));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, -2.5, 0));
- contact << 0, -2.5, 0;
- depth = 2.5;
+ p1 << 0, -5, 0;
+ p2 << 0, -2.5, 0;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
- contact = transform.transform(Vec3f(0, -2.5, 0));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2192,50 +2278,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) {
hs = Plane(Vec3f(0, 0, 1), 0);
- tf1 = Transform3f();
+ eps = 1e-6;
+ tf1 = Transform3f(Vec3f(0, 0, eps));
tf2 = Transform3f();
- contact << 0, 0, 0;
- depth = 5;
+ 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;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
- tf1 = transform;
+ tf1 = transform * tf1;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
- depth = 5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -5 + eps;
+ normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ SET_LINE;
+ testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
+
+ eps = -1e-6;
+ tf1 = Transform3f(Vec3f(0, 0, eps));
+ tf2 = Transform3f();
+ 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;
+ testShapeIntersection(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() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 2.5));
- contact << 0, 0, 2.5;
- depth = 2.5;
+ p1 << 0, 0, 5;
+ p2 << 0, 0, 2.5;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 0, 0, 1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
- contact = transform.transform(Vec3f(0, 0, 2.5));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 0, 1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, -2.5));
- contact << 0, 0, -2.5;
- depth = 2.5;
+ p1 << 0, 0, -5.;
+ p2 << 0, 0, -2.5;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
- contact = transform.transform(Vec3f(0, 0, -2.5));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2278,7 +2390,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << -2.5, 0, -5;
- depth = 5;
+ depth = -5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2286,7 +2398,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(-2.5, 0, -5));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2294,7 +2406,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(2.5, 0, 0));
contact << -1.25, 0, -5;
- depth = 7.5;
+ depth = -7.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2302,7 +2414,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
contact = transform.transform(Vec3f(-1.25, 0, -5));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2310,7 +2422,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-2.5, 0, 0));
contact << -3.75, 0, -5;
- depth = 2.5;
+ depth = -2.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2318,7 +2430,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
contact = transform.transform(Vec3f(-3.75, 0, -5));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2326,7 +2438,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(5.1, 0, 0));
contact << 0.05, 0, -5;
- depth = 10.1;
+ depth = -10.1;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2334,7 +2446,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
contact = transform.transform(Vec3f(0.05, 0, -5));
- depth = 10.1;
+ depth = -10.1;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2354,7 +2466,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, -2.5, -5;
- depth = 5;
+ depth = -5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2362,7 +2474,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, -2.5, -5));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2370,7 +2482,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 2.5, 0));
contact << 0, -1.25, -5;
- depth = 7.5;
+ depth = -7.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2378,7 +2490,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
contact = transform.transform(Vec3f(0, -1.25, -5));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2386,7 +2498,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, -2.5, 0));
contact << 0, -3.75, -5;
- depth = 2.5;
+ depth = -2.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2394,7 +2506,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
contact = transform.transform(Vec3f(0, -3.75, -5));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2402,7 +2514,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 5.1, 0));
contact << 0, 0.05, -5;
- depth = 10.1;
+ depth = -10.1;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2410,7 +2522,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
contact = transform.transform(Vec3f(0, 0.05, -5));
- depth = 10.1;
+ depth = -10.1;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2430,7 +2542,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f();
contact << 0, 0, -2.5;
- depth = 5;
+ depth = -5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2438,7 +2550,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform;
contact = transform.transform(Vec3f(0, 0, -2.5));
- depth = 5;
+ depth = -5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2446,7 +2558,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 2.5));
contact << 0, 0, -1.25;
- depth = 7.5;
+ depth = -7.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2454,7 +2566,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
contact = transform.transform(Vec3f(0, 0, -1.25));
- depth = 7.5;
+ depth = -7.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2462,7 +2574,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, -2.5));
contact << 0, 0, -3.75;
- depth = 2.5;
+ depth = -2.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2470,7 +2582,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
contact = transform.transform(Vec3f(0, 0, -3.75));
- depth = 2.5;
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2478,7 +2590,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 5.1));
contact << 0, 0, 0.05;
- depth = 10.1;
+ depth = -10.1;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2486,7 +2598,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) {
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
contact = transform.transform(Vec3f(0, 0, 0.05));
- depth = 10.1;
+ depth = -10.1;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2515,19 +2627,43 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) {
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
+ Vec3f p1, p2;
- tf1 = Transform3f();
+ FCL_REAL eps = 1e-6;
+ tf1 = Transform3f(Vec3f(eps, 0, 0));
tf2 = Transform3f();
- contact << 0, 0, 0;
- depth = 5;
+ 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;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
- tf1 = transform;
+ tf1 = transform * tf1;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
- depth = 5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -5 + eps;
+ normal =
+ transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ SET_LINE;
+ testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
+
+ eps = -1e-6;
+ tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf2 = Transform3f();
+ 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;
+ testShapeIntersection(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() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
@@ -2535,32 +2671,36 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) {
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(2.5, 0, 0));
- contact << 2.5, 0, -2.5;
- depth = 2.5;
+ p1 << 5, 0, -5;
+ p2 << 2.5, 0, -5;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
- contact = transform.transform(Vec3f(2.5, 0, -2.5));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(-2.5, 0, 0));
- contact << -2.5, 0, -2.5;
- depth = 2.5;
+ p1 << -5, 0, -5;
+ p2 << -2.5, 0, -5;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << -1, 0, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
- contact = transform.transform(Vec3f(-2.5, 0, -2.5));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(-1, 0, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2587,50 +2727,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) {
hs = Plane(Vec3f(0, 1, 0), 0);
- tf1 = Transform3f();
+ eps = 1e-6;
+ tf1 = Transform3f(Vec3f(0, eps, 0));
tf2 = Transform3f();
- contact << 0, 0, 0;
- depth = 5;
+ 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;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
- tf1 = transform;
+ tf1 = transform * tf1;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
- depth = 5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -5 + eps;
+ normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ SET_LINE;
+ testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
+
+ eps = -1e-6;
+ tf1 = Transform3f(Vec3f(0, eps, 0));
+ tf2 = Transform3f();
+ 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;
+ testShapeIntersection(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() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 2.5, 0));
- contact << 0, 2.5, -2.5;
- depth = 2.5;
+ p1 << 0, 5, -5;
+ p2 << 0, 2.5, -5;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 0, 1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
- contact = transform.transform(Vec3f(0, 2.5, -2.5));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, -2.5, 0));
- contact << 0, -2.5, -2.5;
- depth = 2.5;
+ p1 << 0, -5, -5;
+ p2 << 0, -2.5, -5;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 0, -1, 0;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
- contact = transform.transform(Vec3f(0, -2.5, -2.5));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, -1, 0);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2657,50 +2823,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) {
hs = Plane(Vec3f(0, 0, 1), 0);
- tf1 = Transform3f();
+ eps = 1e-6;
+ tf1 = Transform3f(Vec3f(0, 0, eps));
tf2 = Transform3f();
- contact << 0, 0, 0;
- depth = 5;
+ 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;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
- tf1 = transform;
+ tf1 = transform * tf1;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
- depth = 5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -5 + eps;
+ normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ SET_LINE;
+ testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
+
+ eps = -1e-6;
+ tf1 = Transform3f(Vec3f(0, 0, eps));
+ tf2 = Transform3f();
+ 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;
+ testShapeIntersection(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() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, 2.5));
- contact << 0, 0, 2.5;
- depth = 2.5;
+ p1 << 0, 0, 5;
+ p2 << 0, 0, 2.5;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 0, 0, 1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
- contact = transform.transform(Vec3f(0, 0, 2.5));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 0, 1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(0, 0, -2.5));
- contact << 0, 0, -2.5;
- depth = 2.5;
+ p1 << 0, 0, -5;
+ p2 << 0, 0, -2.5;
+ contact << (p1 + p2) / 2;
+ depth = -2.5;
normal << 0, 0, -1;
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
- contact = transform.transform(Vec3f(0, 0, -2.5));
- depth = 2.5;
+ contact = transform.transform((p1 + p2) / 2);
+ depth = -2.5;
normal = transform.getRotation() * Vec3f(0, 0, -1);
SET_LINE;
testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal);
@@ -2823,12 +3015,12 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxbox) {
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
closest_p1, closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver1.shapeDistance(s2, Transform3f(), s2,
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
@@ -2975,12 +3167,12 @@ BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder) {
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
closest_p1, closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver1.shapeDistance(s1, Transform3f(), s2,
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
@@ -3021,12 +3213,12 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecone) {
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
closest_p1, closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver1.shapeDistance(s1, Transform3f(), s2,
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
@@ -3067,12 +3259,12 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder) {
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
closest_p1, closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver1.shapeDistance(s1, Transform3f(), s2,
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
@@ -3210,7 +3402,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) {
// FCL_REAL depth;
Vec3f normal;
- Quaternion3f q;
+ Quatf q;
q = AngleAxis((FCL_REAL)3.140 / 6, UnitZ);
tf1 = Transform3f();
@@ -3807,12 +3999,12 @@ BOOST_AUTO_TEST_CASE(boxbox) {
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
closest_p1, closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver2.shapeDistance(s1, Transform3f(), s2,
Transform3f(Vec3f(15.1, 0, 0)), dist, closest_p1,
@@ -3899,12 +4091,12 @@ BOOST_AUTO_TEST_CASE(cylindercylinder) {
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
closest_p1, closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver2.shapeDistance(s1, Transform3f(), s2,
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
@@ -3945,12 +4137,12 @@ BOOST_AUTO_TEST_CASE(conecone) {
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
closest_p1, closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist <= 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(res);
res = solver2.shapeDistance(s1, Transform3f(), s2,
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
diff --git a/test/gjk.cpp b/test/gjk.cpp
index 32b0ae4c022fea885103c908932d289428551957..2b6f50b7c6f014916b8dbb7de04688e4f5548125 100644
--- a/test/gjk.cpp
+++ b/test/gjk.cpp
@@ -49,7 +49,7 @@ using hpp::fcl::FCL_REAL;
using hpp::fcl::GJKSolver;
using hpp::fcl::GJKVariant;
using hpp::fcl::Matrix3f;
-using hpp::fcl::Quaternion3f;
+using hpp::fcl::Quatf;
using hpp::fcl::Transform3f;
using hpp::fcl::TriangleP;
using hpp::fcl::Vec3f;
@@ -103,8 +103,8 @@ void test_gjk_distance_triangle_triangle(
Q2_loc = Vec3f(-10.926, -1.284259033203125, 3.7281499023437501);
Q3_loc = Vec3f(-10.926, -1.2866180419921875, 3.72335400390625);
Transform3f tf(
- Quaternion3f(-0.42437287410898855, -0.26862477561450587,
- -0.46249645019513175, 0.73064726592483387),
+ Quatf(-0.42437287410898855, -0.26862477561450587,
+ -0.46249645019513175, 0.73064726592483387),
Vec3f(-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
tf1 = tf;
} else if (i == 1) {
@@ -323,8 +323,8 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray,
Sphere sphere(1.);
typedef Eigen::Matrix<FCL_REAL, 4, 1> Vec4f;
- Transform3f tf0(Quaternion3f(Vec4f::Random().normalized()), Vec3f::Zero()),
- tf1(Quaternion3f(Vec4f::Random().normalized()), center_distance * ray);
+ Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3f::Zero()),
+ tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray);
details::MinkowskiDiff shape;
shape.set(&sphere, &sphere, tf0, tf1);
diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp
index cb239d90f8a4c520afe5a142d496bfaa71384fab..844a327b60ddaa2cad0ae3c0e49f41088db53a03 100644
--- a/test/gjk_convergence_criterion.cpp
+++ b/test/gjk_convergence_criterion.cpp
@@ -136,21 +136,21 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1,
mink_diff.set(&shape0, &shape1, identity, transforms[i]);
GJK::Status res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess);
- BOOST_CHECK(gjk1.getIterations() <= max_iterations);
+ BOOST_CHECK(gjk1.iterations <= max_iterations);
Vec3f ray1 = gjk1.ray;
res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res1 != GJK::Status::Failed);
EIGEN_VECTOR_IS_APPROX(gjk1.ray, ray1, 1e-8);
GJK::Status res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess);
- BOOST_CHECK(gjk2.getIterations() <= max_iterations);
+ BOOST_CHECK(gjk2.iterations <= max_iterations);
Vec3f ray2 = gjk2.ray;
res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res2 != GJK::Status::Failed);
EIGEN_VECTOR_IS_APPROX(gjk2.ray, ray2, 1e-8);
GJK::Status res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess);
- BOOST_CHECK(gjk3.getIterations() <= max_iterations);
+ BOOST_CHECK(gjk3.iterations <= max_iterations);
Vec3f ray3 = gjk3.ray;
res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res3 != GJK::Status::Failed);
diff --git a/test/math.cpp b/test/math.cpp
index f26ebb3415a5bbd7aaa49063f14bcd5d53ea10f8..bdbed85c635e7afe36a2f9c950477d88962c945c 100644
--- a/test/math.cpp
+++ b/test/math.cpp
@@ -111,7 +111,7 @@ Vec3f rotate(Vec3f input, FCL_REAL w, Vec3f vec) {
}
BOOST_AUTO_TEST_CASE(quaternion) {
- Quaternion3f q1(Quaternion3f::Identity()), q2, q3;
+ Quatf q1(Quatf::Identity()), q2, q3;
q2 = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
q3 = q2.inverse();
@@ -124,7 +124,7 @@ BOOST_AUTO_TEST_CASE(quaternion) {
}
BOOST_AUTO_TEST_CASE(transform) {
- Quaternion3f q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
+ Quatf q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
Vec3f T(0, 1, 2);
Transform3f tf(q, T);
diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd0b6cff5939fd8b62dae4fbc7444b056aaa2fb9
--- /dev/null
+++ b/test/normal_and_nearest_points.cpp
@@ -0,0 +1,536 @@
+/*
+ * 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 NORMAL_AND_NEAREST_POINTS
+#include <boost/test/included/unit_test.hpp>
+
+#include <hpp/fcl/fwd.hh>
+#include <hpp/fcl/shape/geometric_shapes.h>
+#include <hpp/fcl/collision_data.h>
+#include <hpp/fcl/BV/OBBRSS.h>
+#include <hpp/fcl/BVH/BVH_model.h>
+#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
+
+#include "utility.h"
+
+using hpp::fcl::Box;
+using hpp::fcl::Capsule;
+using hpp::fcl::CollisionRequest;
+using hpp::fcl::CollisionResult;
+using hpp::fcl::Cone;
+using hpp::fcl::constructPolytopeFromEllipsoid;
+using hpp::fcl::Contact;
+using hpp::fcl::Convex;
+using hpp::fcl::Cylinder;
+using hpp::fcl::DistanceRequest;
+using hpp::fcl::DistanceResult;
+using hpp::fcl::Ellipsoid;
+using hpp::fcl::FCL_REAL;
+using hpp::fcl::Halfspace;
+using hpp::fcl::Plane;
+using hpp::fcl::shared_ptr;
+using hpp::fcl::Sphere;
+using hpp::fcl::Transform3f;
+using hpp::fcl::Triangle;
+using hpp::fcl::Vec3f;
+
+// 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
+// wether `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)
+// Thus:
+// --> if o1 and o2 are not in collision, translating o2 by vector (abs(dist) -
+// eps) * normal should bring them in collision.
+// --> if o1 and o2 are in collision, translating o1 by vector (dist + eps)
+// * normal should separate them.
+// --> finally, if abs(dist) > 0, we should have normal = sign(dist) * (p2 -
+// p1).normalized() and p1 = p2 - dist * normal
+template <typename ShapeType1, typename ShapeType2>
+void test_normal_and_nearest_points(const ShapeType1& o1,
+ const ShapeType2& o2) {
+ // Generate random poses for o2
+#ifndef NDEBUG // if debug mode
+ std::size_t n = 1;
+#else
+ size_t n = 10000;
+#endif
+ FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
+ std::vector<Transform3f> transforms;
+ generateRandomTransforms(extents, transforms, n);
+ Transform3f tf1 = Transform3f::Identity();
+
+ CollisionRequest colreq;
+ colreq.distance_upper_bound = std::numeric_limits<FCL_REAL>::max();
+ CollisionResult colres;
+ DistanceRequest distreq;
+ DistanceResult distres;
+
+ 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.
+ Transform3f tf2 = transforms[i];
+ colres.clear();
+ distres.clear();
+ size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres);
+ FCL_REAL dist = distance(&o1, tf1, &o2, tf2, distreq, distres);
+
+ if (col) {
+ BOOST_CHECK(dist <= 0.);
+ BOOST_CHECK_CLOSE(dist, distres.min_distance, 1e-6);
+ Contact contact = colres.getContact(0);
+ BOOST_CHECK_CLOSE(dist, contact.penetration_depth, 1e-6);
+
+ Vec3f cp1 = contact.nearest_points[0];
+ EIGEN_VECTOR_IS_APPROX(cp1, distres.nearest_points[0], 1e-6);
+
+ Vec3f cp2 = contact.nearest_points[1];
+ EIGEN_VECTOR_IS_APPROX(cp2, distres.nearest_points[1], 1e-6);
+ BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), 1e-6);
+ EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, 1e-6);
+
+ Vec3f separation_vector = contact.penetration_depth * contact.normal;
+ EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6);
+
+ if (dist < 0) {
+ EIGEN_VECTOR_IS_APPROX(contact.normal, -(cp2 - cp1).normalized(), 1e-6);
+ }
+
+ // Separate the shapes
+ Vec3f t = tf1.getTranslation();
+ FCL_REAL eps = 1e-2;
+ tf1.setTranslation(t + separation_vector - eps * contact.normal);
+ colres.clear();
+ distres.clear();
+ col = collide(&o1, tf1, &o2, tf2, colreq, colres);
+ dist = distance(&o1, tf1, &o2, tf2, distreq, distres);
+ BOOST_CHECK(dist > 0);
+ BOOST_CHECK(!col);
+ BOOST_CHECK_CLOSE(colres.distance_lower_bound, dist, 1e-6);
+ BOOST_CHECK(fabs(colres.distance_lower_bound - eps) <= 1e-2);
+ cp1 = distres.nearest_points[0];
+ cp2 = distres.nearest_points[1];
+ BOOST_CHECK_CLOSE(dist, (cp1 - cp2).norm(), 1e-6);
+ EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, 1e-6);
+
+ separation_vector = dist * distres.normal;
+ EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6);
+
+ if (dist > 0) {
+ EIGEN_VECTOR_IS_APPROX(distres.normal, (cp2 - cp1).normalized(), 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);
+
+ Vec3f cp1 = distres.nearest_points[0];
+ Vec3f cp2 = distres.nearest_points[1];
+ BOOST_CHECK_CLOSE(dist, (cp1 - cp2).norm(), 1e-6);
+ EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, 1e-6);
+
+ Vec3f separation_vector = dist * distres.normal;
+ EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6);
+
+ if (dist > 0) {
+ EIGEN_VECTOR_IS_APPROX(distres.normal, (cp2 - cp1).normalized(), 1e-6);
+ }
+
+ // Bring the shapes in collision
+ Vec3f t = tf1.getTranslation();
+ FCL_REAL eps = 1e-2;
+ tf1.setTranslation(t + separation_vector + eps * distres.normal);
+ colres.clear();
+ distres.clear();
+ col = collide(&o1, tf1, &o2, tf2, colreq, colres);
+ dist = distance(&o1, tf1, &o2, tf2, distreq, distres);
+ BOOST_CHECK(dist < 0);
+ BOOST_CHECK(col);
+ // Contrary to Contact::penetration_depth,
+ // CollisionResult::distance_lower_bound is a signed distance like
+ // DistanceResult::min_distance
+ BOOST_CHECK_CLOSE(colres.distance_lower_bound, dist, 1e-6);
+ BOOST_CHECK(fabs(colres.distance_lower_bound - -eps) <= 1e-2);
+ Contact contact = colres.getContact(0);
+ cp1 = contact.nearest_points[0];
+ EIGEN_VECTOR_IS_APPROX(cp1, distres.nearest_points[0], 1e-6);
+
+ cp2 = contact.nearest_points[1];
+ EIGEN_VECTOR_IS_APPROX(cp2, distres.nearest_points[1], 1e-6);
+ BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), 1e-6);
+ EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, 1e-6);
+
+ separation_vector = contact.penetration_depth * contact.normal;
+ EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6);
+
+ if (dist < 0) {
+ EIGEN_VECTOR_IS_APPROX(contact.normal, -(cp2 - cp1).normalized(), 1e-6);
+ }
+ }
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_sphere) {
+ FCL_REAL r = 0.5;
+ shared_ptr<Sphere> o1(new Sphere(r));
+ shared_ptr<Sphere> o2(new Sphere(r));
+
+ test_normal_and_nearest_points(*o1.get(), *o2.get());
+}
+
+BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_capsule) {
+ FCL_REAL r = 0.5;
+ FCL_REAL h = 1.;
+ shared_ptr<Sphere> o1(new Sphere(r));
+ shared_ptr<Capsule> o2(new Capsule(r, 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) {
+ FCL_REAL r = 0.5;
+ FCL_REAL rbox = 2 * 0.5;
+ shared_ptr<Box> o1(new Box(rbox, rbox, rbox));
+ shared_ptr<Sphere> o2(new Sphere(r));
+
+ 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) {
+ FCL_REAL r = 0.5;
+ Convex<Triangle> o1_ = constructPolytopeFromEllipsoid(Ellipsoid(r, r, r));
+ shared_ptr<Convex<Triangle>> o1(new Convex<Triangle>(
+ o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons));
+ Convex<Triangle> o2_ = constructPolytopeFromEllipsoid(Ellipsoid(r, r, r));
+ shared_ptr<Convex<Triangle>> o2(new Convex<Triangle>(
+ o2_.points, o2_.num_points, o2_.polygons, o2_.num_polygons));
+
+ test_normal_and_nearest_points(*o1.get(), *o2.get());
+}
+
+BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_box) {
+ FCL_REAL r = 0.5;
+ FCL_REAL rbox = 2 * 0.5;
+ Convex<Triangle> o1_ = constructPolytopeFromEllipsoid(Ellipsoid(r, r, r));
+ shared_ptr<Convex<Triangle>> o1(new Convex<Triangle>(
+ o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons));
+ shared_ptr<Box> o2(new Box(rbox, rbox, rbox));
+
+ 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) {
+ FCL_REAL r = 0.5;
+ Convex<Triangle> o1_ = constructPolytopeFromEllipsoid(Ellipsoid(r, r, r));
+ shared_ptr<Convex<Triangle>> o1(new Convex<Triangle>(
+ o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons));
+ shared_ptr<Ellipsoid> o2(new Ellipsoid(0.5 * r, 1.3 * r, 0.8 * r));
+
+ 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_plane) {
+ FCL_REAL rbox = 1;
+ shared_ptr<Box> o1(new Box(rbox, rbox, rbox));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Plane> 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) {
+ FCL_REAL rbox = 1;
+ shared_ptr<Box> o1(new Box(rbox, rbox, rbox));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Halfspace> 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) {
+ FCL_REAL r = 0.5;
+ FCL_REAL d = 1.;
+ shared_ptr<Capsule> o1(new Capsule(r, d));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Halfspace> 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) {
+ FCL_REAL r = 0.5;
+ shared_ptr<Sphere> o1(new Sphere(r));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Halfspace> 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) {
+ FCL_REAL r = 0.5;
+ shared_ptr<Sphere> o1(new Sphere(r));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Plane> 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) {
+ FCL_REAL r = 0.5;
+ Convex<Triangle> o1_ = constructPolytopeFromEllipsoid(Ellipsoid(r, r, r));
+ shared_ptr<Convex<Triangle>> o1(new Convex<Triangle>(
+ o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Halfspace> 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_halfspace) {
+ FCL_REAL r = 0.5;
+ FCL_REAL h = 1.;
+ shared_ptr<Cone> o1(new Cone(r, h));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Halfspace> 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) {
+ FCL_REAL r = 0.5;
+ FCL_REAL h = 1.;
+ shared_ptr<Cylinder> o1(new Cylinder(r, h));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Halfspace> 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) {
+ FCL_REAL r = 0.5;
+ FCL_REAL h = 1.;
+ shared_ptr<Cone> o1(new Cone(r, h));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Plane> 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) {
+ FCL_REAL r = 0.5;
+ FCL_REAL h = 1.;
+ shared_ptr<Cylinder> o1(new Cylinder(r, h));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Plane> 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) {
+ FCL_REAL r = 0.5;
+ FCL_REAL h = 1.;
+ shared_ptr<Capsule> o1(new Capsule(r, h));
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Plane> 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) {
+ FCL_REAL r = 0.5;
+ FCL_REAL h = 1.;
+ shared_ptr<Capsule> o1(new Capsule(r, h));
+ shared_ptr<Capsule> o2(new Capsule(r, 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_cylinder) {
+ FCL_REAL r = 0.5;
+ FCL_REAL h = 1.;
+ shared_ptr<Sphere> o1(new Sphere(r));
+ shared_ptr<Cylinder> o2(new Cylinder(r, 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) {
+ Vec3f radii(0.3, 0.5, 0.2);
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Ellipsoid> o1(new Ellipsoid(radii));
+ shared_ptr<Halfspace> 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_ellispoid_plane) {
+ Vec3f radii(0.3, 0.5, 0.4);
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Ellipsoid> o1(new Ellipsoid(radii));
+ shared_ptr<Plane> o2(new Plane(n, offset));
+
+ test_normal_and_nearest_points(*o1.get(), *o2.get());
+ test_normal_and_nearest_points(*o2.get(), *o1.get());
+}
+
+using hpp::fcl::BVHModel;
+using hpp::fcl::OBBRSS;
+
+void test_normal_and_nearest_points(const BVHModel<OBBRSS>& 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
+ FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
+ std::vector<Transform3f> transforms;
+ generateRandomTransforms(extents, transforms, n);
+ Transform3f tf1 = Transform3f::Identity();
+ transforms[0] = Transform3f::Identity();
+
+ CollisionRequest colreq;
+ colreq.distance_upper_bound = std::numeric_limits<FCL_REAL>::max();
+ colreq.num_max_contacts = 100;
+ CollisionResult colres;
+ DistanceRequest distreq;
+ DistanceResult distres;
+
+ for (size_t i = 0; i < n; i++) {
+ Transform3f tf2 = transforms[i];
+ colres.clear();
+ distres.clear();
+ size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres);
+ FCL_REAL 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);
+
+ Vec3f cp1 = contact.nearest_points[0];
+ Vec3f 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, 1e-6);
+
+ Vec3f separation_vector = contact.penetration_depth * contact.normal;
+ EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6);
+
+ if (dist < 0) {
+ EIGEN_VECTOR_IS_APPROX(contact.normal, -(cp2 - cp1).normalized(),
+ 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<OBBRSS>& o2) {
+ test_normal_and_nearest_points(o2, o1);
+}
+
+BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) {
+ Box* box_ptr = new hpp::fcl::Box(1, 1, 1);
+ hpp::fcl::CollisionGeometryPtr_t b1(box_ptr);
+ BVHModel<hpp::fcl::OBBRSS> o1 = BVHModel<OBBRSS>();
+ generateBVHModel(o1, *box_ptr, Transform3f());
+ o1.buildConvexRepresentation(false);
+
+ FCL_REAL offset = 0.1;
+ Vec3f n = Vec3f::Random();
+ n.normalize();
+ shared_ptr<Halfspace> o2(new Halfspace(n, offset));
+
+ test_normal_and_nearest_points(o1, *o2.get());
+ test_normal_and_nearest_points(*o2.get(), o1);
+}
diff --git a/test/obb.cpp b/test/obb.cpp
index cad2c8c5a24425fcb7b066a6beb7591c2923de92..5c5b8acf82d1a3c1014c65cadaee563dea24c0c3 100644
--- a/test/obb.cpp
+++ b/test/obb.cpp
@@ -70,7 +70,7 @@ void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b,
T = (Vec3f::Random() / sqrt(3)) * 1.5 * N;
// T.setZero();
- Quaternion3f q;
+ Quatf q;
q.coeffs().setRandom();
q.normalize();
B = q;
@@ -1204,7 +1204,7 @@ BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T,
result.failure = false;
}
if (result.failure) {
- std::cerr << "\nR = " << Quaternion3f(B).coeffs().transpose().format(py_fmt)
+ std::cerr << "\nR = " << Quatf(B).coeffs().transpose().format(py_fmt)
<< "\nT = " << T.transpose().format(py_fmt)
<< "\na = " << a.transpose().format(py_fmt)
<< "\nb = " << b.transpose().format(py_fmt)
diff --git a/test/octree.cpp b/test/octree.cpp
index 222cf5f3418df598786022397f2c817c459e6442..566fb6dd504dd0967a65f3ab8c18f91259c879a6 100644
--- a/test/octree.cpp
+++ b/test/octree.cpp
@@ -2,6 +2,7 @@
* 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
@@ -42,7 +43,9 @@
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/distance.h>
+#include <hpp/fcl/hfield.h>
#include <hpp/fcl/shape/geometric_shapes.h>
+#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/internal/BV_splitter.h>
#include "utility.h"
@@ -50,16 +53,7 @@
namespace utf = boost::unit_test::framework;
-using hpp::fcl::BVHModel;
-using hpp::fcl::BVSplitter;
-using hpp::fcl::CollisionRequest;
-using hpp::fcl::CollisionResult;
-using hpp::fcl::FCL_REAL;
-using hpp::fcl::OBBRSS;
-using hpp::fcl::OcTree;
-using hpp::fcl::Transform3f;
-using hpp::fcl::Triangle;
-using hpp::fcl::Vec3f;
+using namespace hpp::fcl;
void makeMesh(const std::vector<Vec3f>& vertices,
const std::vector<Triangle>& triangles, BVHModel<OBBRSS>& model) {
@@ -108,7 +102,7 @@ hpp::fcl::OcTree makeOctree(const BVHModel<OBBRSS>& mesh,
return OcTree(octree);
}
-BOOST_AUTO_TEST_CASE(OCTREE) {
+BOOST_AUTO_TEST_CASE(octree_mesh) {
Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
"", "", "(", ")");
FCL_REAL resolution(10.);
@@ -130,6 +124,22 @@ BOOST_AUTO_TEST_CASE(OCTREE) {
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<uint8_t> bytes = envOctree.tobytes();
+ BOOST_CHECK(bytes.size() > 0 && bytes.size() <= envOctree.toBoxes().size() *
+ 3 * sizeof(FCL_REAL));
+ }
+
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
#ifndef NDEBUG // if debug mode
@@ -166,3 +176,76 @@ BOOST_AUTO_TEST_CASE(OCTREE) {
}
}
}
+
+BOOST_AUTO_TEST_CASE(octree_height_field) {
+ Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
+ "", "", "(", ")");
+ FCL_REAL resolution(10.);
+ std::vector<Vec3f> pEnv;
+ std::vector<Triangle> tEnv;
+ boost::filesystem::path path(TEST_RESOURCES_DIR);
+ loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv);
+
+ BVHModel<OBBRSS> 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(
+ hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution));
+
+ std::cout << "Finished loading octree." << std::endl;
+
+ // Building hfield
+ const FCL_REAL x_dim = 10, y_dim = 20;
+ const int nx = 100, ny = 100;
+ const FCL_REAL max_altitude = 1., min_altitude = 0.;
+ const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);
+
+ HeightField<AABB> hfield(x_dim, y_dim, heights, min_altitude);
+ hfield.computeLocalAABB();
+
+ std::vector<Transform3f> transforms;
+ FCL_REAL 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 = hpp::fcl::getNbRun(utf::master_test_suite().argc,
+ utf::master_test_suite().argv, N);
+
+ generateRandomTransforms(extents, transforms, 2 * N);
+
+ CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND,
+ 1);
+ for (std::size_t i = 0; i < N; ++i) {
+ CollisionResult resultBox;
+ CollisionResult resultHfield1, resultHfield2;
+ Transform3f tf1(transforms[2 * i]);
+ Transform3f tf2(transforms[2 * i + 1]);
+
+ Box box;
+ Transform3f box_tf;
+ constructBox(hfield.aabb_local, tf2, box, box_tf);
+
+ // Test collision between octree and equivalent box.
+ hpp::fcl::collide(&envOctree, tf1, &box, box_tf, request, resultBox);
+ // Test collision between octree and hfield.
+ hpp::fcl::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1);
+ hpp::fcl::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) {
+ hpp::fcl::DistanceRequest dreq;
+ hpp::fcl::DistanceResult dres;
+ hpp::fcl::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);
+ }
+ }
+}
diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py
index b64850e5996c1a20eea9cbee33820434cd3cb728..0b4d52a278a7d8f279f865cc6a1db2b11f6a67e6 100644
--- a/test/python_unit/geometric_shapes.py
+++ b/test/python_unit/geometric_shapes.py
@@ -156,6 +156,11 @@ class TestGeometricShapes(TestCase):
Ic_ref = np.diag([Icx_ref, Icx_ref, Iz_ref])
self.assertApprox(Ic, Ic_ref)
+ def test_BVH(self):
+ bvh = hppfcl.BVHModelOBBRSS()
+ self.assertEqual(bvh.num_vertices, 0)
+ self.assertEqual(bvh.vertices().shape, (0, 3))
+
def test_convex(self):
verts = hppfcl.StdVec_Vec3f()
faces = hppfcl.StdVec_Triangle()
@@ -174,8 +179,8 @@ class TestGeometricShapes(TestCase):
hppfcl.Convex.convexHull(verts, False, None)
qhullAvailable = True
except Exception as e:
- self.assertEqual(
- str(e), "Library built without qhull. Cannot build object of this type."
+ self.assertIn(
+ "Library built without qhull. Cannot build object of this type.", str(e)
)
qhullAvailable = False
@@ -187,7 +192,7 @@ class TestGeometricShapes(TestCase):
hppfcl.Convex.convexHull(verts[:3], False, None)
except Exception as e:
self.assertIn(
- str(e), "You shouldn't use this function with less than 4 points."
+ "You shouldn't use this function with less than 4 points.", str(e)
)
diff --git a/test/security_margin.cpp b/test/security_margin.cpp
index 9232f9c4e03c75fcfb620fb9dcbb3301764bb6ba..1fd584a7b28c47d0ac15cf36a88baef41755b6ce 100644
--- a/test/security_margin.cpp
+++ b/test/security_margin.cpp
@@ -194,7 +194,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0, 1e-8);
- BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8);
+ BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, 1e-8);
}
// No security margin - no collision
@@ -216,14 +216,13 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
CollisionResult collisionResult;
const double distance = 0.01;
collisionRequest.security_margin = distance;
- Transform3f tf2_no_collision(
+ Transform3f tf2(
Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
- collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest,
- collisionResult);
+ collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0, 1e-8);
- BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth,
- distance, 1e-8);
+ BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance,
+ 1e-8);
}
// Negative security margin - collion because the two spheres are in contact
@@ -237,8 +236,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
- BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth,
- distance, 1e-8);
+ BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance,
+ 1e-8);
}
// Negative security margin - no collision
@@ -269,7 +268,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
- BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8);
+ BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, 1e-8);
}
// No security margin - no collision
@@ -297,8 +296,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
- BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth,
- distance, 1e-8);
+ BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance,
+ 1e-8);
}
// Negative security margin - collion because the two capsules are in contact
@@ -312,8 +311,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
- BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth,
- distance, 1e-8);
+ BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance,
+ 1e-8);
}
// Negative security margin - no collision
@@ -345,7 +344,7 @@ BOOST_AUTO_TEST_CASE(box_box) {
collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol);
- BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8);
+ BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, 1e-8);
}
// No security margin - no collision
@@ -373,7 +372,7 @@ BOOST_AUTO_TEST_CASE(box_box) {
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound,
-collisionRequest.security_margin, tol);
- BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8);
+ BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, 1e-8);
}
// Negative security margin - no collision
@@ -402,8 +401,8 @@ BOOST_AUTO_TEST_CASE(box_box) {
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);
+ BOOST_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance,
+ tol);
}
}
@@ -419,7 +418,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol);
- BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8);
+ BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, 1e-8);
}
// No security margin - no collision
@@ -447,7 +446,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound,
-collisionRequest.security_margin, tol);
- BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8);
+ BOOST_CHECK_SMALL(collisionResult.getContact(0).penetration_depth, 1e-8);
}
// Negative security margin - no collision
@@ -479,8 +478,8 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
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_CHECK_CLOSE(collisionResult.getContact(0).penetration_depth, distance,
+ tol);
}
}
diff --git a/test/serialization.cpp b/test/serialization.cpp
index 14bbf33fb3a67e31d614def9ccaefbcc16beb52c..c4d3fe51bdc157990eaf75f6f07892237a5e3ee7 100644
--- a/test/serialization.cpp
+++ b/test/serialization.cpp
@@ -1,7 +1,7 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2021-2022 INRIA.
+ * Copyright (c) 2021-2023 INRIA.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -49,6 +49,10 @@
#include <hpp/fcl/serialization/convex.h>
#include <hpp/fcl/serialization/memory.h>
+#ifdef HPP_FCL_HAS_OCTOMAP
+#include <hpp/fcl/serialization/octree.h>
+#endif
+
#include "utility.h"
#include "fcl_resources/config.h"
@@ -84,15 +88,80 @@ bool check(const T& value, const T& other) {
return value == other;
}
+template <typename T>
+bool check_ptr(const T* value, const T* other) {
+ return *value == *other;
+}
+
enum SerializationMode { TXT = 1, XML = 2, BIN = 4, STREAM = 8 };
+template <typename T>
+void test_serialization(const T* value, T& other_value,
+ const int mode = TXT | XML | BIN | STREAM) {
+ test_serialization(*value, other_value, mode);
+}
+
+template <typename T,
+ bool is_base = std::is_base_of<T, CollisionGeometry>::value>
+struct test_pointer_serialization_impl {
+ static void run(const T&, T&, const int) {}
+};
+
+template <typename T>
+struct test_pointer_serialization_impl<T, true> {
+ 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<const CollisionGeometry*>(ptr),
+ *reinterpret_cast<const CollisionGeometry*>(ptr)));
+
+ {
+ std::ifstream ifs(txt_filename.c_str());
+ boost::archive::text_iarchive ia(ifs);
+
+ ia >> other_ptr;
+ }
+ BOOST_CHECK(
+ check(*reinterpret_cast<const CollisionGeometry*>(ptr),
+ *reinterpret_cast<const CollisionGeometry*>(other_ptr)));
+ }
+ }
+};
+
+template <typename T>
+void test_pointer_serialization(const T& value, T& other_value,
+ const int mode = TXT | XML | BIN | STREAM) {
+ test_pointer_serialization_impl<T>::run(value, other_value, mode);
+}
+
template <typename T>
void test_serialization(const T& value, T& other_value,
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
@@ -114,6 +183,24 @@ void test_serialization(const T& value, T& other_value,
BOOST_CHECK(check(value, other_value));
}
+ // XML
+ if (mode & 0x2) {
+ {
+ std::ofstream ofs(xml_filename.c_str());
+ boost::archive::xml_oarchive oa(ofs);
+ oa << boost::serialization::make_nvp("value", value);
+ }
+ BOOST_CHECK(check(value, value));
+
+ {
+ std::ifstream ifs(xml_filename.c_str());
+ boost::archive::xml_iarchive ia(ifs, boost::archive::no_codecvt);
+
+ ia >> boost::serialization::make_nvp("value", other_value);
+ }
+ BOOST_CHECK(check(value, other_value));
+ }
+
// BIN
if (mode & 0x4) {
{
@@ -141,6 +228,31 @@ void test_serialization(const T& value, T& other_value,
loadFromBinary(other_value, buffer);
BOOST_CHECK(check(value, other_value));
}
+
+ // Test std::shared_ptr<T>
+ {
+ const boost::filesystem::path txt_ptr_filename(tmp_path / txt_ptr_path);
+ std::shared_ptr<T> ptr = std::make_shared<T>(value);
+
+ {
+ std::ofstream ofs(txt_ptr_filename.c_str());
+
+ boost::archive::text_oarchive oa(ofs);
+ oa << ptr;
+ }
+ BOOST_CHECK(check_ptr(ptr.get(), ptr.get()));
+
+ std::shared_ptr<T> other_ptr = nullptr;
+ {
+ std::ifstream ifs(txt_ptr_filename.c_str());
+ boost::archive::text_iarchive ia(ifs);
+
+ ia >> other_ptr;
+ }
+ BOOST_CHECK(check_ptr(ptr.get(), other_ptr.get()));
+ }
+
+ test_pointer_serialization(value, other_value);
}
template <typename T>
@@ -166,6 +278,9 @@ BOOST_AUTO_TEST_CASE(test_collision_data) {
collision_result.addContact(contact);
collision_result.addContact(contact);
collision_result.distance_lower_bound = 0.1;
+ collision_result.normal.setOnes();
+ collision_result.nearest_points[0].setRandom();
+ collision_result.nearest_points[1].setRandom();
test_serialization(collision_result);
DistanceRequest distance_request(true, 1., 2.);
@@ -178,6 +293,16 @@ BOOST_AUTO_TEST_CASE(test_collision_data) {
test_serialization(distance_result);
}
+template <typename T>
+void checkEqualStdVector(const std::vector<T>& v1, const std::vector<T>& 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<Vec3f> p1, p2;
std::vector<Triangle> t1, t2;
@@ -191,11 +316,19 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) {
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);
@@ -233,6 +366,36 @@ BOOST_AUTO_TEST_CASE(test_Convex) {
Convex<Triangle> convex_copy;
test_serialization(convex, convex_copy);
}
+
+ // Test std::shared_ptr<CollisionGeometry>
+ {
+ 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";
+ Convex<Triangle> convex_copy;
+
+ std::shared_ptr<CollisionGeometry> ptr =
+ std::make_shared<Convex<Triangle>>(convex);
+ BOOST_CHECK(ptr.get());
+ {
+ std::ofstream ofs(txt_filename.c_str());
+
+ boost::archive::text_oarchive oa(ofs);
+ oa << ptr;
+ }
+ BOOST_CHECK(check(*reinterpret_cast<Convex<Triangle>*>(ptr.get()), convex));
+
+ std::shared_ptr<CollisionGeometry> other_ptr = nullptr;
+ BOOST_CHECK(!other_ptr.get());
+ {
+ std::ifstream ifs(txt_filename.c_str());
+ boost::archive::text_iarchive ia(ifs);
+
+ ia >> other_ptr;
+ }
+ BOOST_CHECK(
+ check(convex, *reinterpret_cast<Convex<Triangle>*>(other_ptr.get())));
+ }
}
#endif
@@ -303,6 +466,45 @@ BOOST_AUTO_TEST_CASE(test_shapes) {
}
}
+#ifdef HPP_FCL_HAS_OCTOMAP
+BOOST_AUTO_TEST_CASE(test_octree) {
+ const FCL_REAL resolution = 1e-2;
+ const Matrixx3f points = Matrixx3f::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));
diff --git a/test/utility.cpp b/test/utility.cpp
index d00f9f6924d43f73b867ed310725d4d43143c11f..b343de07925c588ccc7e5b716743a06ce76d20c5 100644
--- a/test/utility.cpp
+++ b/test/utility.cpp
@@ -365,8 +365,8 @@ std::string getNodeTypeName(NODE_TYPE node_type) {
return std::string("invalid");
}
-Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) {
- Quaternion3f q;
+Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) {
+ Quatf q;
q.w() = w;
q.x() = x;
q.y() = y;
@@ -478,53 +478,54 @@ Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
FCL_REAL PHI = (1 + std::sqrt(5)) / 2;
// vertices
- Vec3f* pts = new Vec3f[12];
- pts[0] = Vec3f(-1, PHI, 0);
- pts[1] = Vec3f(1, PHI, 0);
- pts[2] = Vec3f(-1, -PHI, 0);
- pts[3] = Vec3f(1, -PHI, 0);
-
- pts[4] = Vec3f(0, -1, PHI);
- pts[5] = Vec3f(0, 1, PHI);
- pts[6] = Vec3f(0, -1, -PHI);
- pts[7] = Vec3f(0, 1, -PHI);
-
- pts[8] = Vec3f(PHI, 0, -1);
- pts[9] = Vec3f(PHI, 0, 1);
- pts[10] = Vec3f(-PHI, 0, -1);
- pts[11] = Vec3f(-PHI, 0, 1);
-
- for (int i = 0; i < 12; ++i) {
- toEllipsoid(pts[i], ellipsoid);
+ std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
+ Vec3f(-1, PHI, 0),
+ Vec3f(1, PHI, 0),
+ Vec3f(-1, -PHI, 0),
+ Vec3f(1, -PHI, 0),
+
+ Vec3f(0, -1, PHI),
+ Vec3f(0, 1, PHI),
+ Vec3f(0, -1, -PHI),
+ Vec3f(0, 1, -PHI),
+
+ Vec3f(PHI, 0, -1),
+ Vec3f(PHI, 0, 1),
+ Vec3f(-PHI, 0, -1),
+ Vec3f(-PHI, 0, 1),
+ }));
+
+ std::vector<Vec3f>& pts_ = *pts;
+ for (size_t i = 0; i < 12; ++i) {
+ toEllipsoid(pts_[i], ellipsoid);
}
// faces
- Triangle* tris = new Triangle[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 Convex<Triangle>(true,
- pts, // points
+ std::shared_ptr<std::vector<Triangle>> tris(new std::vector<Triangle>(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 Convex<Triangle>(pts, // points
12, // num_points
tris, // triangles
20 // number of triangles
diff --git a/test/utility.h b/test/utility.h
index 370dbf391cc320ffd56b6d1827b4ecec9a53f788..667023883994fdfef1a9b042918b789dfb89fe8b 100644
--- a/test/utility.h
+++ b/test/utility.h
@@ -179,7 +179,7 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
std::string getNodeTypeName(NODE_TYPE node_type);
-Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);
+Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);
std::ostream& operator<<(std::ostream& os, const Transform3f& tf);