diff --git a/.travis.yml b/.travis.yml
index 097781a4d4903027e5ec16ae9cc84e906531033a..37310a76a1aec730f98385a27d0869a6ec8c2ffd 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -36,6 +36,7 @@ script:
# Run unit tests
- make test
- # Make sure we can install with no issues
+ # Make sure we can install and uninstall with no issues
- sudo make -j4 install
+ - sudo make -j4 uninstall
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 41cfb3e8e819545347195e88c1da61a041d3c698..ec5eda945732999fc6eeeaf5a9571c5e255a6531 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -145,6 +145,13 @@ install(DIRECTORY include/ DESTINATION include
install(FILES "${pkg_conf_file_out}" DESTINATION lib/pkgconfig/ COMPONENT pkgconfig)
+# Add uninstall target
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules/cmake_uninstall.cmake.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/CMakeModules/cmake_uninstall.cmake"
+ IMMEDIATE @ONLY)
+add_custom_target(uninstall
+ "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/CMakeModules/cmake_uninstall.cmake")
enable_testing()
add_subdirectory(test)
diff --git a/CMakeModules/cmake_uninstall.cmake.in b/CMakeModules/cmake_uninstall.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..14cd01d2bc9158871c44ca874d6f2026eeea6f57
--- /dev/null
+++ b/CMakeModules/cmake_uninstall.cmake.in
@@ -0,0 +1,21 @@
+if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+ message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+
+file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+string(REGEX REPLACE "\n" ";" files "${files}")
+foreach(file ${files})
+ message(STATUS "Uninstalling $ENV{DESTDIR}${file}")
+ if(EXISTS "$ENV{DESTDIR}${file}")
+ execute_process(
+ COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}"
+ OUTPUT_VARIABLE rm_out
+ RESULT_VARIABLE rm_retval
+ )
+ if(NOT "${rm_retval}" STREQUAL 0)
+ message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}")
+ endif(NOT "${rm_retval}" STREQUAL 0)
+ else(EXISTS "$ENV{DESTDIR}${file}")
+ message(STATUS "File $ENV{DESTDIR}${file} does not exist.")
+ endif(EXISTS "$ENV{DESTDIR}${file}")
+endforeach(file)
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..529a238fa178840a0341d29281d0630b9d558838
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,32 @@
+Software License Agreement (BSD License)
+
+ Copyright (c) 2008-2014, Willow Garage, Inc.
+ Copyright (c) 2014-2015, Open Source Robotics Foundation
+ All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions
+ are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above
+ copyright notice, this list of conditions and the following
+ disclaimer in the documentation and/or other materials provided
+ with the distribution.
+ * Neither the name of Open Source Robotics Foundation nor the names of its
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGE.
diff --git a/README.md b/README.md
index 492826f73d5d742f71eb339b57f21b5db0fd1f0a..6cfa0a149450521dd9337f809a1aa29f6fc6f94e 100644
--- a/README.md
+++ b/README.md
@@ -46,7 +46,7 @@ std::vector<Triangle> triangles;
// code to set the vertices and triangles
...
// BVHModel is a template class for mesh geometry, for default OBBRSS template is used
-typedef BVHModel<OBBRSS>* Model;
+typedef BVHModel<OBBRSS> Model;
Model* model = new Model();
// add the mesh data into the BVHModel structure
model->beginModel();
diff --git a/include/fcl/BV/AABB.h b/include/fcl/BV/AABB.h
index 3a4800fdb065674e2cae90418a5184cbbeaa7263..a29cda41cf724c79ff4694ba0f83c76046b6b479 100644
--- a/include/fcl/BV/AABB.h
+++ b/include/fcl/BV/AABB.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BV/BV.h b/include/fcl/BV/BV.h
index 899e188902a819ac1d0723b85dbff9baaa5c295e..bee56f67afbdb453a25910d5fee7177556278a91 100644
--- a/include/fcl/BV/BV.h
+++ b/include/fcl/BV/BV.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BV/BV_node.h b/include/fcl/BV/BV_node.h
index 8082b2d13881fb056c1cbafd44cf5c73ebf374bc..a39ea548cd3b89d78414e5b318a9bdd3e21b201a 100644
--- a/include/fcl/BV/BV_node.h
+++ b/include/fcl/BV/BV_node.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BV/OBB.h b/include/fcl/BV/OBB.h
index 7dbb2fa552233b857f7b04fd76c8b3384eca7eb7..9290fec8387a47f6eae01a8036bed6bdf6a72d85 100644
--- a/include/fcl/BV/OBB.h
+++ b/include/fcl/BV/OBB.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BV/OBBRSS.h b/include/fcl/BV/OBBRSS.h
index be30bc9b87d43d5214fd8e136ec6e6dbcbc051e8..bb77f1312f38d7d9c6f8716b94ff53535598b727 100644
--- a/include/fcl/BV/OBBRSS.h
+++ b/include/fcl/BV/OBBRSS.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BV/RSS.h b/include/fcl/BV/RSS.h
index fd556f6ef36a17a88c538c62bc0eefe24fbffbd3..91fc442ae356614fe7139660f3e2d1970443e7bc 100644
--- a/include/fcl/BV/RSS.h
+++ b/include/fcl/BV/RSS.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BV/kDOP.h b/include/fcl/BV/kDOP.h
index 8be4aa4b2c8a032ad46b63cb03c334fb9aa32986..add3ba20cd394baa08244287face4e565da8afa2 100644
--- a/include/fcl/BV/kDOP.h
+++ b/include/fcl/BV/kDOP.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BV/kIOS.h b/include/fcl/BV/kIOS.h
index 93383eb6be96215830eb121e075f12c418105961..8e52424d8f2470503eb320868d9efea47c522101 100644
--- a/include/fcl/BV/kIOS.h
+++ b/include/fcl/BV/kIOS.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BVH/BVH_front.h b/include/fcl/BVH/BVH_front.h
index e37c3cbddc77ea9422fe1112e628680d5f43e62a..8aea57c19cc86907d3ed84e439edff57681de23c 100644
--- a/include/fcl/BVH/BVH_front.h
+++ b/include/fcl/BVH/BVH_front.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BVH/BVH_internal.h b/include/fcl/BVH/BVH_internal.h
index 2b099750ff82b889e6cb0ddb785583fdde0588de..397aef431bb3b438021fcd4a2ddce5256eb7fe2d 100644
--- a/include/fcl/BVH/BVH_internal.h
+++ b/include/fcl/BVH/BVH_internal.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BVH/BVH_model.h b/include/fcl/BVH/BVH_model.h
index 847db5bee0c6f9a94febf6e96dd98403e2d9f269..69a30b3d1e8ba0cd5c79f44d90aa1b3be8310a7e 100644
--- a/include/fcl/BVH/BVH_model.h
+++ b/include/fcl/BVH/BVH_model.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BVH/BVH_utility.h b/include/fcl/BVH/BVH_utility.h
index 5937b46948259105effe153d3c714a2bf15ece32..87c190deebc122fe2d3d834c2778ab0992383319 100644
--- a/include/fcl/BVH/BVH_utility.h
+++ b/include/fcl/BVH/BVH_utility.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BVH/BV_fitter.h b/include/fcl/BVH/BV_fitter.h
index 8cd548528a3cfce43b849ab1b9d4bb76dfa18b3f..2e7b40eb28d93c6d0acc116977d588e60a64bdda 100644
--- a/include/fcl/BVH/BV_fitter.h
+++ b/include/fcl/BVH/BV_fitter.h
@@ -1,8 +1,8 @@
-
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -15,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/BVH/BV_splitter.h b/include/fcl/BVH/BV_splitter.h
index 39e819d70c963cc7d402ae974599c20ee2c53c96..890f0af43fe82e5b7dc235d345c124299a3c2829 100644
--- a/include/fcl/BVH/BV_splitter.h
+++ b/include/fcl/BVH/BV_splitter.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/CMakeLists.txt b/include/fcl/CMakeLists.txt
index 723bd6d0089847e36bec320cead75eb30b992324..052e7526f390fbf7318f6bc14906ac7c00c16507 100644
--- a/include/fcl/CMakeLists.txt
+++ b/include/fcl/CMakeLists.txt
@@ -1,5 +1,5 @@
-file(GLOB_RECURSE HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/*.h)
-file(GLOB_RECURSE CONFIGURED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/*.h)
+file(GLOB_RECURSE HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/*.h ${CMAKE_CURRENT_SOURCE_DIR}/*.hxx)
+file(GLOB_RECURSE CONFIGURED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/*.h ${CMAKE_CURRENT_BINARY_DIR}/*.hxx)
set(FCL_HEADERS ${HEADERS} ${CONFIGURED_HEADERS} PARENT_SCOPE)
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" FCL_CONFIG_IN_DIR)
diff --git a/include/fcl/articulated_model/joint.h b/include/fcl/articulated_model/joint.h
index a484c26830c87df4e4e0435cf1523dec8720b4a6..1fa05346513f1db2516e6af885cdf5ba49e147ce 100644
--- a/include/fcl/articulated_model/joint.h
+++ b/include/fcl/articulated_model/joint.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/articulated_model/joint_config.h b/include/fcl/articulated_model/joint_config.h
index 61d365eb4cabb767ac980d078ad85ba75c6086dd..f87533d680833973d0f60a36d3ee805f1a09c6f2 100644
--- a/include/fcl/articulated_model/joint_config.h
+++ b/include/fcl/articulated_model/joint_config.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/articulated_model/link.h b/include/fcl/articulated_model/link.h
index 1b5d370174a1bf8233c5959c13be329c43e465c7..1df1c4cb5d758866afac7ef839f64dbb7f91fb30 100644
--- a/include/fcl/articulated_model/link.h
+++ b/include/fcl/articulated_model/link.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/articulated_model/model.h b/include/fcl/articulated_model/model.h
index 102caa73ca799c2ab65e96d582534d6eca412e41..bc95504cb2283d86815e9544c618d68c65bae3aa 100644
--- a/include/fcl/articulated_model/model.h
+++ b/include/fcl/articulated_model/model.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/articulated_model/model_config.h b/include/fcl/articulated_model/model_config.h
index fdd7c50d63f9a186a8e0d37d6c08d1b0650be776..49e8a6405c7cde3bcf5e4ca06917f147554fdc40 100644
--- a/include/fcl/articulated_model/model_config.h
+++ b/include/fcl/articulated_model/model_config.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase.h b/include/fcl/broadphase/broadphase.h
index f08baa887b067ec39f91232f7260257bc93af94d..ded5e11d4854c702884b5a1b29ae32aa6ad5561d 100644
--- a/include/fcl/broadphase/broadphase.h
+++ b/include/fcl/broadphase/broadphase.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase_SSaP.h b/include/fcl/broadphase/broadphase_SSaP.h
index d72f6a81d675ebdfb2327053fca9c67ffddd3443..be3cc46acac17000d9db7266e34d1e176f6c41e6 100644
--- a/include/fcl/broadphase/broadphase_SSaP.h
+++ b/include/fcl/broadphase/broadphase_SSaP.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase_SaP.h b/include/fcl/broadphase/broadphase_SaP.h
index 8fe62b35d0230d38dfa5ea62936f6782993bdd37..62404b3d2158859c64a6461d9b1565b1ea02584f 100644
--- a/include/fcl/broadphase/broadphase_SaP.h
+++ b/include/fcl/broadphase/broadphase_SaP.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase_bruteforce.h b/include/fcl/broadphase/broadphase_bruteforce.h
index 1b27ad88f753ee3bb19ff0def51770b5b4733d3d..12ebba83b0a16a27182615b74a159eddd869e804 100644
--- a/include/fcl/broadphase/broadphase_bruteforce.h
+++ b/include/fcl/broadphase/broadphase_bruteforce.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
index 6cfd3837b63b0bab79e82358bccbf903e0cc0283..1b8ea89684eaeab5db6001d9ed2385512a386259 100644
--- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
+++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
index 02bae125fd40fdf2bef9d7f920847aa243142755..3c132e0923eb89e59df6030a7c6e0331c18743a0 100644
--- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
+++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase_interval_tree.h b/include/fcl/broadphase/broadphase_interval_tree.h
index ba16940b58beafa8377b138466213fbf509ec2bd..eb9c7cd5bb9fe5449d157e7c547584bf97d2de0a 100644
--- a/include/fcl/broadphase/broadphase_interval_tree.h
+++ b/include/fcl/broadphase/broadphase_interval_tree.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase_spatialhash.h b/include/fcl/broadphase/broadphase_spatialhash.h
index 48ed2c3157457a5f98615f8d37a610a717e4628b..21de859d639a85a230285ac9933d2b669d495e9c 100644
--- a/include/fcl/broadphase/broadphase_spatialhash.h
+++ b/include/fcl/broadphase/broadphase_spatialhash.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/broadphase_spatialhash.hxx b/include/fcl/broadphase/broadphase_spatialhash.hxx
index 0a64039402428358096c522f7432642c835740aa..2c1f62d56adc64dcb859bef276922ba81d0f0459 100644
--- a/include/fcl/broadphase/broadphase_spatialhash.hxx
+++ b/include/fcl/broadphase/broadphase_spatialhash.hxx
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/hash.h b/include/fcl/broadphase/hash.h
index a54aacc3dda2b65301c1e2e6f31f15e11ce3bd99..58686dbd7d91722a891f68fb41e2caecc4baa5be 100644
--- a/include/fcl/broadphase/hash.h
+++ b/include/fcl/broadphase/hash.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/hierarchy_tree.h b/include/fcl/broadphase/hierarchy_tree.h
index 64dcd2839e8961725c9fadef3f4e67b01020159e..ed055ff76d6bdc079bde7f33e1a01dd145eea94c 100644
--- a/include/fcl/broadphase/hierarchy_tree.h
+++ b/include/fcl/broadphase/hierarchy_tree.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/hierarchy_tree.hxx b/include/fcl/broadphase/hierarchy_tree.hxx
index 03bb175e842c9563b5817e30ffaa7c4dda9a617b..10ce2f726aee6b51d61a109c3d49a132a97c6c82 100644
--- a/include/fcl/broadphase/hierarchy_tree.hxx
+++ b/include/fcl/broadphase/hierarchy_tree.hxx
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/interval_tree.h b/include/fcl/broadphase/interval_tree.h
index cab4da64e7acefbd60d49e9546117f8336b8d187..9b32948bb5b64a6518fa7fa37b548679cd04e067 100644
--- a/include/fcl/broadphase/interval_tree.h
+++ b/include/fcl/broadphase/interval_tree.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/broadphase/morton.h b/include/fcl/broadphase/morton.h
index 98eeaea722c252e8b3030ad1e8c2cb4dfea0f7b1..11b91ec679614b3ca22cf9d9a7d9bcbf82ee5ae6 100644
--- a/include/fcl/broadphase/morton.h
+++ b/include/fcl/broadphase/morton.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/conservative_advancement.h b/include/fcl/ccd/conservative_advancement.h
index d6d7aeb876edd8e3baf928de3a4a0d6175a385d4..b94f6c106d5a44059fdfe3701493cc6a89085261 100644
--- a/include/fcl/ccd/conservative_advancement.h
+++ b/include/fcl/ccd/conservative_advancement.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/interpolation/interpolation.h b/include/fcl/ccd/interpolation/interpolation.h
index 4148b8ad838e72f4d61985ce425a9bdcdb140761..4e84a7c63319d2337cba094f06435b66ea89a2ba 100644
--- a/include/fcl/ccd/interpolation/interpolation.h
+++ b/include/fcl/ccd/interpolation/interpolation.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/interpolation/interpolation_factory.h b/include/fcl/ccd/interpolation/interpolation_factory.h
index 08f907fd1722c7e7250ea05fe23191ef2e6ec864..c8b3ec5526d7f839694a120693b42ce2965727f6 100644
--- a/include/fcl/ccd/interpolation/interpolation_factory.h
+++ b/include/fcl/ccd/interpolation/interpolation_factory.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/interpolation/interpolation_linear.h b/include/fcl/ccd/interpolation/interpolation_linear.h
index e15252b3469d8b0f7f7a073319fece8d4898306f..ecef37344285d96a25d6911176d70e8a0af09c51 100644
--- a/include/fcl/ccd/interpolation/interpolation_linear.h
+++ b/include/fcl/ccd/interpolation/interpolation_linear.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/interval.h b/include/fcl/ccd/interval.h
index a613d83e0fa8b9ce5743e45e9bc3b68e631d23f6..5afa5b4c17f65f76c98b5273e8f3ca09e4141673 100644
--- a/include/fcl/ccd/interval.h
+++ b/include/fcl/ccd/interval.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/interval_matrix.h b/include/fcl/ccd/interval_matrix.h
index 701fc468955739feefaf895dedd6868b3f21f1b2..6084812a0d48834c9b50dc8e9cf92a5ebbc6cce9 100644
--- a/include/fcl/ccd/interval_matrix.h
+++ b/include/fcl/ccd/interval_matrix.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/interval_vector.h b/include/fcl/ccd/interval_vector.h
index 922fc3e10406d52c5f56b1500e33cd0b1225dca6..745480a832b55e893b75252223068ef850719127 100644
--- a/include/fcl/ccd/interval_vector.h
+++ b/include/fcl/ccd/interval_vector.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/motion.h b/include/fcl/ccd/motion.h
index a281a182d207c36ace52ae19d80744de2406d3ca..b640d6cff6abdb90e538705295d216866be00240 100644
--- a/include/fcl/ccd/motion.h
+++ b/include/fcl/ccd/motion.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/motion_base.h b/include/fcl/ccd/motion_base.h
index 827c4fe02533222ba2bfdeef299b2ecc1043c41f..b675a3bd0aa59d4c630ebbeb70ce27dfd441af53 100644
--- a/include/fcl/ccd/motion_base.h
+++ b/include/fcl/ccd/motion_base.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/taylor_matrix.h b/include/fcl/ccd/taylor_matrix.h
index 49d026e14d08ce8f43a3f3c53673374d34f3d89c..a27435b791a7157edb1499bdeba00bef34a0f535 100644
--- a/include/fcl/ccd/taylor_matrix.h
+++ b/include/fcl/ccd/taylor_matrix.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/taylor_model.h b/include/fcl/ccd/taylor_model.h
index e3f606eca147de9506c136eef6e6180a7d00a0a5..b53b0140698efd1017c860382220ae2e7ea490d7 100644
--- a/include/fcl/ccd/taylor_model.h
+++ b/include/fcl/ccd/taylor_model.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/ccd/taylor_vector.h b/include/fcl/ccd/taylor_vector.h
index 5b4765bc35bd2cda5ba174d1c4c62658c68f25e0..52a68db2b77f1ecf547ca3e2589e91dd6f786625 100644
--- a/include/fcl/ccd/taylor_vector.h
+++ b/include/fcl/ccd/taylor_vector.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/collision.h b/include/fcl/collision.h
index 2288b3a188ddb92a060337f07fc633afddb91666..e8a1237c2213ae798ef30f51d05e660e07e2b01c 100644
--- a/include/fcl/collision.h
+++ b/include/fcl/collision.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h
index 5ea511434682cea1bb059a635af8a0350bce0387..e299e26ede247732092ac9992c3fe62d6b747d83 100644
--- a/include/fcl/collision_data.h
+++ b/include/fcl/collision_data.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/collision_func_matrix.h b/include/fcl/collision_func_matrix.h
index 88f809ac1916eede64b94ab2accf948d21678905..edbbc94181b4493abce0252f0135f99b953d72ea 100644
--- a/include/fcl/collision_func_matrix.h
+++ b/include/fcl/collision_func_matrix.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/collision_node.h b/include/fcl/collision_node.h
index 4bc7b01524b7e5da7e5602c24f029c564673ab13..7c92543e434e9d88a818a8f355d5e08fc8db5d27 100644
--- a/include/fcl/collision_node.h
+++ b/include/fcl/collision_node.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/collision_object.h b/include/fcl/collision_object.h
index 33b0aec89c80d21a4efe10962eebc26163e8e1b1..7cc2fc5fe4fc1d9ff9fade472b515a63ca8f50e9 100644
--- a/include/fcl/collision_object.h
+++ b/include/fcl/collision_object.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -297,7 +298,8 @@ public:
}
/// @brief get geometry from the object instance
- const CollisionGeometry* getCollisionGeometry() const FCL_DEPRECATED
+ FCL_DEPRECATED
+ const CollisionGeometry* getCollisionGeometry() const
{
return cgeom.get();
}
@@ -446,7 +448,8 @@ public:
}
/// @brief get geometry from the object instance
- inline const CollisionGeometry* getCollisionGeometry() const FCL_DEPRECATED
+ FCL_DEPRECATED
+ inline const CollisionGeometry* getCollisionGeometry() const
{
return cgeom.get();
}
diff --git a/include/fcl/config.h.in b/include/fcl/config.h.in
index 1ef93715aa88d2fc852e5843972640be5125b214..fc26c2320fa521129fec88e472cda4d79f200e4d 100644
--- a/include/fcl/config.h.in
+++ b/include/fcl/config.h.in
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/data_types.h b/include/fcl/data_types.h
index 1e8b43e1da9a29c60111b5a0f4f2dda2420b8926..88e995d495883808c3a0d7938f54f55427d6bc93 100644
--- a/include/fcl/data_types.h
+++ b/include/fcl/data_types.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/deprecated.h b/include/fcl/deprecated.h
index 16c74e83b60c9da829e9189e79ab9c0d58db4b25..6a850468a0c339b488a47a3a319497b4e31969fc 100644
--- a/include/fcl/deprecated.h
+++ b/include/fcl/deprecated.h
@@ -1,7 +1,7 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2013, CNRS-LAAS.
+ * Copyright (c) 2013-2015, CNRS-LAAS and AIST
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
* copyright 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
+ * * Neither the name of CNRS-LAAS and AIST nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -41,14 +41,13 @@
// variable as deprecated (i.e. it will emit a warning when using it).
//
// Tagging a function as deprecated:
-// void foo () FCL_DEPRECATED;
+// FCL_DEPRECATED void foo ();
//
// Tagging a type as deprecated:
-// class Foo {};
-// typedef Foo Bar FCL_DEPRECATED;
+// FCL_DEPRECATED class Foo {};
//
// Tagging a variable as deprecated:
-// int a FCL_DEPRECATED = 0;
+// FCL_DEPRECATED int a = 0;
//
// The use of a macro is required as this is /not/ a standardized
// feature of C++ language or preprocessor, even if most of the
diff --git a/include/fcl/distance.h b/include/fcl/distance.h
index be65f07d3f4720d5ca00dadc814f34a19f2bbb97..4f82c8f03059fd8944756195a1fe1fbeb1160952 100644
--- a/include/fcl/distance.h
+++ b/include/fcl/distance.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/distance_func_matrix.h b/include/fcl/distance_func_matrix.h
index 93a3bea3e3802fbe7265f5cc131b2e929e6d726f..2a190349758d7925da03ad20060738d4d299ac47 100644
--- a/include/fcl/distance_func_matrix.h
+++ b/include/fcl/distance_func_matrix.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/intersect.h b/include/fcl/intersect.h
index 047b50a24cba3284d3191e8aff207db2ac55e2a3..ac5153f82128d64948accab8aa77312b79460a91 100644
--- a/include/fcl/intersect.h
+++ b/include/fcl/intersect.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/knn/greedy_kcenters.h b/include/fcl/knn/greedy_kcenters.h
index f2f64848e63d1564224e66dbfcdef2066677a8aa..030327760d1a0d3b2a3132483d1a20039e139fef 100644
--- a/include/fcl/knn/greedy_kcenters.h
+++ b/include/fcl/knn/greedy_kcenters.h
@@ -1,7 +1,7 @@
-/*********************************************************************
+/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Rice University
+ * Copyright (c) 2011-2015, Rice University.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -30,7 +30,7 @@
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
+ */
/* Author: Mark Moll */
diff --git a/include/fcl/knn/nearest_neighbors.h b/include/fcl/knn/nearest_neighbors.h
index 100d8f878b4d66b7cd0806656c2ab3b351d5c7a9..fccd9daefb6b0e4fe8d72b6b51df9e70b7695395 100644
--- a/include/fcl/knn/nearest_neighbors.h
+++ b/include/fcl/knn/nearest_neighbors.h
@@ -1,7 +1,8 @@
-/*********************************************************************
+/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2008-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -30,7 +31,7 @@
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
+ */
/* Author: Ioan Sucan */
diff --git a/include/fcl/knn/nearest_neighbors_GNAT.h b/include/fcl/knn/nearest_neighbors_GNAT.h
index f7cd0d091a955bd1fcfc179ef182c0c123e82291..e6e8196eade6a66b493196fa8897f53ee8cb2e40 100644
--- a/include/fcl/knn/nearest_neighbors_GNAT.h
+++ b/include/fcl/knn/nearest_neighbors_GNAT.h
@@ -1,7 +1,7 @@
-/*********************************************************************
+/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Rice University
+ * Copyright (c) 2011-2015, Rice University.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -30,7 +30,7 @@
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
+ */
/* Author: Mark Moll */
@@ -53,7 +53,7 @@ namespace fcl
See:
S. Brin, "Near neighbor search in large metric spaces," in Proc. 21st
- Conf. on Very Large Databases (VLDB), pp. 574¡V584, 1995.
+ Conf. on Very Large Databases (VLDB), pp. 574�V584, 1995.
*/
template<typename _T>
diff --git a/include/fcl/knn/nearest_neighbors_flann.h b/include/fcl/knn/nearest_neighbors_flann.h
index 76380f3c39f4dae4277c29bf9d0d86b1d2186d01..c63ff4c8b904ef27e75596ab240752851051d1d1 100644
--- a/include/fcl/knn/nearest_neighbors_flann.h
+++ b/include/fcl/knn/nearest_neighbors_flann.h
@@ -1,7 +1,7 @@
-/*********************************************************************
+/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Rice University
+ * Copyright (c) 2011-2015, Rice University.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -30,7 +30,7 @@
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
+ */
/* Author: Mark Moll */
diff --git a/include/fcl/knn/nearest_neighbors_linear.h b/include/fcl/knn/nearest_neighbors_linear.h
index 3275fa7f795cf964dcd48876a4542fd2fa778afa..04145641d90f2e4da33e5f741eb809dbf80f848a 100644
--- a/include/fcl/knn/nearest_neighbors_linear.h
+++ b/include/fcl/knn/nearest_neighbors_linear.h
@@ -1,7 +1,8 @@
-/*********************************************************************
+/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2008-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -30,7 +31,7 @@
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
+ */
/* Author: Ioan Sucan */
diff --git a/include/fcl/knn/nearest_neighbors_sqrtapprox.h b/include/fcl/knn/nearest_neighbors_sqrtapprox.h
index 5b6c46c119631fe045caf1d1c9a0feb3dc3eeee6..00644e57409481743bf3bac82b46c02625b0b9d1 100644
--- a/include/fcl/knn/nearest_neighbors_sqrtapprox.h
+++ b/include/fcl/knn/nearest_neighbors_sqrtapprox.h
@@ -1,7 +1,8 @@
-/*********************************************************************
+/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2008-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -30,7 +31,7 @@
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
+ */
/* Author: Ioan Sucan */
diff --git a/include/fcl/math/math_details.h b/include/fcl/math/math_details.h
index 8dde27b77c665ffa54bf0122a4208ba23f0bcbf7..91db7f524d8e29406aaae329407f490e9edc870d 100644
--- a/include/fcl/math/math_details.h
+++ b/include/fcl/math/math_details.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/math/matrix_3f.h b/include/fcl/math/matrix_3f.h
index 0eafe80e62dad4e8e0fb5571cbe22dadeb6e1a9d..9ecbeee5fbb2b63414a616e9a4a116b8f141c6d8 100644
--- a/include/fcl/math/matrix_3f.h
+++ b/include/fcl/math/matrix_3f.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/math/transform.h b/include/fcl/math/transform.h
index 9e45f8b15e09970d7fd5dce25c5172f679423e87..6fd7f01a424b0523a31213ebefb7a0828925b215 100644
--- a/include/fcl/math/transform.h
+++ b/include/fcl/math/transform.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -171,6 +172,12 @@ Quaternion3f conj(const Quaternion3f& q);
/// @brief inverse of quaternion
Quaternion3f inverse(const Quaternion3f& q);
+static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q)
+{
+ o << "(" << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << ")";
+ return o;
+}
+
/// @brief Simple transform class used locally by InterpMotion
class Transform3f
diff --git a/include/fcl/math/vec_3f.h b/include/fcl/math/vec_3f.h
index e1bb0409c38edf1363a794a530597485e912c7d7..04fca74e7189591fdebd2fe180cd150d22d01373 100644
--- a/include/fcl/math/vec_3f.h
+++ b/include/fcl/math/vec_3f.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/narrowphase/gjk.h b/include/fcl/narrowphase/gjk.h
index eba8c926a37270c873e5bf794cfc1256867936a7..7849a39df3c76004755d43c567db3aa96db80795 100644
--- a/include/fcl/narrowphase/gjk.h
+++ b/include/fcl/narrowphase/gjk.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/narrowphase/gjk_libccd.h b/include/fcl/narrowphase/gjk_libccd.h
index 43c27c85f48395078af332feeff7b93d7ba5408f..c5c7f09a88a62ae56956bda52ddd6bbbb6643031 100644
--- a/include/fcl/narrowphase/gjk_libccd.h
+++ b/include/fcl/narrowphase/gjk_libccd.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index b4dc1c1360e5386d57a53c60ca12a431e2c73a0a..fbb5ff0a1c475f5ea8494e1c6eeccc8f8b9865c4 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -476,7 +477,7 @@ struct GJKSolver_indep
w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
- if(normal) *normal = -epa.normal;
+ if(normal) *normal = epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
diff --git a/include/fcl/octree.h b/include/fcl/octree.h
index 547965fab307e15b493bc551f26c9b4e75a15a9e..451224e35345d31bd346954fc6a33831313c00d0 100644
--- a/include/fcl/octree.h
+++ b/include/fcl/octree.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/profile.h b/include/fcl/profile.h
index f382d58381f13272006ea2fe069ef00ebdd12729..74add5bf0b3846f9098a00867fc8863e5f99a6b5 100644
--- a/include/fcl/profile.h
+++ b/include/fcl/profile.h
@@ -1,7 +1,8 @@
-/*********************************************************************
+/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2008-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -30,7 +31,7 @@
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
+ */
/* Author Ioan Sucan */
diff --git a/include/fcl/shape/geometric_shape_to_BVH_model.h b/include/fcl/shape/geometric_shape_to_BVH_model.h
index b02c1752ebf8bd6564b00f20e6bd405fbb30957f..a07022b85f0fa4862c8b21d516c711f24ae1ad76 100644
--- a/include/fcl/shape/geometric_shape_to_BVH_model.h
+++ b/include/fcl/shape/geometric_shape_to_BVH_model.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/shape/geometric_shapes.h b/include/fcl/shape/geometric_shapes.h
index f714129c1f1072003a6c10cdb8066ced80658ca8..ad2fbff578427d7da3a48d485c9705e39e2e2f88 100644
--- a/include/fcl/shape/geometric_shapes.h
+++ b/include/fcl/shape/geometric_shapes.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/shape/geometric_shapes_utility.h b/include/fcl/shape/geometric_shapes_utility.h
index 3497239fe0eed86eef9162a0bcde01a07fa8a85f..6feab73e71f2c560fa969c23bbe7bfe24e65f706 100644
--- a/include/fcl/shape/geometric_shapes_utility.h
+++ b/include/fcl/shape/geometric_shapes_utility.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -119,8 +120,25 @@ template<>
void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv);
template<>
-void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv);
+void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f& tf, RSS& bv);
+
+template<>
+void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf, OBBRSS& bv);
+
+template<>
+void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf, kIOS& bv);
+
+template<>
+void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv);
+template<>
+void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv);
+
+template<>
+void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv);
+
+template<>
+void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv);
diff --git a/include/fcl/simd/math_simd_details.h b/include/fcl/simd/math_simd_details.h
index 1f64f1722cfdd9a0b6f2b7e1ca2340664295907b..01ebca04d2a5bc3a84762469157dd774932ca3bc 100644
--- a/include/fcl/simd/math_simd_details.h
+++ b/include/fcl/simd/math_simd_details.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/simd/simd_intersect.h b/include/fcl/simd/simd_intersect.h
index b6e8dbbef39d7b3cc6d6dd39f7b7faf00b63f218..312fa7c192fbd2d0a62cdb7c34ad8df3a68fdae0 100644
--- a/include/fcl/simd/simd_intersect.h
+++ b/include/fcl/simd/simd_intersect.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/traversal/traversal_node_base.h b/include/fcl/traversal/traversal_node_base.h
index a53c2b2415b39844db9bc614fa45e7628d2c6064..f6668131e076e8291b3ed5112a3420c69fa333f2 100644
--- a/include/fcl/traversal/traversal_node_base.h
+++ b/include/fcl/traversal/traversal_node_base.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/traversal/traversal_node_bvh_shape.h b/include/fcl/traversal/traversal_node_bvh_shape.h
index fe62b289fddf20915d1a7e6a68a4ba4db1f7a287..5b4a86cf1338747eaaa0f35cf1714d6e81a6c0c1 100644
--- a/include/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/fcl/traversal/traversal_node_bvh_shape.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h
index 3a6a9b77a704bf2078be86966107f5d53154fd91..ac0508243ed718af3c0d265908111338a5a837ca 100644
--- a/include/fcl/traversal/traversal_node_bvhs.h
+++ b/include/fcl/traversal/traversal_node_bvhs.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h
index 3ef5da228e41a921315de4f4ce069871c569bf50..b1cc27afa7e0d13db61469eff6ca4897b96e89de 100644
--- a/include/fcl/traversal/traversal_node_octree.h
+++ b/include/fcl/traversal/traversal_node_octree.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/traversal/traversal_node_setup.h b/include/fcl/traversal/traversal_node_setup.h
index 5f01167c67290709b1ee67a819ac22a97dbf873f..6e016296254cd92d0dec35138a9abc8f78093db4 100644
--- a/include/fcl/traversal/traversal_node_setup.h
+++ b/include/fcl/traversal/traversal_node_setup.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h
index fb2c406221b81fcfdb084ce489fb165f07e8fd9e..01f98d149a1f319bd425cfdf1de2ee87f4a8858f 100644
--- a/include/fcl/traversal/traversal_node_shapes.h
+++ b/include/fcl/traversal/traversal_node_shapes.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/include/fcl/traversal/traversal_recurse.h b/include/fcl/traversal/traversal_recurse.h
index bf621b469c0e540feb7dd9e697fdeeded4eaa3b6..90c6644208adb2da5c6817d253391619457f08f7 100644
--- a/include/fcl/traversal/traversal_recurse.h
+++ b/include/fcl/traversal/traversal_recurse.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index 78e41f7a17a29cdce1702c34d7efc8b1eb3dd34b..460489441a408f106481b739c55958b29d82f2a0 100644
--- a/src/BV/AABB.cpp
+++ b/src/BV/AABB.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 6b1246a030a7141218fa2da4d3b6eb203142ab9e..60f9de15705c87d264b306e2249c4e30f3ad07e0 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp
index 7a703b97c095134330e69b57794b21ad19a56999..ecc447c283cd45c9c4ff7f7ba1454bddaa00ff11 100644
--- a/src/BV/OBBRSS.cpp
+++ b/src/BV/OBBRSS.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 4c29a127e2b57d5ff403c3fd5ebb6fafec5430eb..118bf11f66a145f3087b0ed06c7c53e11e34de5e 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp
index 2b2b0f815df0f7d15210ffe0ddfbd3c912e678c9..c883327625998216a4614d8ee57665f0f0e53c02 100644
--- a/src/BV/kDOP.cpp
+++ b/src/BV/kDOP.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index 67d068a471677bce09516f4ce4001db4f731ea60..3b922eb46017cdb62f0da7736fbb47ba9fc2f607 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index 9d42558837ce6124f961b436b13f281d0dc141b3..796ab7465cafe84b18854bfb42254b10a66a7195 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -159,7 +160,7 @@ int BVHModel<BV>::beginModel(int num_tris_, int num_vertices_)
template<typename BV>
int BVHModel<BV>::addVertex(const Vec3f& p)
{
- if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
+ if(build_state != BVH_BUILD_STATE_BEGUN)
{
std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index a72eaac268e1862024854ba2b4c8c5f6835197b5..24f40b4bd89e7c8fb222ee7f1951074629b33c55 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index e01e3643241e89494d2492fe6fa87727f2f7443c..ff92a753d42239f8e0c77886b222cc131a7a9e18 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
index da4f605e779a592d60e4d9a8de219fc372193da6..bb423eb35e21260be3d5438c4748b9c977413fbb 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/src/BVH/BV_splitter.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/articulated_model/joint.cpp b/src/articulated_model/joint.cpp
index b42097c2b2980c5c188d946063986b1f643d2b75..645312307998aaef8b38421e661eaa4323d1ffc4 100644
--- a/src/articulated_model/joint.cpp
+++ b/src/articulated_model/joint.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/articulated_model/joint_config.cpp b/src/articulated_model/joint_config.cpp
index 5f6c807e9415f924cb804d3c9836147d8d4c6eef..9a65ff65d88caa857495f98f067a822fad8cfe69 100644
--- a/src/articulated_model/joint_config.cpp
+++ b/src/articulated_model/joint_config.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/articulated_model/link.cpp b/src/articulated_model/link.cpp
index a227f8c5b10a87e7d8706dcc65f727067ddbcbf4..c8c68df2664aa8325908d7044fdf3c52d5e90385 100644
--- a/src/articulated_model/link.cpp
+++ b/src/articulated_model/link.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/articulated_model/model.cpp b/src/articulated_model/model.cpp
index fd45147a8be549d79ad9187fec6846d2e6174335..d95cc95c0ad71fab4c36138dfd08853adaabe187 100644
--- a/src/articulated_model/model.cpp
+++ b/src/articulated_model/model.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/articulated_model/model_config.cpp b/src/articulated_model/model_config.cpp
index dcd151954df9e260ec52b5de86b7902886263f7e..31af9628847517efb975863d27f0746ea75847fe 100644
--- a/src/articulated_model/model_config.cpp
+++ b/src/articulated_model/model_config.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp
index df1d0bd65e4e85721a099d7aaf590cdbb5965de0..bd420c377643da5feae984ee4feea89791531653 100644
--- a/src/broadphase/broadphase_SSaP.cpp
+++ b/src/broadphase/broadphase_SSaP.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp
index 81cb7f86ebdf432cca5022c5916c5ab19443606d..34667b00a6a8317873d76c9fdcd6eca70f1bcbb2 100644
--- a/src/broadphase/broadphase_SaP.cpp
+++ b/src/broadphase/broadphase_SaP.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -89,6 +90,8 @@ void SaPCollisionManager::unregisterObject(CollisionObject* obj)
void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
{
+ if(other_objs.empty()) return;
+
if(size() > 0)
BroadPhaseCollisionManager::registerObjects(other_objs);
else
@@ -272,6 +275,8 @@ void SaPCollisionManager::registerObject(CollisionObject* obj)
void SaPCollisionManager::setup()
{
+ if(size() == 0) return;
+
FCL_REAL scale[3];
scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);;
diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp
index b68508e99abcc982ac199c40667789ac090a69eb..e6e41fc8c32abf7d68ef5dc3fe4a755dff74bf0c 100644
--- a/src/broadphase/broadphase_bruteforce.cpp
+++ b/src/broadphase/broadphase_bruteforce.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
index 69dc7d798e459f3814b49d3395c24997d650163a..1b518589be8058cda61ee7e12e027678afca2282 100644
--- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp
+++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -634,6 +635,8 @@ bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
{
+ if(other_objs.empty()) return;
+
if(size() > 0)
{
BroadPhaseCollisionManager::registerObjects(other_objs);
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
index c8364517058f98e750fda35fabb9c8f696067c04..6f3be3724a861a649a365a505e01da17b76a6433 100644
--- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
+++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -657,6 +658,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nod
void DynamicAABBTreeCollisionManager_Array::registerObjects(const std::vector<CollisionObject*>& other_objs)
{
+ if(other_objs.empty()) return;
+
if(size() > 0)
{
BroadPhaseCollisionManager::registerObjects(other_objs);
diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp
index 219086bc86dd1d35a35a61efd64c3d21ff658919..92a3651875fd20847b21b4f7f7afe30c66d1ef55 100644
--- a/src/broadphase/broadphase_interval_tree.cpp
+++ b/src/broadphase/broadphase_interval_tree.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/broadphase/broadphase_spatialhash.cpp b/src/broadphase/broadphase_spatialhash.cpp
index e39cb3302dd0d3c04e53e06688e7498351ce030d..41cf4767c3c5bd294c1326ddc5a35cd001431a0a 100644
--- a/src/broadphase/broadphase_spatialhash.cpp
+++ b/src/broadphase/broadphase_spatialhash.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/broadphase/hierarchy_tree.cpp b/src/broadphase/hierarchy_tree.cpp
index 9c3ce96dad9140fe67b9b27761993886a8f614b8..76df94d19d70c3ec25c5886971f1ef0a22c3a375 100644
--- a/src/broadphase/hierarchy_tree.cpp
+++ b/src/broadphase/hierarchy_tree.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/broadphase/interval_tree.cpp b/src/broadphase/interval_tree.cpp
index 1585b0145801b9cbefdeb613bd811df8b767b9db..361e2cb504f3353c23e2a3d191196291b28e488f 100644
--- a/src/broadphase/interval_tree.cpp
+++ b/src/broadphase/interval_tree.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/conservative_advancement.cpp b/src/ccd/conservative_advancement.cpp
index 4cf9e39e84d362276585af0811f4fe4463310ea1..c83f3de38639023f323bd1a3b09dfcd66ce45542 100644
--- a/src/ccd/conservative_advancement.cpp
+++ b/src/ccd/conservative_advancement.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -724,6 +725,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Box, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Box, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeConservativeAdvancement<Box, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Box, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeConservativeAdvancement<Sphere, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Sphere, Sphere, NarrowPhaseSolver>;
@@ -732,6 +734,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Sphere, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Sphere, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeConservativeAdvancement<Sphere, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Sphere, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeConservativeAdvancement<Capsule, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Capsule, Sphere, NarrowPhaseSolver>;
@@ -740,6 +743,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Capsule, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Capsule, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeConservativeAdvancement<Capsule, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Capsule, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONE][GEOM_BOX] = &ShapeConservativeAdvancement<Cone, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cone, Sphere, NarrowPhaseSolver>;
@@ -748,6 +752,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cone, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cone, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeConservativeAdvancement<Cone, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Cone, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeConservativeAdvancement<Cylinder, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cylinder, Sphere, NarrowPhaseSolver>;
@@ -756,6 +761,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cylinder, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cylinder, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeConservativeAdvancement<Cylinder, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Cylinder, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeConservativeAdvancement<Convex, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeConservativeAdvancement<Convex, Sphere, NarrowPhaseSolver>;
@@ -764,6 +770,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Convex, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Convex, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeConservativeAdvancement<Convex, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Convex, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeConservativeAdvancement<Plane, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Plane, Sphere, NarrowPhaseSolver>;
@@ -772,6 +779,16 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Plane, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Plane, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeConservativeAdvancement<Plane, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Plane, Halfspace, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeConservativeAdvancement<Halfspace, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Halfspace, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Halfspace, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeConservativeAdvancement<Halfspace, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Halfspace, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Halfspace, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeConservativeAdvancement<Halfspace, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Halfspace, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_AABB][GEOM_BOX] = &BVHShapeConservativeAdvancement<AABB, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<AABB, Sphere, NarrowPhaseSolver>;
@@ -780,6 +797,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<AABB, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<AABB, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<AABB, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<AABB, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBB][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBB, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBB, Sphere, NarrowPhaseSolver>;
@@ -788,6 +806,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBB, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBB, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBB, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<OBB, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBBRSS, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBBRSS, Sphere, NarrowPhaseSolver>;
@@ -796,6 +815,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBBRSS, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBBRSS, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBBRSS, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<OBBRSS, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_RSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<RSS, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<RSS, Sphere, NarrowPhaseSolver>;
@@ -804,6 +824,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<RSS, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<RSS, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<RSS, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<RSS, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<16>, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<16>, Sphere, NarrowPhaseSolver>;
@@ -812,6 +833,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<16>, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<16>, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<16>, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<16>, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<18>, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<18>, Sphere, NarrowPhaseSolver>;
@@ -820,6 +842,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<18>, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<18>, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<18>, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<18>, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<24>, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<24>, Sphere, NarrowPhaseSolver>;
@@ -828,6 +851,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<24>, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<24>, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<24>, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<24>, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeConservativeAdvancement<kIOS, Box, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<kIOS, Sphere, NarrowPhaseSolver>;
@@ -836,6 +860,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<kIOS, Cylinder, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<kIOS, Convex, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<kIOS, Plane, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<kIOS, Halfspace, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][BV_AABB] = &ShapeBVHConservativeAdvancement<Box, AABB, NarrowPhaseSolver>;
@@ -845,6 +870,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][BV_AABB] = &ShapeBVHConservativeAdvancement<Cylinder, AABB, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][BV_AABB] = &ShapeBVHConservativeAdvancement<Convex, AABB, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][BV_AABB] = &ShapeBVHConservativeAdvancement<Plane, AABB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][BV_AABB] = &ShapeBVHConservativeAdvancement<Halfspace, AABB, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][BV_OBB] = &ShapeBVHConservativeAdvancement<Box, OBB, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][BV_OBB] = &ShapeBVHConservativeAdvancement<Sphere, OBB, NarrowPhaseSolver>;
@@ -853,6 +879,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][BV_OBB] = &ShapeBVHConservativeAdvancement<Cylinder, OBB, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][BV_OBB] = &ShapeBVHConservativeAdvancement<Convex, OBB, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][BV_OBB] = &ShapeBVHConservativeAdvancement<Plane, OBB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBB] = &ShapeBVHConservativeAdvancement<Halfspace, OBB, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][BV_RSS] = &ShapeBVHConservativeAdvancement<Box, RSS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][BV_RSS] = &ShapeBVHConservativeAdvancement<Sphere, RSS, NarrowPhaseSolver>;
@@ -861,6 +888,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][BV_RSS] = &ShapeBVHConservativeAdvancement<Cylinder, RSS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][BV_RSS] = &ShapeBVHConservativeAdvancement<Convex, RSS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][BV_RSS] = &ShapeBVHConservativeAdvancement<Plane, RSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][BV_RSS] = &ShapeBVHConservativeAdvancement<Halfspace, RSS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Box, OBBRSS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Sphere, OBBRSS, NarrowPhaseSolver>;
@@ -869,6 +897,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Cylinder, OBBRSS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Convex, OBBRSS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Plane, OBBRSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Halfspace, OBBRSS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Box, KDOP<16>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<16>, NarrowPhaseSolver>;
@@ -877,6 +906,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<16>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Convex, KDOP<16>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Plane, KDOP<16>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<16>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Box, KDOP<18>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<18>, NarrowPhaseSolver>;
@@ -885,6 +915,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<18>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Convex, KDOP<18>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Plane, KDOP<18>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<18>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Box, KDOP<24>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<24>, NarrowPhaseSolver>;
@@ -893,6 +924,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<24>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Convex, KDOP<24>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Plane, KDOP<24>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<24>, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_BOX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Box, kIOS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_SPHERE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Sphere, kIOS, NarrowPhaseSolver>;
@@ -901,6 +933,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
conservative_advancement_matrix[GEOM_CYLINDER][BV_kIOS] = &ShapeBVHConservativeAdvancement<Cylinder, kIOS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_CONVEX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Convex, kIOS, NarrowPhaseSolver>;
conservative_advancement_matrix[GEOM_PLANE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Plane, kIOS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_HALFSPACE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Halfspace, kIOS, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_AABB][BV_AABB] = &BVHConservativeAdvancement<AABB, NarrowPhaseSolver>;
conservative_advancement_matrix[BV_OBB][BV_OBB] = &BVHConservativeAdvancement<OBB, NarrowPhaseSolver>;
diff --git a/src/ccd/interpolation/interpolation.cpp b/src/ccd/interpolation/interpolation.cpp
index 7639e3b3958d501a2cd2825d2a9f19a2a8262155..5f1b914be4823e27059ba99896532acf52b9fe0b 100644
--- a/src/ccd/interpolation/interpolation.cpp
+++ b/src/ccd/interpolation/interpolation.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/interpolation/interpolation_factory.cpp b/src/ccd/interpolation/interpolation_factory.cpp
index 5a7ed8dfaa7a492ce9608d48ca602da5337ef248..d758653169718b943658f9716944dc6082fea3ed 100644
--- a/src/ccd/interpolation/interpolation_factory.cpp
+++ b/src/ccd/interpolation/interpolation_factory.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/interpolation/interpolation_linear.cpp b/src/ccd/interpolation/interpolation_linear.cpp
index 4405f7831e39c5e8158068086fca0801c14867a1..45cb4b3ae3e72cccd27b9d3c71b72fefc1bb0a96 100644
--- a/src/ccd/interpolation/interpolation_linear.cpp
+++ b/src/ccd/interpolation/interpolation_linear.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/interval.cpp b/src/ccd/interval.cpp
index f86e135087f35d68fed8b84d36fac4e94c7b8cd1..ef3b72165914160c19dab64eb06b5daae58ccb54 100644
--- a/src/ccd/interval.cpp
+++ b/src/ccd/interval.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/interval_matrix.cpp b/src/ccd/interval_matrix.cpp
index c4739624d262280efb2cfb819af07833bfa8186e..4b8a346787b8387d568453084703fe9f5c6b3f4b 100644
--- a/src/ccd/interval_matrix.cpp
+++ b/src/ccd/interval_matrix.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/interval_vector.cpp b/src/ccd/interval_vector.cpp
index 0147a09b5563da9a56e2a15ac5cde75f59cae814..3e2c09bbdffa55d86593a400ea22a06f4d24d429 100644
--- a/src/ccd/interval_vector.cpp
+++ b/src/ccd/interval_vector.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/motion.cpp b/src/ccd/motion.cpp
index 016c19b65341050700eee487516de381a48dab18..a08f24672ea684f831d25795638221040765ebb0 100644
--- a/src/ccd/motion.cpp
+++ b/src/ccd/motion.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/taylor_matrix.cpp b/src/ccd/taylor_matrix.cpp
index 0a9a824ab739b5890378466e9b78a120922a264a..b06a64efd37da8d448f57d95adb67dacc39f48d8 100644
--- a/src/ccd/taylor_matrix.cpp
+++ b/src/ccd/taylor_matrix.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/taylor_model.cpp b/src/ccd/taylor_model.cpp
index 2c033cd8d6f5518a51efbff1c96a2501c0e2e328..c08b39c10eada7de21b540773950e50a4b96655e 100644
--- a/src/ccd/taylor_model.cpp
+++ b/src/ccd/taylor_model.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/ccd/taylor_vector.cpp b/src/ccd/taylor_vector.cpp
index 3b12a717af2e3ce78288d55f9fb721727da60cec..340901791eef5f1e53f280a37dcbb6334501fc6b 100644
--- a/src/ccd/taylor_vector.cpp
+++ b/src/ccd/taylor_vector.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/collision.cpp b/src/collision.cpp
index d6119d2e55c19e1bb7951a316f55b906812cb137..b3569e4dc92d4705cb9bca9733badd3a591a4473 100644
--- a/src/collision.cpp
+++ b/src/collision.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/collision_data.cpp b/src/collision_data.cpp
index 1d7a0bc1cc1ddbbe0f916b56ae5019ed12c131ca..2ba31b422465f8838dab39c2b18892091cab5809 100644
--- a/src/collision_data.cpp
+++ b/src/collision_data.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 0b3c2db99c551b472410e1cdfc895cf3771b8208..32ce18e7bbea2872e39a340fa84b012798ad91c1 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -459,6 +460,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide<Box, Halfspace, NarrowPhaseSolver>;
collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box, NarrowPhaseSolver>;
collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere, NarrowPhaseSolver>;
@@ -467,6 +469,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide<Sphere, Halfspace, NarrowPhaseSolver>;
collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box, NarrowPhaseSolver>;
collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere, NarrowPhaseSolver>;
@@ -475,6 +478,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide<Capsule, Halfspace, NarrowPhaseSolver>;
collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box, NarrowPhaseSolver>;
collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere, NarrowPhaseSolver>;
@@ -483,6 +487,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide<Cone, Halfspace, NarrowPhaseSolver>;
collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box, NarrowPhaseSolver>;
collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere, NarrowPhaseSolver>;
@@ -491,6 +496,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide<Cylinder, Halfspace, NarrowPhaseSolver>;
collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box, NarrowPhaseSolver>;
collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere, NarrowPhaseSolver>;
@@ -499,6 +505,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex, Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex, Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide<Convex, Halfspace, NarrowPhaseSolver>;
collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box, NarrowPhaseSolver>;
collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere, NarrowPhaseSolver>;
@@ -507,6 +514,16 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide<Plane, Halfspace, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide<Halfspace, Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide<Halfspace, Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide<Halfspace, Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide<Halfspace, Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide<Halfspace, Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide<Halfspace, Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide<Halfspace, Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide<Halfspace, Halfspace, NarrowPhaseSolver>;
collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box, NarrowPhaseSolver>::collide;
collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere, NarrowPhaseSolver>::collide;
@@ -515,6 +532,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder, NarrowPhaseSolver>::collide;
collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, Convex, NarrowPhaseSolver>::collide;
collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider<AABB, Halfspace, NarrowPhaseSolver>::collide;
collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box, NarrowPhaseSolver>::collide;
collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere, NarrowPhaseSolver>::collide;
@@ -523,6 +541,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder, NarrowPhaseSolver>::collide;
collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, Convex, NarrowPhaseSolver>::collide;
collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider<OBB, Halfspace, NarrowPhaseSolver>::collide;
collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box, NarrowPhaseSolver>::collide;
collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere, NarrowPhaseSolver>::collide;
@@ -531,6 +550,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder, NarrowPhaseSolver>::collide;
collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, Convex, NarrowPhaseSolver>::collide;
collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider<RSS, Halfspace, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere, NarrowPhaseSolver>::collide;
@@ -539,6 +559,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, Convex, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<16>, Halfspace, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere, NarrowPhaseSolver>::collide;
@@ -547,6 +568,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, Convex, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<18>, Halfspace, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere, NarrowPhaseSolver>::collide;
@@ -555,6 +577,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex, NarrowPhaseSolver>::collide;
collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<24>, Halfspace, NarrowPhaseSolver>::collide;
collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box, NarrowPhaseSolver>::collide;
collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere, NarrowPhaseSolver>::collide;
@@ -563,6 +586,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder, NarrowPhaseSolver>::collide;
collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, Convex, NarrowPhaseSolver>::collide;
collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider<kIOS, Halfspace, NarrowPhaseSolver>::collide;
collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box, NarrowPhaseSolver>::collide;
collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere, NarrowPhaseSolver>::collide;
@@ -571,6 +595,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder, NarrowPhaseSolver>::collide;
collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS, Convex, NarrowPhaseSolver>::collide;
collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS, Plane, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider<OBBRSS, Halfspace, NarrowPhaseSolver>::collide;
collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB, NarrowPhaseSolver>;
collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB, NarrowPhaseSolver>;
@@ -589,6 +614,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace, NarrowPhaseSolver>;
collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box, NarrowPhaseSolver>;
collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere, NarrowPhaseSolver>;
@@ -597,6 +623,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder, NarrowPhaseSolver>;
collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<Convex, NarrowPhaseSolver>;
collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane, NarrowPhaseSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace, NarrowPhaseSolver>;
collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide<NarrowPhaseSolver>;
diff --git a/src/collision_node.cpp b/src/collision_node.cpp
index 3a83d1a301a409c120a460065bc6600b9a4d068a..3d7f0c38b2958a24477d8cf99df2c0addb44aa12 100644
--- a/src/collision_node.cpp
+++ b/src/collision_node.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/distance.cpp b/src/distance.cpp
index a9936b858c00307bf3a72bfdc6ff8e1613d1bc2f..7fc6c96dbc75037cd12d2f567293e1ac70534226 100644
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index fca95def495f453d25090e4d85465ea1eb38b439..a83c114a3d31240669ac6d6616cf16f951ae4f5f 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -301,6 +302,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance<Box, Halfspace, NarrowPhaseSolver>;
distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box, NarrowPhaseSolver>;
distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere, Sphere, NarrowPhaseSolver>;
@@ -309,6 +311,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere, Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere, Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance<Sphere, Halfspace, NarrowPhaseSolver>;
distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box, NarrowPhaseSolver>;
distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule, Sphere, NarrowPhaseSolver>;
@@ -317,6 +320,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule, Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule, Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule, Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance<Capsule, Halfspace, NarrowPhaseSolver>;
distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box, NarrowPhaseSolver>;
distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere, NarrowPhaseSolver>;
@@ -325,6 +329,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone, Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone, Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance<Cone, Halfspace, NarrowPhaseSolver>;
distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box, NarrowPhaseSolver>;
distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder, Sphere, NarrowPhaseSolver>;
@@ -333,6 +338,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder, Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder, Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder, Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance<Cylinder, Halfspace, NarrowPhaseSolver>;
distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex, Box, NarrowPhaseSolver>;
distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex, Sphere, NarrowPhaseSolver>;
@@ -341,6 +347,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex, Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<Convex, Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<Convex, Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance<Convex, Halfspace, NarrowPhaseSolver>;
distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box, NarrowPhaseSolver>;
distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere, NarrowPhaseSolver>;
@@ -349,6 +356,16 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane, Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane, Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance<Plane, Halfspace, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance<Halfspace, Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance<Halfspace, Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance<Halfspace, Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance<Halfspace, Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance<Halfspace, Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance<Halfspace, Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance<Halfspace, Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance<Halfspace, Halfspace, NarrowPhaseSolver>;
/* AABB distance not implemented */
/*
@@ -359,6 +376,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB, Cylinder, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
@@ -367,6 +385,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB, Cylinder, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, NarrowPhaseSolver>::distance;
*/
distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
@@ -376,6 +395,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS, Cylinder, NarrowPhaseSolver>::distance;
distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS, Plane, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer<RSS, Halfspace, NarrowPhaseSolver>::distance;
/* KDOP distance not implemented */
/*
@@ -386,6 +406,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>, Cylinder, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<16>, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>, Plane, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<16>, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<18>, Sphere, NarrowPhaseSolver>::distance;
@@ -394,6 +415,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>, Cylinder, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<18>, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>, Plane, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<18>, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<24>, Sphere, NarrowPhaseSolver>::distance;
@@ -402,6 +424,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>, Cylinder, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<24>, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>, Plane, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<24>, Halfspace, NarrowPhaseSolver>::distance;
*/
distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box, NarrowPhaseSolver>::distance;
@@ -411,6 +434,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS, Cylinder, NarrowPhaseSolver>::distance;
distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS, Plane, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer<kIOS, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS, Sphere, NarrowPhaseSolver>::distance;
@@ -419,6 +443,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS, Cylinder, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS, Plane, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
@@ -433,6 +458,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance<Halfspace, NarrowPhaseSolver>;
distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box, NarrowPhaseSolver>;
distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere, NarrowPhaseSolver>;
@@ -441,6 +467,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder, NarrowPhaseSolver>;
distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane, NarrowPhaseSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance<Halfspace, NarrowPhaseSolver>;
distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance<NarrowPhaseSolver>;
diff --git a/src/intersect.cpp b/src/intersect.cpp
index 791d020926ed6cc05d59e88d6fa864fe8c46b17d..66bd4a8ffbe8f8c03ce22eaa9bea88e5718382d8 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/math/transform.cpp b/src/math/transform.cpp
index 408f5df1091296f646a512e2d43632656affc203..a2c20b36e7075883ade9ae15b33d5cb9d83a5372 100644
--- a/src/math/transform.cpp
+++ b/src/math/transform.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 8be837f1c145cfc672d04a6da479e2b831a0c7a8..c880df8517c41005a57e5788d57ba94c7999efe0 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/narrowphase/gjk_libccd.cpp b/src/narrowphase/gjk_libccd.cpp
index 145e3a5b168d61e074957b4a32431ea45a1c754a..5f19800c28875dbea52ba1d5612e10b6d276dc4b 100644
--- a/src/narrowphase/gjk_libccd.cpp
+++ b/src/narrowphase/gjk_libccd.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -775,13 +776,13 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
}
- /// libccd returns dir and pos in world space and dir is pointing from object 2 to object 1
+ /// libccd returns dir and pos in world space and dir is pointing from object 1 to object 2
res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
if(res == 0)
{
contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
*penetration_depth = depth;
- normal->setValue(-ccdVec3X(&dir), -ccdVec3Y(&dir), -ccdVec3Z(&dir));
+ normal->setValue(ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir));
return true;
}
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index aaaf5c5f06fc84ff860d74a13e9d744d51c8ea8f..b295601f8a0025ec32043d2f168fa61f09a2bf69 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -269,13 +270,16 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
- Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
+ Vec3f diff = tf2.transform(Vec3f()) - tf1.transform(Vec3f());
FCL_REAL len = diff.length();
if(len > s1.radius + s2.radius)
return false;
if(penetration_depth)
*penetration_depth = s1.radius + s2.radius - len;
+
+ // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
+ // Otherwise, normal is pointing from center of object 1 to center of object 2
if(normal)
{
if(len > 0)
@@ -285,7 +289,7 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
}
if(contact_points)
- *contact_points = tf1.transform(Vec3f()) - diff * s1.radius / (s1.radius + s2.radius);
+ *contact_points = tf1.transform(Vec3f()) + diff * s1.radius / (s1.radius + s2.radius);
return true;
}
@@ -423,7 +427,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
if(has_contact)
{
- Vec3f contact_to_center = center - contact_point;
+ Vec3f contact_to_center = contact_point - center;
FCL_REAL distance_sqr = contact_to_center.sqrLength();
if(distance_sqr < radius_with_threshold * radius_with_threshold)
@@ -437,7 +441,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
}
else
{
- if(normal_) *normal_ = normal;
+ if(normal_) *normal_ = -normal;
if(contact_points) *contact_points = contact_point;
if(penetration_depth) *penetration_depth = -radius;
}
diff --git a/src/profile.cpp b/src/profile.cpp
index 12a8f274c075ac8976427ff88ea4f94ec5ab1479..f25a8894f4ae66486c339537eff684daed93cd18 100644
--- a/src/profile.cpp
+++ b/src/profile.cpp
@@ -1,7 +1,8 @@
-/*********************************************************************
+/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2008-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -30,7 +31,7 @@
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
+ */
/** \author Ioan Sucan */
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index ce537dfccc5e1eb45c71cf8d06dddd2b9e8055b5..5824f7c9eae8167d5c78c37847f5b6fa19a6aae3 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/src/shape/geometric_shapes.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 6c25ab5e509ab808bb6285d90a5b27eed8c9f8c7..4c377294558fda00eb4ff7f4f51afc24c0d108ff 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/traversal/traversal_node_base.cpp b/src/traversal/traversal_node_base.cpp
index f228395d072e6dbcad3eba4e8dc254ea177e4a69..0ba6c2e084ae09dc8aeb30bc7b01c1507b60166d 100644
--- a/src/traversal/traversal_node_base.cpp
+++ b/src/traversal/traversal_node_base.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index 692d38f75a55fa325fe2404ab2711b95816f9b2c..11bb544a7ee4a15a1c7abf629d9106c66b9276e0 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index bd83d00f757a85f507aa1f43e050d83628dd4373..c3d3ff55bac9eea218fbd98e9e5698519d044a51 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/src/traversal/traversal_node_setup.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp
index ec68b330dfbf0274fd4d01945d2630a685a20bc7..24ac4c0d7189dfed70baa43f79a6c11a2fa1fa09 100644
--- a/src/traversal/traversal_recurse.cpp
+++ b/src/traversal/traversal_recurse.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index ed7e36e54032ea388c0cb71766d0fe7bb21ff5a8..d284712da58837af0e24aa1993d821d40c925b72 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -36,6 +36,7 @@ add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp)
add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp)
#add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
+add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
diff --git a/test/fcl_resources/config.h.in b/test/fcl_resources/config.h.in
index 8486bd250b3b87c205b759b0de1a77667dbccb2c..848ab0142af31cec258f7ad053bc8ec8eef0089e 100644
--- a/test/fcl_resources/config.h.in
+++ b/test/fcl_resources/config.h.in
@@ -1,36 +1,36 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2010, Rice University.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Rice University nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010-2015, Rice University.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Rice University nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
/* Author: Mark Moll */
diff --git a/test/test_fcl_broadphase.cpp b/test/test_fcl_broadphase.cpp
index 55382ade8b5a159883e0e1f63e8acbcd0d93a4f1..91d24bb4ad8d4f0768ca5242fbbcb79822ac0174 100644
--- a/test/test_fcl_broadphase.cpp
+++ b/test/test_fcl_broadphase.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -149,6 +150,26 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance)
broad_phase_self_distance_test(200, 5000);
}
+/// check broad phase collision for empty collision object set and queries
+BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty)
+{
+ broad_phase_collision_test(2000, 0, 0, 10, false, false);
+ broad_phase_collision_test(2000, 0, 1000, 10, false, false);
+ broad_phase_collision_test(2000, 100, 0, 10, false, false);
+
+ broad_phase_collision_test(2000, 0, 0, 10, false, true);
+ broad_phase_collision_test(2000, 0, 1000, 10, false, true);
+ broad_phase_collision_test(2000, 100, 0, 10, false, true);
+
+ broad_phase_collision_test(2000, 0, 0, 10, true, false);
+ broad_phase_collision_test(2000, 0, 1000, 10, true, false);
+ broad_phase_collision_test(2000, 100, 0, 10, true, false);
+
+ broad_phase_collision_test(2000, 0, 0, 10, true, true);
+ broad_phase_collision_test(2000, 0, 1000, 10, true, true);
+ broad_phase_collision_test(2000, 100, 0, 10, true, true);
+}
+
/// check broad phase collision and self collision, only return collision or not
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary)
{
diff --git a/test/test_fcl_bvh_models.cpp b/test/test_fcl_bvh_models.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..397046f5dda90ea58d2c9b7d837b280eecb0fc3a
--- /dev/null
+++ b/test/test_fcl_bvh_models.cpp
@@ -0,0 +1,224 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2015, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jeongseok Lee */
+
+
+#define BOOST_TEST_MODULE "FCL_BVH_MODELS"
+#include <boost/test/unit_test.hpp>
+
+#include "fcl/config.h"
+#include "fcl/BVH/BVH_model.h"
+#include "fcl/math/transform.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "test_fcl_utility.h"
+#include <iostream>
+
+using namespace fcl;
+
+template<typename BV>
+void testBVHModelPointCloud()
+{
+ boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+
+ if (model->getNodeType() != BV_AABB
+ && model->getNodeType() != BV_KDOP16
+ && model->getNodeType() != BV_KDOP18
+ && model->getNodeType() != BV_KDOP24)
+ {
+ std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType())
+ << "' does not support point cloud model. "
+ << "Please see issue #67." << std::endl;
+ return;
+ }
+
+ Box box;
+ double a = box.side[0];
+ double b = box.side[1];
+ double c = box.side[2];
+ std::vector<Vec3f> points(8);
+ points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
+ points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
+ points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
+ points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
+ points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
+ points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
+ points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
+ points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c);
+
+ int result;
+
+ result = model->beginModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ for (std::size_t i = 0; i < points.size(); ++i)
+ {
+ result = model->addVertex(points[i]);
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+ }
+
+ result = model->endModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ model->computeLocalAABB();
+
+ BOOST_CHECK_EQUAL(model->num_vertices, 8);
+ BOOST_CHECK_EQUAL(model->num_tris, 0);
+ BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+template<typename BV>
+void testBVHModelTriangles()
+{
+ boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+ Box box;
+
+ double a = box.side[0];
+ double b = box.side[1];
+ double c = box.side[2];
+ std::vector<Vec3f> points(8);
+ std::vector<Triangle> tri_indices(12);
+ points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
+ points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
+ points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
+ points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
+ points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
+ points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
+ points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
+ points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c);
+
+ tri_indices[0].set(0, 4, 1);
+ tri_indices[1].set(1, 4, 5);
+ tri_indices[2].set(2, 6, 3);
+ tri_indices[3].set(3, 6, 7);
+ tri_indices[4].set(3, 0, 2);
+ tri_indices[5].set(2, 0, 1);
+ tri_indices[6].set(6, 5, 7);
+ tri_indices[7].set(7, 5, 4);
+ tri_indices[8].set(1, 5, 2);
+ tri_indices[9].set(2, 5, 6);
+ tri_indices[10].set(3, 7, 0);
+ tri_indices[11].set(0, 7, 4);
+
+ int result;
+
+ result = model->beginModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ for (std::size_t i = 0; i < tri_indices.size(); ++i)
+ {
+ result = model->addTriangle(points[tri_indices[i][0]], points[tri_indices[i][1]], points[tri_indices[i][2]]);
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+ }
+
+ result = model->endModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ model->computeLocalAABB();
+
+ BOOST_CHECK_EQUAL(model->num_vertices, 12 * 3);
+ BOOST_CHECK_EQUAL(model->num_tris, 12);
+ BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+template<typename BV>
+void testBVHModelSubModel()
+{
+ boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+ Box box;
+
+ double a = box.side[0];
+ double b = box.side[1];
+ double c = box.side[2];
+ std::vector<Vec3f> points(8);
+ std::vector<Triangle> tri_indices(12);
+ points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
+ points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
+ points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
+ points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
+ points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
+ points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
+ points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
+ points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c);
+
+ tri_indices[0].set(0, 4, 1);
+ tri_indices[1].set(1, 4, 5);
+ tri_indices[2].set(2, 6, 3);
+ tri_indices[3].set(3, 6, 7);
+ tri_indices[4].set(3, 0, 2);
+ tri_indices[5].set(2, 0, 1);
+ tri_indices[6].set(6, 5, 7);
+ tri_indices[7].set(7, 5, 4);
+ tri_indices[8].set(1, 5, 2);
+ tri_indices[9].set(2, 5, 6);
+ tri_indices[10].set(3, 7, 0);
+ tri_indices[11].set(0, 7, 4);
+
+ int result;
+
+ result = model->beginModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ result = model->addSubModel(points, tri_indices);
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ result = model->endModel();
+ BOOST_CHECK_EQUAL(result, BVH_OK);
+
+ model->computeLocalAABB();
+
+ BOOST_CHECK_EQUAL(model->num_vertices, 8);
+ BOOST_CHECK_EQUAL(model->num_tris, 12);
+ BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+template<typename BV>
+void testBVHModel()
+{
+ testBVHModelTriangles<BV>();
+ testBVHModelPointCloud<BV>();
+ testBVHModelSubModel<BV>();
+}
+
+BOOST_AUTO_TEST_CASE(building_bvh_models)
+{
+ testBVHModel<AABB>();
+ testBVHModel<OBB>();
+ testBVHModel<RSS>();
+ testBVHModel<kIOS>();
+ testBVHModel<OBBRSS>();
+ testBVHModel<KDOP<16> >();
+ testBVHModel<KDOP<18> >();
+ testBVHModel<KDOP<24> >();
+}
diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp
index 655a3b936f185430ee22499c03222b6fd7d7d101..4a4ae4f01d1bc2d2753762741ce266e495f5c4d6 100644
--- a/test/test_fcl_capsule_box_1.cpp
+++ b/test/test_fcl_capsule_box_1.cpp
@@ -1,7 +1,7 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2014, CNRS-LAAS
+ * Copyright (c) 2014-2015, CNRS-LAAS and AIST
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of CNRS-LAAS nor the names of its
+ * * Neither the name of CNRS-LAAS and AIST nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_capsule_box_2.cpp b/test/test_fcl_capsule_box_2.cpp
index b69528bde51d1937cf6e6346b22455fc6be77966..273423d3c80d789be7ae3ecb678d73a38e8b8ae3 100644
--- a/test/test_fcl_capsule_box_2.cpp
+++ b/test/test_fcl_capsule_box_2.cpp
@@ -1,7 +1,7 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2014, CNRS-LAAS
+ * Copyright (c) 2014-2015, CNRS-LAAS and AIST
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of CNRS-LAAS nor the names of its
+ * * Neither the name of CNRS-LAAS and AIST nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_capsule_capsule.cpp b/test/test_fcl_capsule_capsule.cpp
index cadced0b963e81033bf6fe5bebfbd3384c09f0b4..40910184ca5207cf8fe7a6855b19d84ee121f1d3 100644
--- a/test/test_fcl_capsule_capsule.cpp
+++ b/test/test_fcl_capsule_capsule.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp
index d69b93ea71408c0de5dc785ce127a40020c5f471..a3c5d6faab03912900019a0f59d57ee36acfe701 100644
--- a/test/test_fcl_collision.cpp
+++ b/test/test_fcl_collision.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp
index 8689b8802cd0e1d795336a26df46123193bd1e53..4bfa015690b90ede5431078aded67cd74b154548 100644
--- a/test/test_fcl_distance.cpp
+++ b/test/test_fcl_distance.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_frontlist.cpp b/test/test_fcl_frontlist.cpp
index ccb461d4903bf552b4b7489d44eadaa61e444e7f..041cf6329acb5c417e5e160a86ab9c01ea499176 100644
--- a/test/test_fcl_frontlist.cpp
+++ b/test/test_fcl_frontlist.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 1126820fcb14d26ca2a860539aaf12d7ffa2ac8c..bef3ffe3a94cb26b01f024b6b17aeff22c10af10 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -64,7 +65,7 @@ BOOST_AUTO_TEST_CASE(gjkcache)
TranslationMotion motion(Transform3f(Vec3f(-20.0, -20.0, -20.0)), Transform3f(Vec3f(20.0, 20.0, 20.0)));
- int N = 1000;
+ int N = 1000;
FCL_REAL dt = 1.0 / (N - 1);
/// test exploiting spatial coherence
@@ -113,90 +114,250 @@ BOOST_AUTO_TEST_CASE(gjkcache)
}
}
-BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
+template <typename S1, typename S2>
+void printComparisonError(const std::string& comparison_type,
+ const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ GJKSolverType solver_type,
+ const Vec3f& contact_or_normal,
+ const Vec3f& expected_contact_or_normal,
+ bool check_opposite_normal,
+ FCL_REAL tol)
{
- Sphere s1(20);
- Sphere s2(10);
+ std::cout << "Disagreement between " << comparison_type
+ << " and expected_" << comparison_type << " for "
+ << getNodeTypeName(s1.getNodeType()) << " and "
+ << getNodeTypeName(s2.getNodeType()) << " with '"
+ << getGJKSolverName(solver_type) << "' solver." << std::endl
+ << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl
+ << "tf1.translation: " << tf1.getTranslation() << std::endl
+ << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl
+ << "tf2.translation: " << tf2.getTranslation() << std::endl
+ << comparison_type << ": " << contact_or_normal << std::endl
+ << "expected_" << comparison_type << ": " << expected_contact_or_normal;
+
+ if (check_opposite_normal)
+ std::cout << " or " << -expected_contact_or_normal;
+
+ std::cout << std::endl
+ << "difference: " << (contact_or_normal - expected_contact_or_normal).norm() << std::endl
+ << "tolerance: " << tol << std::endl;
+}
- Transform3f transform;
- generateRandomTransform(extents, transform);
- Transform3f identity;
+template <typename S1, typename S2>
+void printComparisonError(const std::string& comparison_type,
+ const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ GJKSolverType solver_type,
+ FCL_REAL depth,
+ FCL_REAL expected_depth,
+ FCL_REAL tol)
+{
+ std::cout << "Disagreement between " << comparison_type
+ << " and expected_" << comparison_type << " for "
+ << getNodeTypeName(s1.getNodeType()) << " and "
+ << getNodeTypeName(s2.getNodeType()) << " with '"
+ << getGJKSolverName(solver_type) << "' solver." << std::endl
+ << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl
+ << "tf1.translation: " << tf1.getTranslation() << std::endl
+ << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl
+ << "tf2.translation: " << tf2.getTranslation() << std::endl
+ << "depth: " << depth << std::endl
+ << "expected_depth: " << expected_depth << std::endl
+ << "difference: " << std::fabs(depth - expected_depth) << std::endl
+ << "tolerance: " << tol << std::endl;
+}
- CollisionRequest request;
- CollisionResult result;
- bool res;
+template <typename S1, typename S2>
+void compareContact(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ GJKSolverType solver_type,
+ const Vec3f& contact, Vec3f* expected_point,
+ FCL_REAL depth, FCL_REAL* expected_depth,
+ const Vec3f& normal, Vec3f* expected_normal, bool check_opposite_normal,
+ FCL_REAL tol)
+{
+ if (expected_point)
+ {
+ bool contact_equal = contact.equal(*expected_point, tol);
+ BOOST_CHECK(contact_equal);
+ if (!contact_equal)
+ printComparisonError("contact", s1, tf1, s2, tf2, solver_type, contact, *expected_point, false, tol);
+ }
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ if (expected_depth)
+ {
+ bool depth_equal = std::fabs(depth - *expected_depth) < tol;
+ BOOST_CHECK(depth_equal);
+ if (!depth_equal)
+ printComparisonError("depth", s1, tf1, s2, tf2, solver_type, depth, *expected_depth, tol);
+ }
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ if (expected_normal)
+ {
+ bool normal_equal = normal.equal(*expected_normal, tol);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ if (!normal_equal && check_opposite_normal)
+ normal_equal = normal.equal(-(*expected_normal), tol);
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(normal_equal);
+ if (!normal_equal)
+ printComparisonError("normal", s1, tf1, s2, tf2, solver_type, normal, *expected_normal, check_opposite_normal, tol);
+ }
+}
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+template <typename S1, typename S2>
+void testShapeInersection(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ GJKSolverType solver_type,
+ bool expected_res,
+ Vec3f* expected_point = NULL,
+ FCL_REAL* expected_depth = NULL,
+ Vec3f* expected_normal = NULL,
+ bool check_opposite_normal = false,
+ FCL_REAL tol = 1e-9)
+{
+ CollisionRequest request;
+ request.gjk_solver_type = solver_type;
+ CollisionResult result;
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ Vec3f contact;
+ FCL_REAL depth;
+ Vec3f normal; // normal direction should be from object 1 to object 2
+ bool res;
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
- BOOST_CHECK(res);
+ if (solver_type == GST_LIBCCD)
+ {
+ res = solver1.shapeIntersect(s1, tf1, s2, tf2, NULL, NULL, NULL);
+ }
+ else if (solver_type == GST_INDEP)
+ {
+ res = solver2.shapeIntersect(s1, tf1, s2, tf2, NULL, NULL, NULL);
+ }
+ else
+ {
+ std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
+ return;
+ }
+ BOOST_CHECK_EQUAL(res, expected_res);
- res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform, request, result) > 0);
- BOOST_CHECK(res);
+ if (solver_type == GST_LIBCCD)
+ {
+ res = solver1.shapeIntersect(s1, tf1, s2, tf2, &contact, &depth, &normal);
+ }
+ else if (solver_type == GST_INDEP)
+ {
+ res = solver2.shapeIntersect(s1, tf1, s2, tf2, &contact, &depth, &normal);
+ }
+ else
+ {
+ std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
+ return;
+ }
+ BOOST_CHECK_EQUAL(res, expected_res);
+ if (expected_res)
+ compareContact(s1, tf1, s2, tf2, solver_type, contact, expected_point, depth, expected_depth, normal, expected_normal, check_opposite_normal, tol);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
+ request.enable_contact = false;
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
+ BOOST_CHECK_EQUAL(res, expected_res);
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
+ request.enable_contact = true;
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
+ BOOST_CHECK_EQUAL(res, expected_res);
+ if (expected_res)
+ {
+ BOOST_CHECK_EQUAL(result.numContacts(), 1);
+ if (result.numContacts() == 1)
+ {
+ Contact contact = result.getContact(0);
+ compareContact(s1, tf1, s2, tf2, solver_type, contact.pos, expected_point, contact.penetration_depth, expected_depth, contact.normal, expected_normal, check_opposite_normal, tol);
+ }
+ }
+}
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
+{
+ Sphere s1(20);
+ Sphere s2(10);
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ Transform3f tf1;
+ Transform3f tf2;
+
+ Transform3f transform;
+ generateRandomTransform(extents, transform);
+
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(40, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(30, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(30.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(29.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-29.9, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-30.0, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-30.01, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}
bool compareContactPoints(const Vec3f& c1,const Vec3f& c2)
@@ -254,52 +415,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
Box s1(20, 40, 50);
Box s2(10, 10, 10);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Transform3f identity;
-
- CollisionRequest request;
- CollisionResult result;
-
- bool res;
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform, request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(15, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(15.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
Quaternion3f q;
q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(q), request, result) > 0);
- BOOST_CHECK(res);
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(q), request, result) > 0);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(15, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(15.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(q);
+ normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(q);
+ normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
FCL_UINT32 numTests = 1e+2;
for (FCL_UINT32 i = 0; i < numTests; ++i)
@@ -315,51 +473,101 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
Sphere s1(20);
Box s2(5, 5, 5);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Transform3f identity;
-
- CollisionRequest request;
- CollisionResult result;
- bool res;
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
- BOOST_CHECK(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform, request, result) > 0);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0).
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-0.9985590945508502, 0.02998909000838618, -0.04450156368325561).
+ normal.setValue(-0.9985590945508502, 0.02998909000838618, -0.04450156368325561);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(22.5, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(22.501, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(22.4, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+}
+BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule)
+{
+ Sphere s1(20);
+ Capsule s2(5, 10);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.5, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ Transform3f tf1;
+ Transform3f tf2;
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.501, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.501, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ Transform3f transform;
+ generateRandomTransform(extents, transform);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.4, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.4, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(24.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(25, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(25, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(25.1, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder)
@@ -367,50 +575,43 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder)
Cylinder s1(5, 10);
Cylinder s2(5, 10);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Transform3f identity;
-
- CollisionRequest request;
- CollisionResult result;
-
- bool res;
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform, request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(10.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_conecone)
@@ -418,62 +619,53 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone)
Cone s1(5, 10);
Cone s2(5, 10);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Transform3f identity;
-
- CollisionRequest request;
- CollisionResult result;
-
- bool res;
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform, request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10.001, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.001, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
- BOOST_CHECK(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(10.001, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(10.001, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 9.9));
+ normal.setValue(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
+ normal = transform.getRotation() * Vec3f(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder)
@@ -481,74 +673,61 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder)
Cylinder s1(5, 10);
Cone s2(5, 10);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Transform3f identity;
-
- CollisionRequest request;
- CollisionResult result;
-
- bool res;
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform, request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
- BOOST_CHECK(res);
-
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
- BOOST_CHECK(res);
- res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 10)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 10.01)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.061);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.46);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(10.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 9.9));
+ normal.setValue(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
+ normal = transform.getRotation() * Vec3f(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 10.01));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 10.01));
+ testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
@@ -561,8 +740,8 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
Transform3f transform;
generateRandomTransform(extents, transform);
- Transform3f identity;
+ Vec3f normal;
bool res;
res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
@@ -580,273 +759,412 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
-}
-BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
+ res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal);
+ BOOST_CHECK(res);
+ BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+
+ res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
+ BOOST_CHECK(res);
+ BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+}
+
+BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle)
{
- Sphere s(10);
Halfspace hs(Vec3f(1, 0, 0), 0);
+ Vec3f t[3];
+ t[0].setValue(20, 0, 0);
+ t[1].setValue(-20, 0, 0);
+ t[2].setValue(0, 20, 0);
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
- FCL_REAL depth;
+ // Vec3f point;
+ // FCL_REAL depth;
Vec3f normal;
bool res;
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-5, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-5, 0, 0))));
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 15) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 15) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
+ t[0].setValue(20, 0, 0);
+ t[1].setValue(0, -20, 0);
+ t[2].setValue(0, 20, 0);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-7.5, 0, 0)));
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-7.5, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
+ BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
+ BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
-BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere)
+BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle)
{
- Sphere s(10);
Plane hs(Vec3f(1, 0, 0), 0);
+ Vec3f t[3];
+ t[0].setValue(20, 0, 0);
+ t[1].setValue(-20, 0, 0);
+ t[2].setValue(0, 20, 0);
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
- FCL_REAL depth;
+ // Vec3f point;
+ // FCL_REAL depth;
Vec3f normal;
bool res;
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(5, 0, 0)));
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
+ t[0].setValue(20, 0, 0);
+ t[1].setValue(-0.1, -20, 0);
+ t[2].setValue(-0.1, 20, 0);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(5, 0, 0))));
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-5, 0, 0)));
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-5, 0, 0))));
-
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
+ BOOST_CHECK(res);
+ BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
-BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
+BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
{
- Box s(5, 10, 20);
+ Sphere s(10);
Halfspace hs(Vec3f(1, 0, 0), 0);
-
+
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
- bool res;
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(-5, 0, 0);
+ depth = 10;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(-5, 0, 0));
+ depth = 10;
+ normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(5, 0, 0));
+ contact.setValue(-2.5, 0, 0);
+ depth = 15;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(5, 0, 0));
+ contact = transform.transform(Vec3f(-2.5, 0, 0));
+ depth = 15;
+ normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-5, 0, 0));
+ contact.setValue(-7.5, 0, 0);
+ depth = 5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
+ contact = transform.transform(Vec3f(-7.5, 0, 0));
+ depth = 5;
+ normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-10.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(10.1, 0, 0));
+ contact.setValue(0.05, 0, 0);
+ depth = 20.1;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
+ contact = transform.transform(Vec3f(0.05, 0, 0));
+ depth = 20.1;
+ normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+}
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
+BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere)
+{
+ Sphere s(10);
+ Plane hs(Vec3f(1, 0, 0), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 3.75) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-0.625, 0, 0)));
+ Transform3f tf1;
+ Transform3f tf2;
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 3.75) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-0.625, 0, 0))));
+ Transform3f transform;
+ generateRandomTransform(extents, transform);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-1.875, 0, 0)));
+ Vec3f contact;
+ FCL_REAL depth;
+ Vec3f normal;
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.875, 0, 0))));
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setZero();
+ depth = 10;
+ normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, 0));
+ depth = 10;
+ normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(5, 0, 0));
+ contact.setValue(5, 0, 0);
+ depth = 5;
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(5, 0, 0));
+ contact = transform.transform(Vec3f(5, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-5, 0, 0));
+ contact.setValue(-5, 0, 0);
+ depth = 5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
+ contact = transform.transform(Vec3f(-5, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-10.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(10.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+}
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5.01) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0.005, 0, 0)));
+BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
+{
+ Box s(5, 10, 20);
+ Halfspace hs(Vec3f(1, 0, 0), 0);
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5.01) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.005, 0, 0))));
+ Transform3f tf1;
+ Transform3f tf2;
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ Transform3f transform;
+ generateRandomTransform(extents, transform);
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ Vec3f contact;
+ FCL_REAL depth;
+ Vec3f normal;
- res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(-1.25, 0, 0);
+ depth = 2.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(-1.25, 0, 0));
+ depth = 2.5;
+ normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(1.25, 0, 0));
+ contact.setValue(-0.625, 0, 0);
+ depth = 3.75;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+ contact = transform.transform(Vec3f(-0.625, 0, 0));
+ depth = 3.75;
+ normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+ contact.setValue(-1.875, 0, 0);
+ depth = 1.25;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+ contact = transform.transform(Vec3f(-1.875, 0, 0));
+ depth = 1.25;
+ normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(2.51, 0, 0));
+ contact.setValue(0.005, 0, 0);
+ depth = 5.01;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+ contact = transform.transform(Vec3f(0.005, 0, 0));
+ depth = 5.01;
+ normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f(transform.getQuatRotation());
+ tf2 = Transform3f();
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
{
Box s(5, 10, 20);
Plane hs(Vec3f(1, 0, 0), 0);
-
+
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
- bool res;
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(1.25, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(1.25, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 2.5;
+ normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, 0));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(1.25, 0, 0));
+ contact.setValue(1.25, 0, 0);
+ depth = 1.25;
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+ contact.setValue(-1.25, 0, 0);
+ depth = 1.25;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(2.51, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f(transform.getQuatRotation());
+ tf2 = Transform3f();
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
@@ -854,185 +1172,217 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
Capsule s(5, 10);
Halfspace hs(Vec3f(1, 0, 0), 0);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
- bool res;
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(-2.5, 0, 0);
+ depth = 5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(-2.5, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ contact.setValue(-1.25, 0, 0);
+ depth = 7.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+ contact = transform.transform(Vec3f(-1.25, 0, 0));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ contact.setValue(-3.75, 0, 0);
+ depth = 2.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+ contact = transform.transform(Vec3f(-3.75, 0, 0));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ contact.setValue(0.05, 0, 0);
+ depth = 10.1;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ contact = transform.transform(Vec3f(0.05, 0, 0));
+ depth = 10.1;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Halfspace(Vec3f(0, 1, 0), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -1.25, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -3.75, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0.05, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, -2.5, 0);
+ depth = 5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, -2.5, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ contact.setValue(0, -1.25, 0);
+ depth = 7.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+ contact = transform.transform(Vec3f(0, -1.25, 0));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ contact.setValue(0, -3.75, 0);
+ depth = 2.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+ contact = transform.transform(Vec3f(0, -3.75, 0));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ contact.setValue(0, 0.05, 0);
+ depth = 10.1;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ contact = transform.transform(Vec3f(0, 0.05, 0));
+ depth = 10.1;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Halfspace(Vec3f(0, 0, 1), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 12.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 12.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -6.25)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -6.25))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, -5);
+ depth = 10;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, -5));
+ depth = 10;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ contact.setValue(0, 0, -3.75);
+ depth = 12.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+ contact = transform.transform(Vec3f(0, 0, -3.75));
+ depth = 12.5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ contact.setValue(0, 0, -6.25);
+ depth = 7.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+ contact = transform.transform(Vec3f(0, 0, -6.25));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 10.1));
+ contact.setValue(0, 0, 0.05);
+ depth = 20.1;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ contact = transform.transform(Vec3f(0, 0, 0.05));
+ depth = 20.1;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
@@ -1040,167 +1390,199 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
Capsule s(5, 10);
Plane hs(Vec3f(1, 0, 0), 0);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
- bool res;
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(2.5, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 5;
+ normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ 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)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ contact.setValue(2.5, 0, 0);
+ depth = 2.5;
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ contact.setValue(-2.5, 0, 0);
+ depth = 2.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Plane(Vec3f(0, 1, 0), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 2.5, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 5;
+ normal.setValue(0, 1, 0); // (0, 1, 0) or (0, -1, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ contact.setValue(0, 2.5, 0);
+ depth = 2.5;
+ normal.setValue(0, 1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, 1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ contact.setValue(0, -2.5, 0);
+ depth = 2.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Plane(Vec3f(0, 0, 1), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 10;
+ normal.setValue(0, 0, 1); // (0, 0, 1) or (0, 0, -1)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, 0));
+ depth = 10;
+ normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ contact.setValue(0, 0, 2.5);
+ depth = 7.5;
+ normal.setValue(0, 0, 1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+ contact = transform.transform(Vec3f(0, 0, 2.5));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(0, 0, 1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ contact.setValue(0, 0, -2.5);
+ depth = 7.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+ contact = transform.transform(Vec3f(0, 0, -2.5));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
@@ -1208,185 +1590,217 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
Cylinder s(5, 10);
Halfspace hs(Vec3f(1, 0, 0), 0);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
- bool res;
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(-2.5, 0, 0);
+ depth = 5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(-2.5, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ contact.setValue(-1.25, 0, 0);
+ depth = 7.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+ contact = transform.transform(Vec3f(-1.25, 0, 0));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ contact.setValue(-3.75, 0, 0);
+ depth = 2.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+ contact = transform.transform(Vec3f(-3.75, 0, 0));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ contact.setValue(0.05, 0, 0);
+ depth = 10.1;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ contact = transform.transform(Vec3f(0.05, 0, 0));
+ depth = 10.1;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Halfspace(Vec3f(0, 1, 0), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -1.25, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -3.75, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0.05, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, -2.5, 0);
+ depth = 5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, -2.5, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ contact.setValue(0, -1.25, 0);
+ depth = 7.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+ contact = transform.transform(Vec3f(0, -1.25, 0));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ contact.setValue(0, -3.75, 0);
+ depth = 2.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+ contact = transform.transform(Vec3f(0, -3.75, 0));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ contact.setValue(0, 0.05, 0);
+ depth = 10.1;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ contact = transform.transform(Vec3f(0, 0.05, 0));
+ depth = 10.1;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Halfspace(Vec3f(0, 0, 1), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -1.25)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -1.25))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, -2.5);
+ depth = 5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, -2.5));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ contact.setValue(0, 0, -1.25);
+ depth = 7.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+ contact = transform.transform(Vec3f(0, 0, -1.25));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ contact.setValue(0, 0, -3.75);
+ depth = 2.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+ contact = transform.transform(Vec3f(0, 0, -3.75));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 5.1));
+ contact.setValue(0, 0, 0.05);
+ depth = 10.1;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
+ contact = transform.transform(Vec3f(0, 0, 0.05));
+ depth = 10.1;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -5.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
@@ -1394,167 +1808,199 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
Cylinder s(5, 10);
Plane hs(Vec3f(1, 0, 0), 0);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
- bool res;
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(2.5, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 5;
+ normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ 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)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ contact.setValue(2.5, 0, 0);
+ depth = 2.5;
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ contact.setValue(-2.5, 0, 0);
+ depth = 2.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Plane(Vec3f(0, 1, 0), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 2.5, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 5;
+ normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ contact.setValue(0, 2.5, 0);
+ depth = 2.5;
+ normal.setValue(0, 1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, 1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ contact.setValue(0, -2.5, 0);
+ depth = 2.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Plane(Vec3f(0, 0, 1), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 5;
+ normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ contact.setValue(0, 0, 2.5);
+ depth = 2.5;
+ normal.setValue(0, 0, 1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, 0, 1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ contact.setValue(0, 0, -2.5);
+ depth = 2.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}
@@ -1563,185 +2009,217 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
Cone s(5, 10);
Halfspace hs(Vec3f(1, 0, 0), 0);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
- bool res;
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, -5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, -5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, -5))));
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0.05, 0, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, -5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(-2.5, 0, -5);
+ depth = 5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(-2.5, 0, -5));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ contact.setValue(-1.25, 0, -5);
+ depth = 7.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+ contact = transform.transform(Vec3f(-1.25, 0, -5));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ contact.setValue(-3.75, 0, -5);
+ depth = 2.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+ contact = transform.transform(Vec3f(-3.75, 0, -5));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ contact.setValue(0.05, 0, -5);
+ depth = 10.1;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ contact = transform.transform(Vec3f(0.05, 0, -5));
+ depth = 10.1;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Halfspace(Vec3f(0, 1, 0), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -2.5, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, -5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -1.25, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, -5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -3.75, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, -5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0.05, -5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, -5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, -2.5, -5);
+ depth = 5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, -2.5, -5));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ contact.setValue(0, -1.25, -5);
+ depth = 7.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+ contact = transform.transform(Vec3f(0, -1.25, -5));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ contact.setValue(0, -3.75, -5);
+ depth = 2.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+ contact = transform.transform(Vec3f(0, -3.75, -5));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ contact.setValue(0, 0.05, -5);
+ depth = 10.1;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ contact = transform.transform(Vec3f(0, 0.05, -5));
+ depth = 10.1;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
hs = Halfspace(Vec3f(0, 0, 1), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -1.25)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -1.25))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, -2.5);
+ depth = 5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, -2.5));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ contact.setValue(0, 0, -1.25);
+ depth = 7.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+ contact = transform.transform(Vec3f(0, 0, -1.25));
+ depth = 7.5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ contact.setValue(0, 0, -3.75);
+ depth = 2.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+ contact = transform.transform(Vec3f(0, 0, -3.75));
+ depth = 2.5;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 5.1));
+ contact.setValue(0, 0, 0.05);
+ depth = 10.1;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
+ contact = transform.transform(Vec3f(0, 0, 0.05));
+ depth = 10.1;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -5.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
@@ -1749,173 +2227,205 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
Cone s(5, 10);
Plane hs(Vec3f(1, 0, 0), 0);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
Vec3f contact;
- FCL_REAL depth;
- Vec3f normal;
- bool res;
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(2.5, 0, -2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, -2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
- BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, -2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, -2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
-
-
-
- hs = Plane(Vec3f(0, 1, 0), 0);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, 2.5, -2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, -2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
- BOOST_CHECK(contact.equal(Vec3f(0, -2.5, -2.5)));
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, -2.5))));
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
-
-
-
+ FCL_REAL depth;
+ Vec3f normal;
- hs = Plane(Vec3f(0, 0, 1), 0);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 5;
+ normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ 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)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ contact.setValue(2.5, 0, -2.5);
+ depth = 2.5;
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ contact.setValue(-2.5, 0, -2.5);
+ depth = 2.5;
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
- res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
+ hs = Plane(Vec3f(0, 1, 0), 0);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
- BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 5;
+ normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ contact.setValue(0, 2.5, -2.5);
+ depth = 2.5;
+ normal.setValue(0, 1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, 1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ contact.setValue(0, -2.5, -2.5);
+ depth = 2.5;
+ normal.setValue(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, -1, 0);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
- BOOST_CHECK(res);
- BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
- BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
- BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
- res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ hs = Plane(Vec3f(0, 0, 1), 0);
- res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ contact.setValue(0, 0, 0);
+ depth = 5;
+ normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = transform;
+ tf2 = transform;
+ contact = transform.transform(Vec3f(0, 0, 0));
+ depth = 5;
+ normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ contact.setValue(0, 0, 2.5);
+ depth = 2.5;
+ normal.setValue(0, 0, 1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, 0, 1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ contact.setValue(0, 0, -2.5);
+ depth = 2.5;
+ normal.setValue(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, 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;
+ normal = transform.getRotation() * Vec3f(0, 0, -1);
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, -10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+ testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}
BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
-{
+{
Sphere s1(20);
Sphere s2(10);
@@ -1977,7 +2487,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
}
BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
-{
+{
Box s1(20, 40, 50);
Box s2(10, 10, 10);
Vec3f closest_p1, closest_p2;
@@ -2064,7 +2574,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
-
+
res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 0.1) < 0.05);
BOOST_CHECK(res);
@@ -2072,7 +2582,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 17.5) < 0.001);
BOOST_CHECK(res);
-
+
res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 17.5) < 0.001);
BOOST_CHECK(res);
@@ -2195,124 +2705,81 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
Sphere s1(20);
Sphere s2(10);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- CollisionRequest request;
- CollisionResult result;
-
- Vec3f contact;
- FCL_REAL penetration_depth;
- Vec3f normal;
- bool res;
-
- request.gjk_solver_type = GST_INDEP; // use indep GJK solver
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
-
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
-
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
-
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
-
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
-
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
- BOOST_CHECK(res);
-
-
- res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform, request, result) > 0);
- BOOST_CHECK(res);
-
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
-
-
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
-
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0);
- BOOST_CHECK(res);
+// Vec3f contact;
+// FCL_REAL depth;
+ Vec3f normal;
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
- result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(40, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(30, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(30.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(29.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-29.9, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-30.0, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(-30.01, 0, 0));
+ normal.setValue(-1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
@@ -2320,45 +2787,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
Box s1(20, 40, 50);
Box s2(10, 10, 10);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
- FCL_REAL penetration_depth;
- Vec3f normal;
- bool res;
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
-
- res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
-
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
Quaternion3f q;
q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
-
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(15, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(15.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(q);
+ normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(q);
+ normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
}
BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox)
@@ -2366,43 +2837,92 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox)
Sphere s1(20);
Box s2(5, 5, 5);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
- FCL_REAL penetration_depth;
- Vec3f normal;
- bool res;
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(22.5, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-7); // built-in GJK solver requires larger tolerance than libccd
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(22.51, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(22.4, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+ // built-in GJK solver returns incorrect normal.
+ // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+}
- res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule)
+{
+ Sphere s1(20);
+ Capsule s2(5, 10);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+ Transform3f tf1;
+ Transform3f tf2;
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
+ Transform3f transform;
+ generateRandomTransform(extents, transform);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(24.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(25, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
}
BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
@@ -2410,167 +2930,254 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
Cylinder s1(5, 10);
Cylinder s2(5, 10);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
- FCL_REAL penetration_depth;
- Vec3f normal;
- bool res;
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true);
+ // built-in GJK solver returns incorrect normal.
+ // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(10, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+}
- res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone)
+{
+ Cone s1(5, 10);
+ Cone s2(5, 10);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+ Transform3f tf1;
+ Transform3f tf2;
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+ Transform3f transform;
+ generateRandomTransform(extents, transform);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ normal.setValue(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 5.7e-1); // built-in GJK solver requires larger tolerance than libccd
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+ normal = transform.getRotation() * Vec3f(1, 0, 0);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+ // built-in GJK solver returns incorrect normal.
+ // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(10.1, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 9.9));
+ normal.setValue(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
+ normal = transform.getRotation() * Vec3f(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+ // built-in GJK solver returns incorrect normal.
+ // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
}
-BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone)
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder)
{
- Cone s1(5, 10);
+ Cylinder s1(5, 10);
Cone s2(5, 10);
+ Transform3f tf1;
+ Transform3f tf2;
+
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
- FCL_REAL penetration_depth;
- Vec3f normal;
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
+
+ tf1 = Transform3f();
+ tf2 = Transform3f();
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform;
+ // TODO: Need convention for normal when the centers of two objects are at same position.
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(10, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(10, 0, 0));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 9.9));
+ normal.setValue(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
+ normal = transform.getRotation() * Vec3f(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+ // built-in GJK solver returns incorrect normal.
+ // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = Transform3f();
+ tf2 = Transform3f(Vec3f(0, 0, 10));
+ normal.setValue(0, 0, 1);
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+ tf1 = transform;
+ tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+}
+
+
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
+{
+ Sphere s(10);
+ Vec3f t[3];
+ t[0].setValue(20, 0, 0);
+ t[1].setValue(-20, 0, 0);
+ t[2].setValue(0, 20, 0);
+
+ Transform3f transform;
+ generateRandomTransform(extents, transform);
+
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
bool res;
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+ t[0].setValue(30, 0, 0);
+ t[1].setValue(9.9, -20, 0);
+ t[2].setValue(9.9, 20, 0);
+ res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal);
BOOST_CHECK(res);
+ BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
+ BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
-BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder)
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle)
{
- Cylinder s1(5, 10);
- Cone s2(5, 10);
+ Halfspace hs(Vec3f(1, 0, 0), 0);
+ Vec3f t[3];
+ t[0].setValue(20, 0, 0);
+ t[1].setValue(-20, 0, 0);
+ t[2].setValue(0, 20, 0);
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
- FCL_REAL penetration_depth;
- Vec3f normal;
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
bool res;
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
-
- res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
-
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
+ t[0].setValue(20, 0, 0);
+ t[1].setValue(0, -20, 0);
+ t[2].setValue(0, 20, 0);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
BOOST_CHECK(res);
+ BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), NULL, NULL, NULL);
- BOOST_CHECK(res);
- res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), &contact, &penetration_depth, &normal);
+ res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
-
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), NULL, NULL, NULL);
- BOOST_CHECK_FALSE(res);
- res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &penetration_depth, &normal);
- BOOST_CHECK_FALSE(res);
+ BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
-
-BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle)
{
- Sphere s(10);
+ Plane hs(Vec3f(1, 0, 0), 0);
Vec3f t[3];
t[0].setValue(20, 0, 0);
t[1].setValue(-20, 0, 0);
@@ -2579,29 +3186,41 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
Transform3f transform;
generateRandomTransform(extents, transform);
+ // Vec3f point;
+ // FCL_REAL depth;
+ Vec3f normal;
bool res;
- res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+ res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
BOOST_CHECK(res);
- t[0].setValue(30, 0, 0);
- t[1].setValue(9.9, -20, 0);
- t[2].setValue(9.9, 20, 0);
- res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+
+ t[0].setValue(20, 0, 0);
+ t[1].setValue(-0.1, -20, 0);
+ t[2].setValue(-0.1, 20, 0);
+ res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
- res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+ res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+ BOOST_CHECK(res);
+
+ res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
+ BOOST_CHECK(res);
+ BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+
+ res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
+ BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
BOOST_AUTO_TEST_CASE(spheresphere)
-{
+{
Sphere s1(20);
Sphere s2(10);
@@ -2610,7 +3229,7 @@ BOOST_AUTO_TEST_CASE(spheresphere)
bool res;
FCL_REAL dist = -1;
-
+
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 10) < 0.001);
BOOST_CHECK(res);
@@ -2662,7 +3281,7 @@ BOOST_AUTO_TEST_CASE(spheresphere)
}
BOOST_AUTO_TEST_CASE(boxbox)
-{
+{
Box s1(20, 40, 50);
Box s2(10, 10, 10);
@@ -2719,7 +3338,7 @@ BOOST_AUTO_TEST_CASE(boxsphere)
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 0.1) < 0.01);
BOOST_CHECK(res);
-
+
res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 0.1) < 0.01);
BOOST_CHECK(res);
@@ -2727,7 +3346,7 @@ BOOST_AUTO_TEST_CASE(boxsphere)
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 17.5) < 0.001);
BOOST_CHECK(res);
-
+
res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 17.5) < 0.001);
BOOST_CHECK(res);
diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp
index 62f36693db2a0bb9cf7542a740267fe960f2a68d..58dedd96a45449997f6fef511eed9c9b85f9b41d 100644
--- a/test/test_fcl_math.cpp
+++ b/test/test_fcl_math.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_octomap.cpp b/test/test_fcl_octomap.cpp
index 6b964b26ffff3bec92c58bfe10387f235f87eb40..0d4b49cc857c5b00b037aaa70ea53f8b58782c6d 100644
--- a/test/test_fcl_octomap.cpp
+++ b/test/test_fcl_octomap.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_shape_mesh_consistency.cpp b/test/test_fcl_shape_mesh_consistency.cpp
index 9fc9e9a46931aae1e86c766886ec8a0ab58b3931..552f3fe1c02187de9854a92132dc25baa407357e 100644
--- a/test/test_fcl_shape_mesh_consistency.cpp
+++ b/test/test_fcl_shape_mesh_consistency.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp
index 6d8cf82c66b90cfda194f7c1f38f3c805d6ad85c..e9612b3963b0b1d89ae3d66541732079c5680a7f 100644
--- a/test/test_fcl_sphere_capsule.cpp
+++ b/test/test_fcl_sphere_capsule.cpp
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index e3bda2d8870ebb4be1550eaec6b228864ebcc291..76f8e952f29abf4c6538fbbf500c50b27afb458c 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -400,4 +400,58 @@ bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, Continuous
return true;
}
+std::string getNodeTypeName(NODE_TYPE node_type)
+{
+ if (node_type == BV_UNKNOWN)
+ return std::string("BV_UNKNOWN");
+ else if (node_type == BV_AABB)
+ return std::string("BV_AABB");
+ else if (node_type == BV_OBB)
+ return std::string("BV_OBB");
+ else if (node_type == BV_RSS)
+ return std::string("BV_RSS");
+ else if (node_type == BV_kIOS)
+ return std::string("BV_kIOS");
+ else if (node_type == BV_OBBRSS)
+ return std::string("BV_OBBRSS");
+ else if (node_type == BV_KDOP16)
+ return std::string("BV_KDOP16");
+ else if (node_type == BV_KDOP18)
+ return std::string("BV_KDOP18");
+ else if (node_type == BV_KDOP24)
+ return std::string("BV_KDOP24");
+ else if (node_type == GEOM_BOX)
+ return std::string("GEOM_BOX");
+ else if (node_type == GEOM_SPHERE)
+ return std::string("GEOM_SPHERE");
+ else if (node_type == GEOM_CAPSULE)
+ return std::string("GEOM_CAPSULE");
+ else if (node_type == GEOM_CONE)
+ return std::string("GEOM_CONE");
+ else if (node_type == GEOM_CYLINDER)
+ return std::string("GEOM_CYLINDER");
+ else if (node_type == GEOM_CONVEX)
+ return std::string("GEOM_CONVEX");
+ else if (node_type == GEOM_PLANE)
+ return std::string("GEOM_PLANE");
+ else if (node_type == GEOM_HALFSPACE)
+ return std::string("GEOM_HALFSPACE");
+ else if (node_type == GEOM_TRIANGLE)
+ return std::string("GEOM_TRIANGLE");
+ else if (node_type == GEOM_OCTREE)
+ return std::string("GEOM_OCTREE");
+ else
+ return std::string("invalid");
+}
+
+std::string getGJKSolverName(GJKSolverType solver_type)
+{
+ if (solver_type == GST_LIBCCD)
+ return std::string("libccd");
+ else if (solver_type == GST_INDEP)
+ return std::string("built-in");
+ else
+ return std::string("invalid");
+}
+
}
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index 45d58936011be0c515ad9d610ec92cb1767a50e3..6d04a91a329e73c0cabfffdebd042d88c27eeb59 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -1,7 +1,8 @@
/*
* Software License Agreement (BSD License)
*
- * Copyright (c) 2011, Willow Garage, Inc.
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
* copyright 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
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -182,6 +183,10 @@ bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, Continuou
bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist);
+std::string getNodeTypeName(NODE_TYPE node_type);
+
+std::string getGJKSolverName(GJKSolverType solver_type);
+
}
#endif