Unverified Commit 94146680 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #103 from humanoid-path-planner/refactoring

Refactoring
parents 6b10c369 45819b0d
...@@ -4,30 +4,78 @@ env: ...@@ -4,30 +4,78 @@ env:
global: global:
- CTEST_PARALLEL_LEVEL=4 - CTEST_PARALLEL_LEVEL=4
- CTEST_OUTPUT_ON_FAILURE=1 - CTEST_OUTPUT_ON_FAILURE=1
- CXX_FLAGS_DEBUG="-O1"
- BUILD_PYTHON_INTERFACE=ON
- MAKEFLAGS="-j2" - MAKEFLAGS="-j2"
cache:
ccache: true
matrix: matrix:
include: include:
- dist: trusty - name: "Trusty - Release - g++"
compiler: gcc
env: BUILD_TYPE=Release env: BUILD_TYPE=Release
- dist: xenial dist: trusty
compiler: gcc compiler: g++
addons:
apt:
packages:
- cmake
- libboost-all-dev
- libassimp-dev
- libeigen3-dev
- name: "Xenial - Release - g++"
env: BUILD_TYPE=Release env: BUILD_TYPE=Release
- dist: bionic dist: xenial
compiler: gcc compiler: g++
addons:
apt:
packages:
- cmake
- libboost-all-dev
- libassimp-dev
- libeigen3-dev
- liboctomap-dev
- name: "Bionic - Release - g++"
env: BUILD_TYPE=Release env: BUILD_TYPE=Release
- dist: bionic dist: bionic
compiler: gcc compiler: g++
addons:
apt:
packages:
- cmake
- libboost-all-dev
- libassimp-dev
- libeigen3-dev
- liboctomap-dev
- name: "Bionic - Debug - g++"
env: BUILD_TYPE=Debug env: BUILD_TYPE=Debug
- os: osx dist: bionic
compiler: clang compiler: g++
addons:
apt:
packages:
- cmake
- libboost-all-dev
- libassimp-dev
- libeigen3-dev
- liboctomap-dev
- name: "OSX - Release - clang"
env: BUILD_TYPE=Release env: BUILD_TYPE=Release
os: osx
compiler: clang
cache:
ccache: true
directories:
- $HOME/Library/Caches/Homebrew
install: before_install:
# Install dependencies for FCL - if [ "$TRAVIS_OS_NAME" = "linux" ]; then source travis_custom/custom_before_install_linux.sh ; fi
- if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi - if [ "$TRAVIS_OS_NAME" = "osx" ]; then source travis_custom/custom_before_install_osx.sh ; fi
- if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/install_osx.sh' ; fi
script: script:
# Create build directory # Create build directory
...@@ -35,7 +83,7 @@ script: ...@@ -35,7 +83,7 @@ script:
- cd build - cd build
# Configure # Configure
- cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DCMAKE_CXX_FLAGS=-w .. - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DCMAKE_CXX_FLAGS=-w -DCMAKE_CXX_FLAGS_DEBUG=${CXX_FLAGS_DEBUG} -DBUILD_PYTHON_INTERFACE=${BUILD_PYTHON_INTERFACE} ..
# Build # Build
- make - make
......
...@@ -34,16 +34,17 @@ ...@@ -34,16 +34,17 @@
cmake_minimum_required(VERSION 2.8) cmake_minimum_required(VERSION 2.8)
set(CXX_DISABLE_WERROR TRUE) set(CXX_DISABLE_WERROR TRUE)
include(cmake/base.cmake)
include(cmake/eigen.cmake)
include(cmake/boost.cmake)
include(cmake/hpp.cmake)
set(PROJECT_NAME hpp-fcl) set(PROJECT_NAME hpp-fcl)
set(PROJECT_DESCRIPTION set(PROJECT_DESCRIPTION
"HPP fork of FCL -- The Flexible Collision Library" "HPP fork of FCL -- The Flexible Collision Library"
) )
include(cmake/eigen.cmake)
include(cmake/boost.cmake)
include(cmake/python.cmake)
include(cmake/hpp.cmake)
IF(APPLE) IF(APPLE)
SET(CMAKE_MACOSX_RPATH TRUE) SET(CMAKE_MACOSX_RPATH TRUE)
SET(CMAKE_SKIP_BUILD_RPATH FALSE) SET(CMAKE_SKIP_BUILD_RPATH FALSE)
...@@ -57,7 +58,11 @@ IF(APPLE) ...@@ -57,7 +58,11 @@ IF(APPLE)
endif("${isSystemDir}" STREQUAL "-1") endif("${isSystemDir}" STREQUAL "-1")
ENDIF(APPLE) ENDIF(APPLE)
setup_hpp_project() OPTION(BUILD_PYTHON_INTERFACE "Build the python bindings" OFF)
# Tell CMake that we compute the PROJECT_VERSION manually.
CMAKE_POLICY(SET CMP0048 OLD)
project(${PROJECT_NAME} CXX)
add_required_dependency("eigen3 >= 3.0.0") add_required_dependency("eigen3 >= 3.0.0")
include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS})
...@@ -66,10 +71,13 @@ include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) ...@@ -66,10 +71,13 @@ include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS})
set (RUN_TESTS TRUE CACHE BOOL "compile and run unit tests") set (RUN_TESTS TRUE CACHE BOOL "compile and run unit tests")
# Required dependencies # Required dependencies
set(BOOST_COMPONENTS thread date_time system)
if (RUN_TESTS) if (RUN_TESTS)
set(BOOST_COMPONENTS thread date_time filesystem system unit_test_framework) set(BOOST_COMPONENTS ${BOOST_COMPONENTS} filesystem unit_test_framework)
else () endif ()
set(BOOST_COMPONENTS thread date_time system) if (BUILD_PYTHON_INTERFACE)
FINDPYTHON()
set(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
endif () endif ()
search_for_boost() search_for_boost()
...@@ -97,10 +105,8 @@ ADD_REQUIRED_DEPENDENCY("assimp >= 2.0") ...@@ -97,10 +105,8 @@ ADD_REQUIRED_DEPENDENCY("assimp >= 2.0")
if(ASSIMP_FOUND) if(ASSIMP_FOUND)
if (NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150") if (NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150")
add_definitions(-DHPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES) add_definitions(-DHPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES)
SET(HPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES TRUE)
message(STATUS "Assimp version has unified headers") message(STATUS "Assimp version has unified headers")
else() else()
SET(HPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES FALSE)
message(STATUS "Assimp version does not have unified headers") message(STATUS "Assimp version does not have unified headers")
endif() endif()
endif() endif()
...@@ -118,57 +124,53 @@ SET(${PROJECT_NAME}_HEADERS ...@@ -118,57 +124,53 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/BV/kDOP.h include/hpp/fcl/BV/kDOP.h
include/hpp/fcl/narrowphase/narrowphase.h include/hpp/fcl/narrowphase/narrowphase.h
include/hpp/fcl/narrowphase/gjk.h include/hpp/fcl/narrowphase/gjk.h
include/hpp/fcl/shape/geometric_shapes_utility.h
include/hpp/fcl/shape/geometric_shape_to_BVH_model.h include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
include/hpp/fcl/shape/geometric_shapes.h include/hpp/fcl/shape/geometric_shapes.h
include/hpp/fcl/distance_func_matrix.h include/hpp/fcl/distance_func_matrix.h
include/hpp/fcl/collision.h include/hpp/fcl/collision.h
include/hpp/fcl/collision_node.h
include/hpp/fcl/collision_func_matrix.h include/hpp/fcl/collision_func_matrix.h
include/hpp/fcl/distance.h include/hpp/fcl/distance.h
include/hpp/fcl/math/matrix_3f.h include/hpp/fcl/math/matrix_3f.h
include/hpp/fcl/math/vec_3f.h include/hpp/fcl/math/vec_3f.h
include/hpp/fcl/math/tools.h include/hpp/fcl/math/types.h
include/hpp/fcl/math/transform.h include/hpp/fcl/math/transform.h
include/hpp/fcl/traversal/details/traversal.h
include/hpp/fcl/traversal/traversal_node_shapes.h
include/hpp/fcl/traversal/traversal_node_setup.h
include/hpp/fcl/traversal/traversal_recurse.h
include/hpp/fcl/traversal/traversal_node_octree.h
include/hpp/fcl/traversal/traversal_node_bvhs.h
include/hpp/fcl/traversal/traversal_node_bvh_shape.h
include/hpp/fcl/traversal/traversal_node_base.h
include/hpp/fcl/data_types.h include/hpp/fcl/data_types.h
include/hpp/fcl/BVH/BV_splitter.h
include/hpp/fcl/BVH/BVH_internal.h include/hpp/fcl/BVH/BVH_internal.h
include/hpp/fcl/BVH/BVH_model.h include/hpp/fcl/BVH/BVH_model.h
include/hpp/fcl/BVH/BV_fitter.h
include/hpp/fcl/BVH/BVH_front.h include/hpp/fcl/BVH/BVH_front.h
include/hpp/fcl/BVH/BVH_utility.h include/hpp/fcl/BVH/BVH_utility.h
include/hpp/fcl/intersect.h
include/hpp/fcl/collision_object.h include/hpp/fcl/collision_object.h
include/hpp/fcl/collision_utility.h include/hpp/fcl/collision_utility.h
include/hpp/fcl/octree.h include/hpp/fcl/octree.h
include/hpp/fcl/fwd.hh include/hpp/fcl/fwd.hh
include/hpp/fcl/mesh_loader/assimp.h include/hpp/fcl/mesh_loader/assimp.h
include/hpp/fcl/mesh_loader/loader.h include/hpp/fcl/mesh_loader/loader.h
include/hpp/fcl/internal/BV_fitter.h
include/hpp/fcl/internal/BV_splitter.h
include/hpp/fcl/internal/intersect.h
include/hpp/fcl/internal/tools.h
include/hpp/fcl/internal/traversal_node_base.h
include/hpp/fcl/internal/traversal_node_bvh_shape.h
include/hpp/fcl/internal/traversal_node_bvhs.h
include/hpp/fcl/internal/traversal_node_octree.h
include/hpp/fcl/internal/traversal_node_setup.h
include/hpp/fcl/internal/traversal_node_shapes.h
include/hpp/fcl/internal/traversal_recurse.h
include/hpp/fcl/internal/traversal.h
) )
add_subdirectory(src) add_subdirectory(src)
if (BUILD_PYTHON_INTERFACE)
add_subdirectory(python)
endif ()
if (RUN_TESTS) if (RUN_TESTS)
add_subdirectory(test) add_subdirectory(test)
endif () endif ()
pkg_config_append_libs("hpp-fcl") pkg_config_append_libs("hpp-fcl")
PKG_CONFIG_APPEND_BOOST_LIBS(thread date_time filesystem system) PKG_CONFIG_APPEND_BOOST_LIBS(thread date_time filesystem system)
IF(HPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES)
# FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES kept for backard compatibility reasons.
PKG_CONFIG_APPEND_CFLAGS("-DFCL_USE_ASSIMP_UNIFIED_HEADER_NAMES -DHPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES")
ENDIF(HPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES)
IF(HPP_FCL_HAVE_OCTOMAP) IF(HPP_FCL_HAVE_OCTOMAP)
# FCL_HAVE_OCTOMAP kept for backward compatibility reasons. # FCL_HAVE_OCTOMAP kept for backward compatibility reasons.
PKG_CONFIG_APPEND_CFLAGS( PKG_CONFIG_APPEND_CFLAGS(
"-DHPP_FCL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}") "-DHPP_FCL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}")
ENDIF(HPP_FCL_HAVE_OCTOMAP) ENDIF(HPP_FCL_HAVE_OCTOMAP)
setup_hpp_project_finalize()
...@@ -4,4 +4,8 @@ ...@@ -4,4 +4,8 @@
[![Pipeline status](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/commits/master) [![Pipeline status](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-fcl/master/coverage/) [![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-fcl/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-fcl/master/coverage/)
This project is a fork from https://github.com/flexible-collision-library/fcl. The main difference is the computation of a lower bound of the distance between two objects when collision checking is performed and no collision is found. This project is a fork from https://github.com/flexible-collision-library/fcl.
The main differences are.
- the use of a safety margin when detecting collision,
- the computation of a lower bound of the distance between two objects when collision checking is performed and no collision is found.
sudo apt-get -qq update
########################
# Mendatory dependencies
########################
sudo apt-get -qq --yes --force-yes install cmake
sudo apt-get -qq --yes --force-yes install libboost-all-dev
sudo apt-get -qq --yes --force-yes install libassimp-dev
sudo apt-get -qq --yes --force-yes install libeigen3-dev
# Octomap
git clone https://github.com/OctoMap/octomap
cd octomap
git checkout tags/v1.8.0
mkdir build
cd build
cmake ..
make
sudo make install
brew tap homebrew/science
brew install git
brew install cmake
brew install boost
brew install libccd
brew install assimp
brew install eigen
brew install octomap
Subproject commit 441552634e4c427956be7b2834a6bbf894b24f0c Subproject commit 46dc4a57521bde14ea75c959b6b4f887af50c65d
#!/usr/bin/env python3
import pdb
import sys
# ABC = AB^AC
# (ABC^AJ).a = (j.c - j.b) a.a + (j.a - j.c) b.a + (j.b - j.a) c.a, for j = b or c
segment_fmt = "{j}a_aa"
plane_fmt = ""
edge_fmt = "{j}a * {b}a_{c}a + {j}{b} * {c}a_aa - {j}{c} * {b}a_aa"
# These checks must be negative and not positive, as in the cheat sheet.
# They are the same as in the cheat sheet, except that we consider (...).dot(A) instead of (...).dot(-A)
plane_tests = [ "C.dot (a_cross_b)", "D.dot(a_cross_c)", "-D.dot(a_cross_b)" ]
checks = plane_tests \
+ [ edge_fmt.format (**{'j':j,'b':"b",'c':"c"}) for j in [ "b", "c" ] ] \
+ [ edge_fmt.format (**{'j':j,'b':"c",'c':"d"}) for j in [ "c", "d" ] ] \
+ [ edge_fmt.format (**{'j':j,'b':"d",'c':"b"}) for j in [ "d", "b" ] ] \
+ [ segment_fmt.format(**{'j':j}) for j in [ "b", "c", "d" ] ]
checks_hr = [ "ABC.AO >= 0", "ACD.AO >= 0", "ADB.AO >= 0" ] \
+ [ "(ABC ^ {}).AO >= 0".format(n) for n in [ "AB", "AC" ] ] \
+ [ "(ACD ^ {}).AO >= 0".format(n) for n in [ "AC", "AD" ] ] \
+ [ "(ADB ^ {}).AO >= 0".format(n) for n in [ "AD", "AB" ] ] \
+ [ "AB.AO >= 0", "AC.AO >= 0", "AD.AO >= 0" ]
# weights of the checks.
weights = [ 2, ] * 3 + [ 3, ] * 6 + [ 1, ] * 3
# Segment tests first, because they have lower weight.
#tests = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, ]
tests = [9, 10, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, ]
assert len(tests) == len(checks)
assert sorted(tests) == list(range(len(tests)))
regions = [ "ABC", "ACD", "ADB", "AB", "AC", "AD", "A", "Inside", ]
cases = list(range(len(regions)))
# The following 3 lists refer to table doc/GJK_tetrahedra_boolean_table.ods
# A check ID is (+/- (index+1)) where a minus sign encodes a NOT operation
# and index refers to an index in list checks.
# definitions is a list of list of check IDs to be ANDed.
# For instance, a0.a3.!a4 -> [ 1, 4, -5]
definitions = [
[ 1, 4,- 5 ],
[ 2, 6,- 7 ],
[ 3, 8,- 9 ],
[- 4, 9, 10 ],
[- 6, 5, 11 ],
[- 8, 7, 12 ],
[-10,-11,-12 ],
[- 1,- 2,- 3 ],
]
# conditions is a list of (list of (list of check IDs to be ANDed) to be ORed).
conditions = [
[],
[],
[],
[],
[],
[],
[],
[ ], #[ [10, 11, 12], ], # I don't think this is always true...
]
# rejections is a list of (list of (list of check IDs to be ANDed) to be ORed).
rejections = [
[ [ 2, 6, 7], [ 3,- 8,- 9], ],
[ [ 3, 8, 9], [ 1,- 4,- 5], ],
[ [ 1, 4, 5], [ 2,- 6,- 7], ],
[ [- 1,- 3], ],
[ [- 2,- 1], ],
[ [- 3,- 2], ],
[ [ 4,- 5], [ 6,- 7], [ 8,- 9], ],
[],
]
implications = [
[ [ 4, 5, 10, ], [ 11],],
[ [ 6, 7, 11, ], [ 12],],
[ [ 8, 9, 12, ], [ 10],],
[ [- 4,- 5, 11, ], [ 10],],
[ [- 6,- 7, 12, ], [ 11],],
[ [- 8,- 9, 10, ], [ 12],],
[ [ 1, 4, 5, 6], [- 7] ],
[ [ 2, 6, 9, 8], [- 9] ],
[ [ 3, 8, 9, 4], [- 5] ],
[ [- 4, 5, 10,], [- 11] ],
[ [ 4,- 5,- 10,], [ 11] ],
[ [- 6, 7, 11,], [- 12] ],
[ [ 6,- 7,- 11,], [ 12] ],
[ [- 8, 9, 12,], [- 10] ],
[ [ 8,- 9,- 12,], [ 10] ],
[ [ 10, 3, 9, -12, 4, -5], [1] ],
[ [ 10, -3, 1, -4], [9] ],
[ [ 10, -3, -1, 2, -6, 11], [5] ],
[ [ -10, 11, 2, -12, -5, -1], [6] ],
[ [ -10,11,-2,1,5], [-6] ],
[ [-10,-11,12,1,-7,-2,4],[-5]],
[ [-10,-11,12,-3,2,7],[-8]],
[ [-10,-11,12,-3,-2],[-1]],
]
def set_test_values (current_tests, test_values, itest, value):
def satisfies (values, indices):
for k in indices:
if k > 0 and values[ k-1] != True : return False
if k < 0 and values[-k-1] != False: return False
return True
remaining_tests = list(current_tests)
next_test_values = list(test_values)
remaining_tests.remove (itest)
next_test_values[itest] = value
rerun = True
while rerun:
rerun = False
for impl in implications:
if satisfies(next_test_values, impl[0]):
for id in impl[1]:
k = (id - 1) if id > 0 else (-id-1)
if k in remaining_tests:
next_test_values[k] = (id > 0)
remaining_tests.remove(k)
rerun = True
else:
if next_test_values[k] != (id > 0):
raise ValueError ("Absurd case")
return remaining_tests, next_test_values
def set_tests_values (current_tests, test_values, itests, values):
for itest,value in zip(itests,values):
current_tests, test_values = set_test_values (current_tests, test_values, itest, value)
return current_tests, test_values
def apply_test_values (cases, test_values):
def canSatisfy (values, indices):
for k in indices:
if k > 0 and values[ k-1] == False: return False
if k < 0 and values[-k-1] == True : return False
return True
def satisfies (values, indices):
for k in indices:
if k > 0 and values[ k-1] != True : return False
if k < 0 and values[-k-1] != False: return False
return True
# Check all cases.
left_cases = []
for case in cases:
defi = definitions[case]
conds = conditions[case]
rejs = rejections[case]
if satisfies (test_values, defi):
# A definition is True, stop recursion
return [ case ]
if not canSatisfy (test_values, defi):
continue
for cond in conds:
if satisfies (test_values, cond):
# A condition is True, stop recursion
return [ case ]
append = True
for rej in rejs:
if satisfies (test_values, rej):
# A rejection is True, discard this case
append = False
break
if append: left_cases.append (case)
return left_cases
def max_number_of_tests (current_tests, cases, test_values = [None,]*len(tests), prevBestScore = float('inf'), prevScore = 0):
for test in current_tests:
assert test_values[test] == None, "Test " +str(test)+ " already performed"
left_cases = apply_test_values (cases, test_values)
if len(left_cases) == 1:
return prevScore, { 'case': left_cases[0], }
elif len(left_cases) == 0:
return prevScore, { 'case': None, 'comments': [ "applied " + str(test_values), "to " + ", ".join([regions[c] for c in cases ]) ] }
assert len(current_tests) > 0, "No more test but " + str(left_cases) + " remains"
currentBestScore = prevBestScore
bestScore = float('inf')
bestOrder = [None, None]
for i, test in enumerate(current_tests):
assert bestScore >= currentBestScore
currentScore = prevScore + len(left_cases) * weights[test]
#currentScore = prevScore + weights[test]
if currentScore > currentBestScore: # Cannot do better -> stop
continue
try:
remaining_tests, next_test_values = set_test_values (current_tests, test_values, test, True)
except ValueError:
remaining_tests = None
if remaining_tests is not None:
# Do not put this in try catch as I do not want other ValueError to be understood as an infeasible branch.
score_if_t, order_if_t = max_number_of_tests (remaining_tests, left_cases, next_test_values, currentBestScore, currentScore)
if score_if_t >= currentBestScore: # True didn't do better -> stop
continue
else:
score_if_t, order_if_t = prevScore, None
try:
remaining_tests, next_test_values = set_test_values (current_tests, test_values, test, False)
except ValueError:
remaining_tests = None
if remaining_tests is not None:
# Do not put this in try catch as I do not want other ValueError to be understood as an infeasible branch.
score_if_f, order_if_f = max_number_of_tests (remaining_tests, left_cases, next_test_values, currentBestScore, currentScore)
else:
score_if_f, order_if_f = prevScore, None
currentScore = max(score_if_t, score_if_f)
if currentScore < bestScore:
if currentScore < currentBestScore:
bestScore = currentScore
bestOrder = { 'test': test, 'true': order_if_t, 'false': order_if_f }
#pdb.set_trace()
currentBestScore = currentScore
if len(tests) == len(current_tests):
print ("New best score: {}".format(currentBestScore))
return bestScore, bestOrder
def printComments (order, indent, file):
if 'comments' in order:
for comment in order['comments']:
print (indent + "// " + comment, file=file)
def printOrder (order, indent = "", start=True,file=sys.stdout,curTests=[]):
if start:
print ("bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)", file=file)
print ("{", file=file)
print (indent+"// The code of this function was generated using doc/gjk.py", file=file)
print (indent+"const vertex_id_t a = 3, b = 2, c = 1, d = 0;", file=file)
for l in "abcd":
print (indent+"const Vec3f& {} (current.vertex[{}]->w);".format(l.upper(),l), file=file)
print (indent+"const FCL_REAL aa = A.squaredNorm();".format(l), file=file)
for l in "dcb":
for m in "abcd":
if m <= l:
print (indent+"const FCL_REAL {0}{1} = {2}.dot({3});".format(l,m,l.upper(),m.upper()), file=file)
else:
print (indent+"const FCL_REAL& {0}{1} = {1}{0};".format(l,m), file=file)
print (indent+"const FCL_REAL {0}a_aa = {0}a - aa;".format(l), file=file)
for l0,l1 in zip("bcd","cdb"):
print (indent+"const FCL_REAL {0}a_{1}a = {0}a - {1}a;".format(l0,l1), file=file)
for l in "bc":
print (indent+"const Vec3f a_cross_{0} = A.cross({1});".format(l,l.upper()), file=file)
print("", file=file)
print( "#define REGION_INSIDE() "+indent+"\\", file=file)
print(indent+" ray.setZero(); \\", file=file)
print(indent+" next.vertex[0] = current.vertex[d]; \\", file=file)
print(indent+" next.vertex[1] = current.vertex[c]; \\", file=file)
print(indent+" next.vertex[2] = current.vertex[b]; \\", file=file)
print(indent+" next.vertex[3] = current.vertex[a]; \\", file=file)
print(indent+" next.rank=4; \\", file=file)