diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs new file mode 100644 index 000000000..5ded1382c --- /dev/null +++ b/.git-blame-ignore-revs @@ -0,0 +1 @@ +0067c8aa66aac548601e2a3fd029aa264cc59f2a diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..020c493bc --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,14 @@ +repos: +- repo: https://github.com/pre-commit/mirrors-clang-format + rev: v13.0.1 + hooks: + - id: clang-format + args: ['-i', '--style={BasedOnStyle: Google, SortIncludes: false}'] +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.1.0 + hooks: + - id: trailing-whitespace +- repo: https://github.com/psf/black + rev: 22.3.0 + hooks: + - id: black diff --git a/INSTALL b/INSTALL index 7f53fd30c..a70b86267 100644 --- a/INSTALL +++ b/INSTALL @@ -22,7 +22,7 @@ Install: cd build cmake .. make -jN # N is the maximum number of parallel compile jobs - + Once the compilation is finished, make install will install the project. To specify the installation prefix, diff --git a/cmake-modules/Findassimp.cmake b/cmake-modules/Findassimp.cmake index 6c323ada1..db98c00af 100644 --- a/cmake-modules/Findassimp.cmake +++ b/cmake-modules/Findassimp.cmake @@ -3,7 +3,7 @@ if(CMAKE_SIZEOF_VOID_P EQUAL 8) elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) set(ASSIMP_ARCHITECTURE "32") endif(CMAKE_SIZEOF_VOID_P EQUAL 8) - + if(WIN32) set(ASSIMP_ROOT_DIR CACHE PATH "ASSIMP root directory") @@ -18,12 +18,12 @@ if(WIN32) if(MSVC12) set(ASSIMP_MSVC_VERSIONS "vc120") - elseif(MSVC14) + elseif(MSVC14) set(ASSIMP_MSVC_VERSIONS "vc140;vc141") endif(MSVC12) - + if(MSVC12 OR MSVC14) - + foreach(ASSIMP_MSVC_VERSION ${ASSIMP_MSVC_VERSIONS}) find_path(ASSIMP_LIBRARY_DIR NAMES @@ -32,27 +32,27 @@ if(WIN32) HINTS ${ASSIMP_ROOT_DIR}/lib${ASSIMP_ARCHITECTURE} ) - + find_library(ASSIMP_LIBRARY_RELEASE assimp-${ASSIMP_MSVC_VERSION}-mt.lib PATHS ${ASSIMP_LIBRARY_DIR}) find_library(ASSIMP_LIBRARY_DEBUG assimp-${ASSIMP_MSVC_VERSION}-mtd.lib PATHS ${ASSIMP_LIBRARY_DIR}) IF(NOT ASSIMP_LIBRARY_RELEASE AND NOT ASSIMP_LIBRARY_DEBUG) continue() ENDIF() - + IF(ASSIMP_LIBRARY_DEBUG) - set(ASSIMP_LIBRARY + set(ASSIMP_LIBRARY optimized ${ASSIMP_LIBRARY_RELEASE} debug ${ASSIMP_LIBRARY_DEBUG} ) ELSE() - set(ASSIMP_LIBRARY + set(ASSIMP_LIBRARY optimized ${ASSIMP_LIBRARY_RELEASE} ) ENDIF() - + set(ASSIMP_LIBRARIES ${ASSIMP_LIBRARY_RELEASE} ${ASSIMP_LIBRARY_DEBUG}) - + FUNCTION(ASSIMP_COPY_BINARIES TargetDirectory) ADD_CUSTOM_TARGET(AssimpCopyBinaries COMMAND ${CMAKE_COMMAND} -E copy ${ASSIMP_ROOT_DIR}/bin${ASSIMP_ARCHITECTURE}/assimp-${ASSIMP_MSVC_VERSION}-mtd.dll ${TargetDirectory}/Debug/assimp-${ASSIMP_MSVC_VERSION}-mtd.dll @@ -60,11 +60,11 @@ if(WIN32) COMMENT "Copying Assimp binaries to '${TargetDirectory}'" VERBATIM) ENDFUNCTION(ASSIMP_COPY_BINARIES) - + SET(assimp_LIBRARIES ${ASSIMP_LIBRARY}) endforeach() endif() - + else(WIN32) find_path( @@ -96,5 +96,5 @@ else(WIN32) message(FATAL_ERROR "Could not find asset importer library") endif (assimp_FIND_REQUIRED) endif (assimp_FOUND) - + endif(WIN32) diff --git a/doc/generate_distance_plot.py b/doc/generate_distance_plot.py index f5260de36..bfc66deb8 100644 --- a/doc/generate_distance_plot.py +++ b/doc/generate_distance_plot.py @@ -4,27 +4,38 @@ interactive = False -m = 1. +m = 1.0 b = 1.2 -mb = m+b +mb = m + b + +X = np.array([-mb / 2, 0, m, mb, 2 * mb]) +# X = np.linspace(-1, 4., 21) -X = np.array([ -mb/2, 0, m, mb, 2*mb ]) -#X = np.linspace(-1, 4., 21) def dlb(d): - if d<0: return None + if d < 0: + return None if d > mb: - u = d-mb - return mb-m + u / 2 - return d-m + u = d - mb + return mb - m + u / 2 + return d - m + plt.figure(figsize=(9, 3.5)) -#plt.plot(X, X-m, ":k") -#plt.plot([m+b, X[-1]], [b, b], ":k") -plt.fill_between([m+b, X[-1]], [b, b], [b, X[-1]-m], alpha=0.2, hatch="|", facecolor="g", label="Distance lower band area") -plt.plot(X, [ dlb(x) for x in X ], "-g", label="distance lower bound") -#plt.plot([X[0], m, m, X[-1]], [0, 0, b, b], ":k") +# plt.plot(X, X-m, ":k") +# plt.plot([m+b, X[-1]], [b, b], ":k") +plt.fill_between( + [m + b, X[-1]], + [b, b], + [b, X[-1] - m], + alpha=0.2, + hatch="|", + facecolor="g", + label="Distance lower band area", +) +plt.plot(X, [dlb(x) for x in X], "-g", label="distance lower bound") +# plt.plot([X[0], m, m, X[-1]], [0, 0, b, b], ":k") plt.axvspan(X[0], m, alpha=0.5, hatch="\\", facecolor="r", label="Collision area") @@ -38,9 +49,9 @@ def dlb(d): ax.grid(which="minor", ls="dashed") plt.axvline(0, ls="solid") -#plt.axvline(m, ls="dashed", label="margin") -#plt.axvline(mb, ls="dashed") -plt.axhline(0., ls="solid") +# plt.axvline(m, ls="dashed", label="margin") +# plt.axvline(mb, ls="dashed") +plt.axhline(0.0, ls="solid") plt.title("Collision and distance lower band") plt.legend(loc="lower right") @@ -48,7 +59,10 @@ def dlb(d): plt.show() else: import os.path as path + dir_path = path.dirname(path.realpath(__file__)) - plt.savefig(path.join(dir_path, "distance_computation.png"), - bbox_inches="tight", - orientation="landscape") + plt.savefig( + path.join(dir_path, "distance_computation.png"), + bbox_inches="tight", + orientation="landscape", + ) diff --git a/doc/gjk.py b/doc/gjk.py index 06d25c93e..3d200b128 100644 --- a/doc/gjk.py +++ b/doc/gjk.py @@ -2,7 +2,7 @@ import pdb import sys -# ABC = AB^AC +# 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" @@ -11,28 +11,67 @@ # 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" ] +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 +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, ] +# 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", ] +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 @@ -43,79 +82,181 @@ # 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 ], - ] + [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... - ] + [], + [], + [], + [], + [], + [], + [], + [], # [ [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], ], - [], - ] + [ + [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): + [ + [ + 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 + 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) + remaining_tests.remove(itest) next_test_values[itest] = value rerun = True while rerun: @@ -123,31 +264,40 @@ def satisfies (values, indices): 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) + k = (id - 1) if id > 0 else (-id - 1) if k in remaining_tests: - next_test_values[k] = (id > 0) + next_test_values[k] = id > 0 remaining_tests.remove(k) rerun = True else: if next_test_values[k] != (id > 0): - raise ValueError ("Absurd case") + 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) + +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): + +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 + 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): + + 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 + 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. @@ -156,69 +306,104 @@ def satisfies (values, indices): defi = definitions[case] conds = conditions[case] rejs = rejections[case] - if satisfies (test_values, defi): + if satisfies(test_values, defi): # A definition is True, stop recursion - return [ case ] - if not canSatisfy (test_values, defi): + return [case] + if not canSatisfy(test_values, defi): continue for cond in conds: - if satisfies (test_values, cond): + if satisfies(test_values, cond): # A condition is True, stop recursion - return [ case ] + return [case] append = True for rej in rejs: - if satisfies (test_values, rej): + if satisfies(test_values, rej): # A rejection is True, discard this case append = False break - if append: left_cases.append (case) + 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): + +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" + assert test_values[test] == None, "Test " + str(test) + " already performed" - left_cases = apply_test_values (cases, test_values) + left_cases = apply_test_values(cases, test_values) if len(left_cases) == 1: - return prevScore, { 'case': left_cases[0], } + 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 ]) ] } + 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') + 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 + # 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) + 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 + 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) + 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) + 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 @@ -226,126 +411,245 @@ def max_number_of_tests (current_tests, cases, test_values = [None,]*len(tests), if currentScore < bestScore: if currentScore < currentBestScore: bestScore = currentScore - bestOrder = { 'test': test, 'true': order_if_t, 'false': order_if_f } - #pdb.set_trace() + 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)) + 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=[]): +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) + 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) + 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) + 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) + 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( + 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) - print(indent+" return true;", file=file) + print("#define REGION_INSIDE() " + indent + "\\", file=file) + print(indent + " ray.setZero(); \\", file=file) + print(indent + " next.vertex[0] = current.vertex[d]; \\", file=file) + print(indent + " next.vertex[1] = current.vertex[c]; \\", file=file) + print(indent + " next.vertex[2] = current.vertex[b]; \\", file=file) + print(indent + " next.vertex[3] = current.vertex[a]; \\", file=file) + print(indent + " next.rank=4; \\", file=file) + print(indent + " return true;", file=file) print("", file=file) - if 'case' in order: - case = order['case'] + if "case" in order: + case = order["case"] if case is None: - print (indent + "// There are no case corresponding to this set of tests.", file=file) - printComments (order, indent, file) - print (indent + "assert(false);", file=file) + print( + indent + "// There are no case corresponding to this set of tests.", + file=file, + ) + printComments(order, indent, file) + print(indent + "assert(false);", file=file) return region = regions[case] - print (indent + "// Region " + region, file=file) - printComments (order, indent, file) - toFree = ['b', 'c', 'd'] + print(indent + "// Region " + region, file=file) + printComments(order, indent, file) + toFree = ["b", "c", "d"] if region == "Inside": - print (indent + "REGION_INSIDE()", file=file) + print(indent + "REGION_INSIDE()", file=file) toFree = [] - elif region == 'A': - print (indent + "originToPoint (current, a, A, next, ray);", file=file) - elif len(region)==2: + elif region == "A": + print(indent + "originToPoint (current, a, A, next, ray);", file=file) + elif len(region) == 2: a = region[0] B = region[1] - print (indent + "originToSegment (current, a, {b}, A, {B}, {B}-A, -{b}a_aa, next, ray);".format( - **{ 'b':B.lower(), 'B':B,} ), file=file) + print( + indent + + "originToSegment (current, a, {b}, A, {B}, {B}-A, -{b}a_aa, next, ray);".format( + **{ + "b": B.lower(), + "B": B, + } + ), + file=file, + ) toFree.remove(B.lower()) - elif len(region)==3: + elif len(region) == 3: B = region[1] C = region[2] - test = plane_tests[['ABC','ACD','ADB'].index(region)] - if test.startswith('-'): test = test[1:] - else: test = '-'+test - print (indent + "originToTriangle (current, a, {b}, {c}, ({B}-A).cross({C}-A), {t}, next, ray);".format( - **{'b':B.lower(), 'c':C.lower(), 'B':B, 'C':C, 't':test}), file=file) + test = plane_tests[["ABC", "ACD", "ADB"].index(region)] + if test.startswith("-"): + test = test[1:] + else: + test = "-" + test + print( + indent + + "originToTriangle (current, a, {b}, {c}, ({B}-A).cross({C}-A), {t}, next, ray);".format( + **{"b": B.lower(), "c": C.lower(), "B": B, "C": C, "t": test} + ), + file=file, + ) toFree.remove(B.lower()) toFree.remove(C.lower()) else: assert False, "Unknown region " + region for pt in toFree: - print (indent + "free_v[nfree++] = current.vertex[{}];".format(pt), file=file) + print( + indent + "free_v[nfree++] = current.vertex[{}];".format(pt), file=file + ) else: - assert "test" in order and 'true' in order and 'false' in order - check = checks[order['test']] - check_hr = checks_hr[order['test']] - printComments (order, indent, file) - nextTests_t=curTests+["a"+str(order['test']+1),] - nextTests_f=curTests+["!a"+str(order['test']+1),] - if order['true'] is None: - if order['false'] is None: - print (indent + """assert(false && "Case {} should never happen.");""".format(check_hr)) + assert "test" in order and "true" in order and "false" in order + check = checks[order["test"]] + check_hr = checks_hr[order["test"]] + printComments(order, indent, file) + nextTests_t = curTests + [ + "a" + str(order["test"] + 1), + ] + nextTests_f = curTests + [ + "!a" + str(order["test"] + 1), + ] + if order["true"] is None: + if order["false"] is None: + print( + indent + + """assert(false && "Case {} should never happen.");""".format( + check_hr + ) + ) else: - print (indent + "assert(!({} <= 0)); // Not {} / {}".format(check, check_hr, ".".join(nextTests_f)), file=file) - printOrder (order['false'], indent=indent, start=False, file=file, curTests=nextTests_f) - elif order['false'] is None: - print (indent + "assert({} <= 0); // {} / {}".format(check, check_hr, ".".join(nextTests_t)), file=file) - printOrder (order['true'], indent=indent, start=False, file=file, curTests=nextTests_t) + print( + indent + + "assert(!({} <= 0)); // Not {} / {}".format( + check, check_hr, ".".join(nextTests_f) + ), + file=file, + ) + printOrder( + order["false"], + indent=indent, + start=False, + file=file, + curTests=nextTests_f, + ) + elif order["false"] is None: + print( + indent + + "assert({} <= 0); // {} / {}".format( + check, check_hr, ".".join(nextTests_t) + ), + file=file, + ) + printOrder( + order["true"], + indent=indent, + start=False, + file=file, + curTests=nextTests_t, + ) else: - print (indent + "if ({} <= 0) {{ // if {} / {}".format(check, check_hr, ".".join(nextTests_t)), file=file) - printOrder (order['true'], indent=indent+" ", start=False, file=file, curTests=nextTests_t) - print (indent + "}} else {{ // not {} / {}".format(check_hr, ".".join(nextTests_f)), file=file) - printOrder (order['false'], indent=indent+" ", start=False, file=file, curTests=nextTests_f) - print (indent + "}} // end of {}".format(check_hr), file=file) + print( + indent + + "if ({} <= 0) {{ // if {} / {}".format( + check, check_hr, ".".join(nextTests_t) + ), + file=file, + ) + printOrder( + order["true"], + indent=indent + " ", + start=False, + file=file, + curTests=nextTests_t, + ) + print( + indent + + "}} else {{ // not {} / {}".format(check_hr, ".".join(nextTests_f)), + file=file, + ) + printOrder( + order["false"], + indent=indent + " ", + start=False, + file=file, + curTests=nextTests_f, + ) + print(indent + "}} // end of {}".format(check_hr), file=file) if start: - print ("",file=file) - print ("#undef REGION_INSIDE", file=file) - print (indent+"return false;", file=file) - print ("}", file=file) + print("", file=file) + print("#undef REGION_INSIDE", file=file) + print(indent + "return false;", file=file) + print("}", file=file) -def unit_tests (): + +def unit_tests(): # a4, a5, a10, a11, a12 cases = list(range(len(regions))) pdb.set_trace() - left_cases = apply_test_values (cases, - test_values=[None,None,None,True,True,None,None,None,None,True,True,True]) + left_cases = apply_test_values( + cases, + test_values=[ + None, + None, + None, + True, + True, + None, + None, + None, + None, + True, + True, + True, + ], + ) assert len(left_cases) > 1 -#unit_tests() -score, order = max_number_of_tests (tests, cases) +# unit_tests() + +score, order = max_number_of_tests(tests, cases) print(score) printOrder(order, indent=" ") diff --git a/doc/python/doxygen-boost.hh b/doc/python/doxygen-boost.hh index 6bca94626..689170883 100644 --- a/doc/python/doxygen-boost.hh +++ b/doc/python/doxygen-boost.hh @@ -2,40 +2,39 @@ #define DOXYGEN_BOOST_DOC_HH #ifndef DOXYGEN_DOC_HH -# error "You should have included doxygen.hh first." -#endif // DOXYGEN_DOC_HH +#error "You should have included doxygen.hh first." +#endif // DOXYGEN_DOC_HH #include -namespace doxygen -{ +namespace doxygen { -namespace visitor -{ +namespace visitor { -template -struct member_func_impl : boost::python::def_visitor > -{ - member_func_impl(const char* n, const function_type& f) : name(n), function(f) {} +template +struct member_func_impl : boost::python::def_visitor< + member_func_impl > { + member_func_impl(const char* n, const function_type& f) + : name(n), function(f) {} - member_func_impl(const char* n, const function_type& f, policy_type p) : name(n), function(f), policy(p) {} + member_func_impl(const char* n, const function_type& f, policy_type p) + : name(n), function(f), policy(p) {} template - inline void visit(classT& c) const - { + inline void visit(classT& c) const { // Either a boost::python::keyword object or a void_ object - call (c, member_func_args(function)); + call(c, member_func_args(function)); } template - inline void call(classT& c, const boost::python::detail::keywords& args) const - { + inline void call( + classT& c, const boost::python::detail::keywords& args) const { c.def(name, function, member_func_doc(function), args, policy); } template - inline void call(classT& c, const void_&) const - { + inline void call(classT& c, const void_&) const { c.def(name, function, member_func_doc(function), policy); } @@ -47,67 +46,67 @@ struct member_func_impl : boost::python::def_visitor -inline member_func_impl member_func (const char* name, const function_type& function) -{ +inline member_func_impl member_func( + const char* name, const function_type& function) { return member_func_impl(name, function); } template -inline member_func_impl member_func (const char* name, const function_type& function, const policy_type& policy) -{ +inline member_func_impl member_func( + const char* name, const function_type& function, + const policy_type& policy) { return member_func_impl(name, function, policy); } -#define DOXYGEN_DOC_DECLARE_INIT_VISITOR(z,nargs,unused) \ -template < \ - typename Class \ - BOOST_PP_COMMA_IF(nargs) \ - BOOST_PP_ENUM_PARAMS(nargs, class Arg)> \ -struct init_##nargs##_impl : boost::python::def_visitor > { \ - typedef constructor_##nargs##_impl constructor; \ - typedef boost::python::init init_base; \ - \ - template \ - inline void visit(classT& c) const { call (c, constructor::args()); } \ - \ - template \ - void call(classT& c, const boost::python::detail::keywords& args) \ - const \ - { \ - c.def(init_base(constructor::doc(), args)); \ - } \ - \ - template \ - void call(classT& c, const void_&) const \ - { \ - c.def(init_base(constructor::doc())); \ - } \ -}; \ - \ -template \ -inline init_##nargs##_impl init () \ -{ \ - return init_##nargs##_impl(); \ -} +#define DOXYGEN_DOC_DECLARE_INIT_VISITOR(z, nargs, unused) \ + template \ + struct init_##nargs##_impl \ + : boost::python::def_visitor< \ + init_##nargs##_impl > { \ + typedef constructor_##nargs##_impl \ + constructor; \ + typedef boost::python::init init_base; \ + \ + template \ + inline void visit(classT& c) const { \ + call(c, constructor::args()); \ + } \ + \ + template \ + void call(classT& c, \ + const boost::python::detail::keywords& args) const { \ + c.def(init_base(constructor::doc(), args)); \ + } \ + \ + template \ + void call(classT& c, const void_&) const { \ + c.def(init_base(constructor::doc())); \ + } \ + }; \ + \ + template \ + inline init_##nargs##_impl \ + init() { \ + return init_##nargs##_impl(); \ + } -BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR, DOXYGEN_DOC_DECLARE_INIT_VISITOR, ~) +BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR, + DOXYGEN_DOC_DECLARE_INIT_VISITOR, ~) #undef DOXYGEN_DOC_DECLARE_INIT_VISITOR -} // namespace visitor +} // namespace visitor -template -void def(const char* name, Func func) -{ +template +void def(const char* name, Func func) { boost::python::def(name, func, member_func_doc(func)); } -} // namespace doxygen +} // namespace doxygen -#endif // DOXYGEN_BOOST_DOC_HH +#endif // DOXYGEN_BOOST_DOC_HH diff --git a/doc/python/doxygen.hh b/doc/python/doxygen.hh index 1b7e79b23..2aa8258f2 100644 --- a/doc/python/doxygen.hh +++ b/doc/python/doxygen.hh @@ -10,76 +10,52 @@ #define DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR 10 #endif -namespace doxygen -{ +namespace doxygen { typedef boost::mpl::void_ void_; template -struct class_doc_impl -{ -static inline const char* run () -{ - return ""; -} -static inline const char* attribute (const char*) -{ - return ""; -} +struct class_doc_impl { + static inline const char* run() { return ""; } + static inline const char* attribute(const char*) { return ""; } }; template -inline const char* class_doc () -{ +inline const char* class_doc() { return class_doc_impl<_class>::run(); } template -inline const char* class_attrib_doc (const char* name) -{ +inline const char* class_attrib_doc(const char* name) { return class_doc_impl<_class>::attribute(name); } template -inline const char* member_func_doc (FuncPtr) -{ +inline const char* member_func_doc(FuncPtr) { return ""; } template -inline void_ member_func_args (FuncPtr) -{ - return void_(); +inline void_ member_func_args(FuncPtr) { + return void_(); } -#define DOXYGEN_DOC_DECLARE_CONSTRUCTOR(z,nargs,unused) \ -template < \ - typename Class \ - BOOST_PP_COMMA_IF(nargs) \ - BOOST_PP_ENUM_PARAMS(nargs, class Arg)> \ -struct constructor_##nargs##_impl { \ -static inline const char* doc () \ -{ \ - return ""; \ -} \ -static inline void_ args () \ -{ \ - return void_(); \ -} \ -}; \ - \ -template < \ - typename Class \ - BOOST_PP_COMMA_IF(nargs) \ - BOOST_PP_ENUM_PARAMS(nargs, class Arg)> \ -inline const char* constructor_doc () \ -{ \ - return constructor_##nargs##_impl< \ - Class \ - BOOST_PP_COMMA_IF(nargs) \ - BOOST_PP_ENUM_PARAMS(nargs, Arg)>::doc(); \ -} - -BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR, DOXYGEN_DOC_DECLARE_CONSTRUCTOR, ~) +#define DOXYGEN_DOC_DECLARE_CONSTRUCTOR(z, nargs, unused) \ + template \ + struct constructor_##nargs##_impl { \ + static inline const char* doc() { return ""; } \ + static inline void_ args() { return void_(); } \ + }; \ + \ + template \ + inline const char* constructor_doc() { \ + return constructor_##nargs##_impl::doc(); \ + } + +BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR, + DOXYGEN_DOC_DECLARE_CONSTRUCTOR, ~) #undef DOXYGEN_DOC_DECLARE_CONSTRUCTOR /* template @@ -90,17 +66,12 @@ inline const char* constructor_doc () */ template -struct destructor_doc_impl -{ -static inline const char* run () -{ - return ""; -} +struct destructor_doc_impl { + static inline const char* run() { return ""; } }; template -inline const char* destructor_doc () -{ +inline const char* destructor_doc() { return destructor_doc_impl::run(); } @@ -116,6 +87,6 @@ const char* attribute_doc (AttributeType Class::* ptr) } */ -} // namespace doxygen +} // namespace doxygen -#endif // DOXYGEN_DOC_HH +#endif // DOXYGEN_DOC_HH diff --git a/doc/python/doxygen_xml_parser.py b/doc/python/doxygen_xml_parser.py index 93f7013af..3c5b0e3dc 100755 --- a/doc/python/doxygen_xml_parser.py +++ b/doc/python/doxygen_xml_parser.py @@ -6,19 +6,16 @@ from xml_docstring import XmlDocString import sys -template_file_header = \ -"""#ifndef DOXYGEN_AUTODOC_{header_guard} +template_file_header = """#ifndef DOXYGEN_AUTODOC_{header_guard} #define DOXYGEN_AUTODOC_{header_guard} #include "{path}/doxygen.hh" """ -template_file_footer = \ -""" +template_file_footer = """ #endif // DOXYGEN_AUTODOC_{header_guard} """ -template_class_doc = \ -""" +template_class_doc = """ template <{tplargs}> struct class_doc_impl< {classname} > {{ @@ -32,12 +29,10 @@ return ""; }} }};""" -template_class_attribute_body = \ -""" +template_class_attribute_body = """ if (strcmp(attrib, "{attribute}") == 0) return "{docstring}";""" -template_constructor_doc = \ -""" +template_constructor_doc = """ template <{tplargs}> struct constructor_{nargs}_impl< {classname_prefix}{comma}{argsstring} > {{ @@ -50,8 +45,7 @@ return ({argnamesstring}); }} }};""" -template_destructor_doc = \ -""" +template_destructor_doc = """ template <{tplargs}> struct destructor_doc_impl < {classname_prefix} > {{ @@ -60,93 +54,95 @@ return "{docstring}"; }} }};""" -template_member_func_doc = \ -""" +template_member_func_doc = """ {template}inline const char* member_func_doc ({rettype} ({classname_prefix}*function_ptr) {argsstring}) {{{body} return ""; }}""" -template_member_func_doc_body = \ -""" +template_member_func_doc_body = """ if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername})) return "{docstring}";""" -template_member_func_args = \ -""" +template_member_func_args = """ {template}inline boost::python::detail::keywords<{n}> member_func_args ({rettype} ({classname_prefix}*function_ptr) {argsstring}) {{{body} return ({default_args}); }}""" -template_member_func_args_body = \ -""" +template_member_func_args_body = """ if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername})) return ({args});""" -template_static_func_doc = \ -""" +template_static_func_doc = """ {template}inline const char* member_func_doc ({rettype} (*function_ptr) {argsstring}) {{{body} return ""; }}""" -template_static_func_doc_body = \ -""" +template_static_func_doc_body = """ if (function_ptr == static_cast<{rettype} (*) {argsstring}>(&{namespace}::{membername})) return "{docstring}";""" -template_open_namespace = \ -"""namespace {namespace} {{""" -template_close_namespace = \ -"""}} // namespace {namespace}""" -template_include_intern = \ -"""#include +template_open_namespace = """namespace {namespace} {{""" +template_close_namespace = """}} // namespace {namespace}""" +template_include_intern = """#include """ -template_include_extern = \ -"""#include <{filename}> +template_include_extern = """#include <{filename}> """ -def _templateParamToDict (param): - type = param.find('type') - declname = param.find('declname') - defname = param.find('defname') + +def _templateParamToDict(param): + type = param.find("type") + declname = param.find("declname") + defname = param.find("defname") # FIXME type may contain references in two ways: # - the real param type # - the name of the template argument is recognized as the name of a type... if defname is None and declname is None: typetext = type.text for c in type.iter(): - if c == type: continue - if c.text is not None: typetext += c.text - if c.tail is not None: typetext += c.tail - if typetext.startswith ("typename") or typetext.startswith ("class"): + if c == type: + continue + if c.text is not None: + typetext += c.text + if c.tail is not None: + typetext += c.tail + if typetext.startswith("typename") or typetext.startswith("class"): if sys.version_info.major == 2: s = typetext.split() - return { "type": s[0].strip(), "name": typetext[len(s[0]):].strip() } + return {"type": s[0].strip(), "name": typetext[len(s[0]) :].strip()} else: s = typetext.split(maxsplit=1) assert len(s) == 2 - return { "type": s[0].strip(), "name": s[1].strip() } + return {"type": s[0].strip(), "name": s[1].strip()} else: - return { "type": type.text, "name": "" } + return {"type": type.text, "name": ""} else: assert defname.text == declname.text - return { "type": type.text, "name": defname.text } + return {"type": type.text, "name": defname.text} + -def makeHeaderGuard (filename): +def makeHeaderGuard(filename): import os - return filename.upper().replace('.', '_').replace('/', '_').replace('-', '_') -def format_description (brief, detailed): - b = [ el.text.strip() for el in brief .iter() if el.text ] if brief is not None else [] - d = [ el.text.strip() for el in detailed.iter() if el.text ] if detailed is not None else [] + return filename.upper().replace(".", "_").replace("/", "_").replace("-", "_") + + +def format_description(brief, detailed): + b = [el.text.strip() for el in brief.iter() if el.text] if brief is not None else [] + d = ( + [el.text.strip() for el in detailed.iter() if el.text] + if detailed is not None + else [] + ) text = "".join(b) if d: - text += '\n' + "".join(d) + text += "\n" + "".join(d) return text + class Reference(object): - def __init__ (self, index, id=None, name=None): + def __init__(self, index, id=None, name=None): self.id = id self.name = name self.index = index - def xmlToType (self, node, array=None, parentClass=None, tplargs=None): + def xmlToType(self, node, array=None, parentClass=None, tplargs=None): """ - node: - parentClass: a class @@ -163,23 +159,29 @@ def xmlToType (self, node, array=None, parentClass=None, tplargs=None): refid = c.attrib["refid"] if parentClass is not None and refid == parentClass.id: t += " " + parentClass.name - if c.tail is not None and c.tail.lstrip()[0] != '<': + if c.tail is not None and c.tail.lstrip()[0] != "<": if tplargs is not None: t += tplargs - elif parentClass is not None and isinstance(parentClass,ClassCompound) and parentClass.hasTypeDef(c.text.strip()): + elif ( + parentClass is not None + and isinstance(parentClass, ClassCompound) + and parentClass.hasTypeDef(c.text.strip()) + ): parent_has_templates = len(parentClass.template_params) > 0 if parent_has_templates: t += " typename " + parentClass._className() + "::" else: t += " " + parentClass._className() + "::" - self_has_templates = c.tail is not None and c.tail.strip().find('<') != -1 + self_has_templates = ( + c.tail is not None and c.tail.strip().find("<") != -1 + ) if self_has_templates: t += " template " - t += c.text.strip() + t += c.text.strip() elif self.index.hasref(refid): t += " " + self.index.getref(refid).name else: - self.index.output.warn ("Unknown reference: ", c.text, refid) + self.index.output.warn("Unknown reference: ", c.text, refid) t += " " + c.text.strip() else: if c.text is not None: @@ -190,109 +192,161 @@ def xmlToType (self, node, array=None, parentClass=None, tplargs=None): t += array.text return t + # Only for function as of now. class MemberDef(Reference): - def __init__ (self, index, memberdefxml, parent): - super(MemberDef, self).__init__ (index=index, - id = memberdefxml.attrib["id"], - name = memberdefxml.find("definition").text) + def __init__(self, index, memberdefxml, parent): + super(MemberDef, self).__init__( + index=index, + id=memberdefxml.attrib["id"], + name=memberdefxml.find("definition").text, + ) self.parent = parent self.xml = memberdefxml - self.const = (memberdefxml.attrib['const']=="yes") - self.static = (memberdefxml.attrib['static']=="yes") - self.rettype = memberdefxml.find('type') - self.params = tuple( [ (param.find('type'), param.find('declname'), param.find('array')) for param in self.xml.findall("param") ] ) - self.special = self.rettype.text is None and len(self.rettype.getchildren())==0 - #assert self.special or len(self.rettype.text) > 0 - - self._templateParams (self.xml.find('templateparamlist')) - - def _templateParams (self, tpl): + self.const = memberdefxml.attrib["const"] == "yes" + self.static = memberdefxml.attrib["static"] == "yes" + self.rettype = memberdefxml.find("type") + self.params = tuple( + [ + (param.find("type"), param.find("declname"), param.find("array")) + for param in self.xml.findall("param") + ] + ) + self.special = ( + self.rettype.text is None and len(self.rettype.getchildren()) == 0 + ) + # assert self.special or len(self.rettype.text) > 0 + + self._templateParams(self.xml.find("templateparamlist")) + + def _templateParams(self, tpl): if tpl is not None: - self.template_params = tuple ([ _templateParamToDict(param) for param in tpl.iterchildren(tag="param") ]) + self.template_params = tuple( + [_templateParamToDict(param) for param in tpl.iterchildren(tag="param")] + ) else: self.template_params = tuple() - def prototypekey (self): + def prototypekey(self): prototype = ( - self.xmlToType(self.rettype, parentClass=self.parent), - tuple( [ tuple(t.items()) for t in self.template_params ]), - tuple( [ self.xmlToType(param.find('type'), parentClass=self.parent) for param in self.xml.findall("param") ] ), - self.const, - ) + self.xmlToType(self.rettype, parentClass=self.parent), + tuple([tuple(t.items()) for t in self.template_params]), + tuple( + [ + self.xmlToType(param.find("type"), parentClass=self.parent) + for param in self.xml.findall("param") + ] + ), + self.const, + ) return prototype - def s_prototypeArgs (self): - return "({0}){1}".format (self.s_args(), " const" if self.const else "") + def s_prototypeArgs(self): + return "({0}){1}".format(self.s_args(), " const" if self.const else "") - def s_args (self): + def s_args(self): # If the class is templated, check if one of the argument is the class itself. # If so, we must add the template arguments to the class (if there is none) if len(self.parent.template_params) > 0: - tplargs = " <" + ", ".join([ d['name'] for d in self.parent.template_params ]) + " > " + tplargs = ( + " <" + + ", ".join([d["name"] for d in self.parent.template_params]) + + " > " + ) args = ", ".join( - [ self.xmlToType(type, array, parentClass=self.parent, tplargs=tplargs) for type,declname,array in self.params]) + [ + self.xmlToType( + type, array, parentClass=self.parent, tplargs=tplargs + ) + for type, declname, array in self.params + ] + ) else: - args = ", ".join([ self.xmlToType(type, array, parentClass=self.parent) for type,declname,array in self.params]) + args = ", ".join( + [ + self.xmlToType(type, array, parentClass=self.parent) + for type, declname, array in self.params + ] + ) return args - def s_tpldecl (self): - if len(self.template_params) == 0: return "" - return ", ".join([ d['type'] + " " + d['name'] for d in self.template_params ]) - - def s_rettype (self): - assert not self.special, "Member {} ({}) is a special function and no return type".format(self.name, self.id) + def s_tpldecl(self): + if len(self.template_params) == 0: + return "" + return ", ".join([d["type"] + " " + d["name"] for d in self.template_params]) + + def s_rettype(self): + assert ( + not self.special + ), "Member {} ({}) is a special function and no return type".format( + self.name, self.id + ) if len(self.parent.template_params) > 0: - tplargs = " <" + ", ".join([ d['name'] for d in self.parent.template_params ]) + " > " + tplargs = ( + " <" + + ", ".join([d["name"] for d in self.parent.template_params]) + + " > " + ) else: tplargs = None - if isinstance(self.parent, ClassCompound): - return self.xmlToType(self.rettype, parentClass=self.parent, tplargs=tplargs) + if isinstance(self.parent, ClassCompound): + return self.xmlToType( + self.rettype, parentClass=self.parent, tplargs=tplargs + ) else: return self.xmlToType(self.rettype) - def s_name (self): - return self.xml.find('name').text.strip() + def s_name(self): + return self.xml.find("name").text.strip() - def s_docstring (self): - return self.index.xml_docstring.getDocString ( - self.xml.find('briefdescription'), - self.xml.find('detaileddescription'), - self.index.output) + def s_docstring(self): + return self.index.xml_docstring.getDocString( + self.xml.find("briefdescription"), + self.xml.find("detaileddescription"), + self.index.output, + ) - def n_args (self): + def n_args(self): return len(self.params) - def s_argnamesstring (self): - def getdeclname(i,declname): + def s_argnamesstring(self): + def getdeclname(i, declname): if declname is None or declname.text is None or declname.text.strip() == "": return "arg{}".format(i) return declname.text.strip() + arg = """boost::python::arg("{}")""" - argnames = ["self", ] + [ getdeclname(i, declname) for i,(_,declname,_) in enumerate(self.params)] - return ", ".join([ arg.format(n) for n in argnames ]) + argnames = [ + "self", + ] + [getdeclname(i, declname) for i, (_, declname, _) in enumerate(self.params)] + return ", ".join([arg.format(n) for n in argnames]) - def include (self): + def include(self): import os.path - loc = self.xml.find('location') + + loc = self.xml.find("location") # The location is based on $CMAKE_SOURCE_DIR. Remove first directory. - return loc.attrib['file'].split('/',1)[1] + return loc.attrib["file"].split("/", 1)[1] + class CompoundBase(Reference): - def __init__ (self, compound, index): + def __init__(self, compound, index): self.compound = compound - self.filename = path.join (index.directory, compound.attrib["refid"]+".xml") - self.tree = etree.parse (self.filename) + self.filename = path.join(index.directory, compound.attrib["refid"] + ".xml") + self.tree = etree.parse(self.filename) self.definition = self.tree.getroot().find("compounddef") - super(CompoundBase, self).__init__ (index, - id = self.definition.attrib['id'], - name = self.definition.find("compoundname").text) + super(CompoundBase, self).__init__( + index, + id=self.definition.attrib["id"], + name=self.definition.find("compoundname").text, + ) + -class NamespaceCompound (CompoundBase): - def __init__ (self, *args): - super(NamespaceCompound, self).__init__ (*args) +class NamespaceCompound(CompoundBase): + def __init__(self, *args): + super(NamespaceCompound, self).__init__(*args) self.typedefs = [] self.enums = [] self.static_funcs = [] @@ -303,149 +357,181 @@ def __init__ (self, *args): assert "kind" in section.attrib kind = section.attrib["kind"] if kind == "enum": - self.parseEnumSection (section) + self.parseEnumSection(section) elif kind == "typedef": - self.parseTypedefSection (section) + self.parseTypedefSection(section) elif kind == "func": - self.parseFuncSection (section) + self.parseFuncSection(section) - def parseEnumSection (self, section): + def parseEnumSection(self, section): for member in section.iterchildren("memberdef"): - ref = Reference (index=self.index, - id=member.attrib["id"], - name= self.name + "::" + member.find("name").text) - self.index.registerReference (ref) + ref = Reference( + index=self.index, + id=member.attrib["id"], + name=self.name + "::" + member.find("name").text, + ) + self.index.registerReference(ref) self.enums.append(member) for value in member.iterchildren("enumvalue"): - ref = Reference (index=self.index, - id=value.attrib["id"], - name= self.name + "::" + member.find("name").text) + ref = Reference( + index=self.index, + id=value.attrib["id"], + name=self.name + "::" + member.find("name").text, + ) - def parseTypedefSection (self, section): + def parseTypedefSection(self, section): for member in section.iterchildren("memberdef"): - ref = Reference (index=self.index, - id=member.attrib["id"], - name= self.name + "::" + member.find("name").text) - self.index.registerReference (ref) + ref = Reference( + index=self.index, + id=member.attrib["id"], + name=self.name + "::" + member.find("name").text, + ) + self.index.registerReference(ref) self.typedefs.append(member) - def parseFuncSection (self, section): + def parseFuncSection(self, section): for member in section.iterchildren("memberdef"): - self.static_funcs.append (MemberDef (self.index, member, self)) + self.static_funcs.append(MemberDef(self.index, member, self)) - def innerNamespace (self): + def innerNamespace(self): return self.name - def write (self, output): + def write(self, output): pass -class ClassCompound (CompoundBase): - def __init__ (self, *args): - super(ClassCompound, self).__init__ (*args) + +class ClassCompound(CompoundBase): + def __init__(self, *args): + super(ClassCompound, self).__init__(*args) self.member_funcs = list() self.static_funcs = list() self.special_funcs = list() self.attributes = list() - self.struct = (self.compound.attrib['kind'] == "struct") - self.public = (self.definition.attrib['prot'] == "public") - self.template_specialization = (self.name.find('<') > 0) + self.struct = self.compound.attrib["kind"] == "struct" + self.public = self.definition.attrib["prot"] == "public" + self.template_specialization = self.name.find("<") > 0 self.typedef = dict() # Handle templates - self._templateParams (self.definition.find('templateparamlist')) + self._templateParams(self.definition.find("templateparamlist")) for memberdef in self.definition.iter(tag="memberdef"): - if memberdef.attrib['prot'] != "public": + if memberdef.attrib["prot"] != "public": continue - if memberdef.attrib['kind'] == "variable": - self._attribute (memberdef) - elif memberdef.attrib['kind'] == "typedef": - ref = Reference (index=self.index, - id=memberdef.attrib["id"], - name= self._className() + "::" + memberdef.find("name").text) - self.index.registerReference (ref) + if memberdef.attrib["kind"] == "variable": + self._attribute(memberdef) + elif memberdef.attrib["kind"] == "typedef": + ref = Reference( + index=self.index, + id=memberdef.attrib["id"], + name=self._className() + "::" + memberdef.find("name").text, + ) + self.index.registerReference(ref) self.typedef[memberdef.find("name").text.strip()] = True - elif memberdef.attrib['kind'] == "enum": - ref = Reference (index=self.index, - id=memberdef.attrib["id"], - name= self._className()+ "::" + memberdef.find("name").text) - self.index.registerReference (ref) + elif memberdef.attrib["kind"] == "enum": + ref = Reference( + index=self.index, + id=memberdef.attrib["id"], + name=self._className() + "::" + memberdef.find("name").text, + ) + self.index.registerReference(ref) for value in memberdef.iterchildren("enumvalue"): - ref = Reference (index=self.index, - id=value.attrib["id"], - name= self._className() + "::" + memberdef.find("name").text) - self.index.registerReference (ref) - elif memberdef.attrib['kind'] == "function": - self._memberfunc (memberdef) - - def _templateParams (self, tpl): + ref = Reference( + index=self.index, + id=value.attrib["id"], + name=self._className() + "::" + memberdef.find("name").text, + ) + self.index.registerReference(ref) + elif memberdef.attrib["kind"] == "function": + self._memberfunc(memberdef) + + def _templateParams(self, tpl): if tpl is not None: - self.template_params = tuple([ _templateParamToDict(param) for param in tpl.iterchildren(tag="param") ]) + self.template_params = tuple( + [_templateParamToDict(param) for param in tpl.iterchildren(tag="param")] + ) else: self.template_params = tuple() - def _templateDecl (self): + def _templateDecl(self): if not hasattr(self, "template_params") or len(self.template_params) == 0: return "" - return ", ".join([ d['type'] + " " + d['name'] for d in self.template_params ]) + return ", ".join([d["type"] + " " + d["name"] for d in self.template_params]) - def _className (self): + def _className(self): if not hasattr(self, "template_params") or len(self.template_params) == 0: return self.name - return self.name + " <" + ", ".join([ d['name'] for d in self.template_params ]) + " >" + return ( + self.name + + " <" + + ", ".join([d["name"] for d in self.template_params]) + + " >" + ) def hasTypeDef(self, typename): return typename in self.typedef - def innerNamespace (self): + + def innerNamespace(self): return self._className() - def _memberfunc (self, member): - m = MemberDef (self.index, member, self) + def _memberfunc(self, member): + m = MemberDef(self.index, member, self) if m.special: - self.special_funcs.append (m) + self.special_funcs.append(m) elif m.static: - self.static_funcs.append (m) + self.static_funcs.append(m) else: - self.member_funcs.append (m) - - def _writeClassDoc (self, output): - docstring = self.index.xml_docstring.getDocString ( - self.definition.find('briefdescription'), - self.definition.find('detaileddescription'), - self.index.output) + self.member_funcs.append(m) + + def _writeClassDoc(self, output): + docstring = self.index.xml_docstring.getDocString( + self.definition.find("briefdescription"), + self.definition.find("detaileddescription"), + self.index.output, + ) attribute_docstrings = "" for member in self.attributes: _dc = self.index.xml_docstring.getDocString( - member.find('briefdescription'), - member.find('detaileddescription'), - self.index.output) - if len(_dc) == 0: continue - attribute_docstrings += template_class_attribute_body.format ( - attribute = member.find('name').text, - docstring = _dc, - ) - if len(docstring) == 0 and len(attribute_docstrings) == 0: return - output.out (template_class_doc.format ( - tplargs = self._templateDecl(), - classname = self._className(), - docstring = docstring, - attributes = attribute_docstrings, - )) - - def write (self, output): - if not self.public: return + member.find("briefdescription"), + member.find("detaileddescription"), + self.index.output, + ) + if len(_dc) == 0: + continue + attribute_docstrings += template_class_attribute_body.format( + attribute=member.find("name").text, + docstring=_dc, + ) + if len(docstring) == 0 and len(attribute_docstrings) == 0: + return + output.out( + template_class_doc.format( + tplargs=self._templateDecl(), + classname=self._className(), + docstring=docstring, + attributes=attribute_docstrings, + ) + ) + + def write(self, output): + if not self.public: + return if self.template_specialization: - output.warn ("Disable class {} because template argument are not resolved for templated class specialization.".format(self.name)) + output.warn( + "Disable class {} because template argument are not resolved for templated class specialization.".format( + self.name + ) + ) return - include = self.definition.find('includes') + include = self.definition.find("includes") if include is None: output.err("Does not know where to write doc of", self.name) return - output.open (include.text) - output.out (template_include_extern.format (filename=include.text)) - output.out (template_open_namespace.format (namespace="doxygen")) + output.open(include.text) + output.out(template_include_extern.format(filename=include.text)) + output.out(template_open_namespace.format(namespace="doxygen")) # Write class doc self._writeClassDoc(output) @@ -455,32 +541,44 @@ def write (self, output): for m in self.member_funcs: prototype = m.prototypekey() if prototype in member_funcs: - member_funcs[prototype].append (m) + member_funcs[prototype].append(m) else: - member_funcs[prototype] = [ m, ] + member_funcs[prototype] = [ + m, + ] classname_prefix = self._className() + "::" for member in self.special_funcs: docstring = member.s_docstring() argnamesstring = member.s_argnamesstring() - if len(docstring) == 0 and len(argnamesstring) == 0: continue - if member.s_name()[0] == '~': - output.out (template_destructor_doc.format ( - tplargs = self._templateDecl(), - classname_prefix = self._className(), - docstring = docstring, - )) + if len(docstring) == 0 and len(argnamesstring) == 0: + continue + if member.s_name()[0] == "~": + output.out( + template_destructor_doc.format( + tplargs=self._templateDecl(), + classname_prefix=self._className(), + docstring=docstring, + ) + ) else: - output.out (template_constructor_doc.format ( - tplargs = ", ".join([ d['type'] + " " + d['name'] for d in self.template_params + member.template_params ]), - nargs = len(member.params), - comma = ", " if len(member.params) > 0 else "", - classname_prefix = self._className(), - argsstring = member.s_args(), - docstring = docstring, - argnamesstring = argnamesstring, - )) + output.out( + template_constructor_doc.format( + tplargs=", ".join( + [ + d["type"] + " " + d["name"] + for d in self.template_params + member.template_params + ] + ), + nargs=len(member.params), + comma=", " if len(member.params) > 0 else "", + classname_prefix=self._className(), + argsstring=member.s_args(), + docstring=docstring, + argnamesstring=argnamesstring, + ) + ) for prototype, members in member_funcs.items(): # remove undocumented members @@ -490,94 +588,122 @@ def write (self, output): for member in members: docstring = member.s_docstring() argnamesstring = member.s_argnamesstring() - if len(docstring) == 0 and len(argnamesstring) == 0: continue - documented_members.append (member) - docstrings.append (docstring) - argnamesstrings.append (argnamesstring) - if len(documented_members) == 0: continue + if len(docstring) == 0 and len(argnamesstring) == 0: + continue + documented_members.append(member) + docstrings.append(docstring) + argnamesstrings.append(argnamesstring) + if len(documented_members) == 0: + continue # Write docstrings - body = "".join([ - template_member_func_doc_body.format ( - classname_prefix = classname_prefix, - membername = member.s_name(), - docstring = docstring, - rettype = member.s_rettype(), - argsstring = member.s_prototypeArgs(), + body = "".join( + [ + template_member_func_doc_body.format( + classname_prefix=classname_prefix, + membername=member.s_name(), + docstring=docstring, + rettype=member.s_rettype(), + argsstring=member.s_prototypeArgs(), ) - for member, docstring in zip(documented_members,docstrings) ]) + for member, docstring in zip(documented_members, docstrings) + ] + ) member = members[0] - tplargs = ", ".join([ d['type'] + " " + d['name'] for d in self.template_params + member.template_params ]) - output.out (template_member_func_doc.format ( - template = "template <{}>\n".format (tplargs) if len(tplargs) > 0 else "", - rettype = member.s_rettype(), - classname_prefix = classname_prefix, - argsstring = member.s_prototypeArgs(), - body = body - )) + tplargs = ", ".join( + [ + d["type"] + " " + d["name"] + for d in self.template_params + member.template_params + ] + ) + output.out( + template_member_func_doc.format( + template="template <{}>\n".format(tplargs) + if len(tplargs) > 0 + else "", + rettype=member.s_rettype(), + classname_prefix=classname_prefix, + argsstring=member.s_prototypeArgs(), + body=body, + ) + ) # Write argnamesstrings - body = "".join([ - template_member_func_args_body.format ( - classname_prefix = classname_prefix, - membername = member.s_name(), - args = argnamesstring, - rettype = member.s_rettype(), - argsstring = member.s_prototypeArgs(), + body = "".join( + [ + template_member_func_args_body.format( + classname_prefix=classname_prefix, + membername=member.s_name(), + args=argnamesstring, + rettype=member.s_rettype(), + argsstring=member.s_prototypeArgs(), + ) + for member, argnamesstring in zip( + documented_members, argnamesstrings ) - for member, argnamesstring in zip(documented_members,argnamesstrings) ]) + ] + ) n_args = member.n_args() - default_args = ", ".join(["""boost::python::arg("self")""", ] + - [ """boost::python::arg("arg{}")""".format(i) for i in range(n_args) ] - ) - output.out (template_member_func_args.format ( - template = "template <{}>\n".format (tplargs) if len(tplargs) > 0 else "", - rettype = member.s_rettype(), - n = n_args+1, - default_args = default_args, - classname_prefix = classname_prefix, - argsstring = member.s_prototypeArgs(), - body = body - )) - - output.out (template_close_namespace.format (namespace="doxygen")) + default_args = ", ".join( + [ + """boost::python::arg("self")""", + ] + + ["""boost::python::arg("arg{}")""".format(i) for i in range(n_args)] + ) + output.out( + template_member_func_args.format( + template="template <{}>\n".format(tplargs) + if len(tplargs) > 0 + else "", + rettype=member.s_rettype(), + n=n_args + 1, + default_args=default_args, + classname_prefix=classname_prefix, + argsstring=member.s_prototypeArgs(), + body=body, + ) + ) + + output.out(template_close_namespace.format(namespace="doxygen")) output.close() - def _attribute (self, member): - self.attributes.append (member) + def _attribute(self, member): + self.attributes.append(member) + class Index: """ This class is responsible for generating the list of all C++-usable documented elements. """ - def __init__ (self, input, output): - self.tree = etree.parse (input) - self.directory = path.dirname (input) - self.xml_docstring = XmlDocString (self) + + def __init__(self, input, output): + self.tree = etree.parse(input) + self.directory = path.dirname(input) + self.xml_docstring = XmlDocString(self) self.compounds = list() self.references = dict() self.output = output - def parseCompound (self): - for compound in self.tree.getroot().iterchildren ("compound"): - if compound.attrib['kind'] in ["class", "struct"]: - obj = ClassCompound (compound, self) - elif compound.attrib['kind'] == "namespace": - obj = NamespaceCompound (compound, self) + def parseCompound(self): + for compound in self.tree.getroot().iterchildren("compound"): + if compound.attrib["kind"] in ["class", "struct"]: + obj = ClassCompound(compound, self) + elif compound.attrib["kind"] == "namespace": + obj = NamespaceCompound(compound, self) if obj.id not in self.compounds: - self.compounds.append (obj.id) - self.registerReference (obj) + self.compounds.append(obj.id) + self.registerReference(obj) - def write (self): + def write(self): # Header from os.path import abspath, dirname from time import asctime - self.output.open ("doxygen_xml_parser_for_cmake.hh") - #self.output.out ("// Generated on {}".format (asctime())) + self.output.open("doxygen_xml_parser_for_cmake.hh") + # self.output.out ("// Generated on {}".format (asctime())) self.output.close() # Implement template specialization for classes and member functions @@ -585,7 +711,7 @@ def write (self): compound = self.references[id] compound.write(self.output) - self.output.open ("functions.h") + self.output.open("functions.h") # Implement template specialization for static functions static_funcs = dict() @@ -596,64 +722,90 @@ def write (self): for m in compound.static_funcs: include = m.include() if include not in includes: - includes.append (include) + includes.append(include) docstring = m.s_docstring() - if len(docstring) == 0: continue + if len(docstring) == 0: + continue prototype = m.prototypekey() if prototype in static_funcs: - static_funcs[prototype].append ( (m, docstring) ) + static_funcs[prototype].append((m, docstring)) else: - static_funcs[prototype] = [ (m, docstring) , ] - prototypes.append (prototype) - - self.output.out ( - "".join([ template_include_extern.format (filename=filename) - for filename in includes])) - - self.output.out (template_open_namespace.format (namespace="doxygen")) + static_funcs[prototype] = [ + (m, docstring), + ] + prototypes.append(prototype) + + self.output.out( + "".join( + [ + template_include_extern.format(filename=filename) + for filename in includes + ] + ) + ) + + self.output.out(template_open_namespace.format(namespace="doxygen")) for prototype in prototypes: member_and_docstring_s = static_funcs[prototype] - body = "".join([ - template_static_func_doc_body.format ( - namespace = member.parent.innerNamespace(), - membername = member.s_name(), - docstring = docstring, - rettype = member.s_rettype(), - argsstring = member.s_prototypeArgs(), + body = "".join( + [ + template_static_func_doc_body.format( + namespace=member.parent.innerNamespace(), + membername=member.s_name(), + docstring=docstring, + rettype=member.s_rettype(), + argsstring=member.s_prototypeArgs(), ) - for member, docstring in member_and_docstring_s ]) + for member, docstring in member_and_docstring_s + ] + ) member = member_and_docstring_s[0][0] # TODO fix case of static method in templated class. - tplargs = ", ".join([ d['type'] + " " + d['name'] for d in member.parent.template_params + member.template_params ]) - self.output.out (template_static_func_doc.format ( - template = "template <{}>\n".format (tplargs) if len(tplargs) > 0 else "", - rettype = member.s_rettype(), - argsstring = member.s_prototypeArgs(), - body = body - )) - - self.output.out (template_close_namespace.format (namespace="doxygen")) - self.output.close () - - def registerReference (self, obj, overwrite=True): + tplargs = ", ".join( + [ + d["type"] + " " + d["name"] + for d in member.parent.template_params + member.template_params + ] + ) + self.output.out( + template_static_func_doc.format( + template="template <{}>\n".format(tplargs) + if len(tplargs) > 0 + else "", + rettype=member.s_rettype(), + argsstring=member.s_prototypeArgs(), + body=body, + ) + ) + + self.output.out(template_close_namespace.format(namespace="doxygen")) + self.output.close() + + def registerReference(self, obj, overwrite=True): if obj.id in self.references: if obj.name != self.references[obj.id].name: - self.output.warn ("!!!! Compounds " + obj.id + " already exists.", obj.name, self.references[obj.id].name) + self.output.warn( + "!!!! Compounds " + obj.id + " already exists.", + obj.name, + self.references[obj.id].name, + ) else: - self.output.warn ("Reference " + obj.id + " already exists.", obj.name) - if not overwrite: return + self.output.warn("Reference " + obj.id + " already exists.", obj.name) + if not overwrite: + return self.references[obj.id] = obj - def hasref (self, id): - return (id in self.references) + def hasref(self, id): + return id in self.references - def getref (self, id): + def getref(self, id): return self.references[id] + class OutputStreams(object): - def __init__ (self, output_dir, warn, error, errorPrefix = ""): + def __init__(self, output_dir, warn, error, errorPrefix=""): self.output_dir = output_dir self._out = None self._warn = warn @@ -662,64 +814,78 @@ def __init__ (self, output_dir, warn, error, errorPrefix = ""): self._created_files = dict() - def open (self, name): + def open(self, name): assert self._out == None, "You did not close the previous file" import os + fullname = os.path.join(self.output_dir, name) dirname = os.path.dirname(fullname) - if not os.path.isdir (dirname): os.makedirs (dirname) + if not os.path.isdir(dirname): + os.makedirs(dirname) if name in self._created_files: self._out = self._created_files[name] else: import codecs + if sys.version_info >= (3,): - encoding = 'utf-8' + encoding = "utf-8" else: - encoding = 'latin1' - self._out = codecs.open(fullname, mode='w', encoding=encoding) + encoding = "latin1" + self._out = codecs.open(fullname, mode="w", encoding=encoding) self._created_files[name] = self._out # Header - self.out(template_file_header.format ( - path = os.path.dirname(os.path.abspath(__file__)), - header_guard = makeHeaderGuard (name), - )) + self.out( + template_file_header.format( + path=os.path.dirname(os.path.abspath(__file__)), + header_guard=makeHeaderGuard(name), + ) + ) - def close (self): + def close(self): self._out = None - def writeFooterAndCloseFiles (self): + def writeFooterAndCloseFiles(self): for n, f in self._created_files.items(): # Footer self._out = f - self.out(template_file_footer.format( - header_guard = makeHeaderGuard (n), - )) + self.out( + template_file_footer.format( + header_guard=makeHeaderGuard(n), + ) + ) f.close() self._created_files.clear() self._out = None def out(self, *args): if sys.version_info >= (3,): - print (*args, file=self._out) + print(*args, file=self._out) else: - print(' '.join(str(arg) for arg in args).decode("latin1"), file=self._out) + print(" ".join(str(arg) for arg in args).decode("latin1"), file=self._out) + def warn(self, *args): - print (self.errorPrefix, *args, file=self._warn) + print(self.errorPrefix, *args, file=self._warn) + def err(self, *args): - print (self.errorPrefix, *args, file=self._err) + print(self.errorPrefix, *args, file=self._err) + if __name__ == "__main__": import argparse - parser = argparse.ArgumentParser(description='Process Doxygen XML documentation and generate C++ code.') - parser.add_argument('doxygen_index_xml', type=str, help='the Doxygen XML index.') - parser.add_argument('output_directory', type=str, help='the output directory.') + parser = argparse.ArgumentParser( + description="Process Doxygen XML documentation and generate C++ code." + ) + parser.add_argument("doxygen_index_xml", type=str, help="the Doxygen XML index.") + parser.add_argument("output_directory", type=str, help="the output directory.") args = parser.parse_args() - index = Index (input = sys.argv[1], - output = OutputStreams (args.output_directory, sys.stdout, sys.stderr)) + index = Index( + input=sys.argv[1], + output=OutputStreams(args.output_directory, sys.stdout, sys.stderr), + ) index.parseCompound() index.write() index.output.writeFooterAndCloseFiles() diff --git a/doc/python/xml_docstring.py b/doc/python/xml_docstring.py index 9709033ea..0b1fd0975 100644 --- a/doc/python/xml_docstring.py +++ b/doc/python/xml_docstring.py @@ -1,75 +1,87 @@ -class XmlDocString (object): - def __init__ (self, index): +class XmlDocString(object): + def __init__(self, index): self.index = index self.tags = { - "para": self.para, - "ref": self.ref, - "briefdescription": self.otherTags, - "detaileddescription": self.otherTags, - "parameterlist": self.parameterlist, - "parameterdescription": self.otherTags, - "emphasis": self.emphasis, - "simplesect": self.simplesect, - "formula": self.formula, - "itemizedlist": self.itemizedlist, - "listitem": self.listitem, - } + "para": self.para, + "ref": self.ref, + "briefdescription": self.otherTags, + "detaileddescription": self.otherTags, + "parameterlist": self.parameterlist, + "parameterdescription": self.otherTags, + "emphasis": self.emphasis, + "simplesect": self.simplesect, + "formula": self.formula, + "itemizedlist": self.itemizedlist, + "listitem": self.listitem, + } self.unkwownTags = set() self.unkwownReferences = dict() - self._linesep = "\\n\"\n\"" + self._linesep = '\\n"\n"' try: from pylatexenc.latex2text import LatexNodes2Text + self.latex = LatexNodes2Text() except ImportError: self.latex = None - def clear (self): + def clear(self): self.lines = [] self.unkwownTags.clear() self.unkwownReferences.clear() - def writeErrors (self, output): + def writeErrors(self, output): ret = False for t in self.unkwownTags: - output.warn ("Unknown tag: ", t) + output.warn("Unknown tag: ", t) ret = True - for ref,node in self.unkwownReferences.items(): - output.warn ("Unknown reference: ", ref, node.text) + for ref, node in self.unkwownReferences.items(): + output.warn("Unknown reference: ", ref, node.text) ret = True return ret - def _write (self, str): - nlines=str.split("\n") - if len(self.lines)==0: + def _write(self, str): + nlines = str.split("\n") + if len(self.lines) == 0: self.lines += nlines else: self.lines[-1] += nlines[0] self.lines += nlines[1:] - #self.lines += nlines[1:] + # self.lines += nlines[1:] - def _newline (self,n=1): - self.lines.extend (["",] * n) + def _newline(self, n=1): + self.lines.extend( + [ + "", + ] + * n + ) def _clean(self): s = 0 for l in self.lines: - if len(l.strip())==0: s+=1 - else: break + if len(l.strip()) == 0: + s += 1 + else: + break e = len(self.lines) for l in reversed(self.lines): - if len(l.strip())==0: e-=1 - else: break + if len(l.strip()) == 0: + e -= 1 + else: + break self.lines = self.lines[s:e] - def getDocString (self, brief, detailled, output): + def getDocString(self, brief, detailled, output): self.clear() if brief is not None: - self.visit (brief) + self.visit(brief) if detailled is not None and len(detailled.getchildren()) > 0: - if brief is not None: self._newline () - self.visit (detailled) + if brief is not None: + self._newline() + self.visit(detailled) from sys import stdout, stderr, version_info + self.writeErrors(output) self._clean() if version_info[0] == 2: @@ -77,82 +89,85 @@ def getDocString (self, brief, detailled, output): else: return self._linesep.join(self.lines) - def visit (self, node): + def visit(self, node): assert isinstance(node.tag, str) tag = node.tag if tag not in self.tags: - self.unknownTag (node) + self.unknownTag(node) else: self.tags[tag](node) - def unknownTag (self, node): - self.unkwownTags.add (node.tag) - self.otherTags (node) + def unknownTag(self, node): + self.unkwownTags.add(node.tag) + self.otherTags(node) - def otherTags (self, node): + def otherTags(self, node): if node.text: - self._write (node.text.strip().replace('"', r'\"')) + self._write(node.text.strip().replace('"', r"\"")) for c in node.iterchildren(): - self.visit (c) - if c.tail: self._write (c.tail.strip().replace('"', r'\"')) + self.visit(c) + if c.tail: + self._write(c.tail.strip().replace('"', r"\"")) - def emphasis (self, node): - self._write ("*") - self.otherTags (node) - self._write ("*") + def emphasis(self, node): + self._write("*") + self.otherTags(node) + self._write("*") - def simplesect (self, node): - self._write (node.attrib["kind"].title()+": ") - self.otherTags (node) + def simplesect(self, node): + self._write(node.attrib["kind"].title() + ": ") + self.otherTags(node) - def para (self, node): - if node.text: self._write (node.text.replace('"', r'\"')) + def para(self, node): + if node.text: + self._write(node.text.replace('"', r"\"")) for c in node.iterchildren(): - self.visit (c) - if c.tail: self._write (c.tail.replace('"', r'\"')) + self.visit(c) + if c.tail: + self._write(c.tail.replace('"', r"\"")) self._newline() - def ref (self, node): + def ref(self, node): refid = node.attrib["refid"] if self.index.hasref(refid): - self._write (self.index.getref(refid).name) + self._write(self.index.getref(refid).name) else: self.unkwownReferences[refid] = node - self._write (node.text) + self._write(node.text) assert len(node.getchildren()) == 0 - def parameterlist (self, node): + def parameterlist(self, node): self._newline() - self._write (node.attrib["kind"].title()) + self._write(node.attrib["kind"].title()) self._newline() for item in node.iterchildren("parameteritem"): - self.parameteritem (item) + self.parameteritem(item) - def parameteritem (self, node): + def parameteritem(self, node): indent = " " - self._write (indent + "- ") + self._write(indent + "- ") # should contain two children assert len(node.getchildren()) == 2 - namelist = node.find ("parameternamelist") - desc = node.find ("parameterdescription") + namelist = node.find("parameternamelist") + desc = node.find("parameterdescription") sep = "" for name in namelist.iterchildren("parametername"): - self._write (sep + name.text) + self._write(sep + name.text) sep = ", " - self._write (" ") - self.visit (desc) + self._write(" ") + self.visit(desc) def itemizedlist(self, node): self._newline() - self.otherTags (node) + self.otherTags(node) - def listitem (self, node): - self._write ("- ") - self.otherTags (node) + def listitem(self, node): + self._write("- ") + self.otherTags(node) - def formula (self, node): + def formula(self, node): if node.text: if self.latex is None: - self._write (node.text.strip()) + self._write(node.text.strip()) else: - self._write (self.latex.latex_to_text(node.text)) + self._write(self.latex.latex_to_text(node.text)) diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h index 941998d51..df6d86c7a 100644 --- a/include/hpp/fcl/BV/AABB.h +++ b/include/hpp/fcl/BV/AABB.h @@ -40,10 +40,8 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { struct CollisionRequest; @@ -51,11 +49,10 @@ struct CollisionRequest; /// Classes of different types of bounding volume. /// @{ -/// @brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points -class HPP_FCL_DLLAPI AABB -{ -public: - +/// @brief A class describing the AABB collision structure, which is a box in 3D +/// space determined by two diagonal points +class HPP_FCL_DLLAPI AABB { + public: /// @brief The min point in the AABB Vec3f min_; /// @brief The max point in the AABB @@ -65,74 +62,60 @@ class HPP_FCL_DLLAPI AABB AABB(); /// @brief Creating an AABB at position v with zero size - AABB(const Vec3f& v) : min_(v), max_(v) - { - } + AABB(const Vec3f& v) : min_(v), max_(v) {} /// @brief Creating an AABB with two endpoints a and b - AABB(const Vec3f& a, const Vec3f&b) : min_(a.cwiseMin(b)), - max_(a.cwiseMax(b)) - { - } + AABB(const Vec3f& a, const Vec3f& b) + : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {} /// @brief Creating an AABB centered as core and is of half-dimension delta - AABB(const AABB& core, const Vec3f& delta) : min_(core.min_ - delta), - max_(core.max_ + delta) - { - } + AABB(const AABB& core, const Vec3f& delta) + : min_(core.min_ - delta), max_(core.max_ + delta) {} /// @brief Creating an AABB contains three points - AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(a.cwiseMin(b).cwiseMin(c)), - max_(a.cwiseMax(b).cwiseMax(c)) - { - } - - AABB(const AABB & other) = default; - - AABB & update(const Vec3f& a, const Vec3f& b) - { - min_ = a.cwiseMin(b); max_ = a.cwiseMax(b); + AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) + : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {} + + AABB(const AABB& other) = default; + + AABB& update(const Vec3f& a, const Vec3f& b) { + min_ = a.cwiseMin(b); + max_ = a.cwiseMax(b); return *this; } - + /// @brief Comparison operator - bool operator==(const AABB & other) const - { + bool operator==(const AABB& other) const { return min_ == other.min_ && max_ == other.max_; } - - bool operator!=(const AABB & other) const - { - return !(*this == other); - } + + bool operator!=(const AABB& other) const { return !(*this == other); } /// @name Bounding volume API /// Common API to BVs. /// @{ - + /// @brief Check whether the AABB contains a point - inline bool contain(const Vec3f& p) const - { - if(p[0] < min_[0] || p[0] > max_[0]) return false; - if(p[1] < min_[1] || p[1] > max_[1]) return false; - if(p[2] < min_[2] || p[2] > max_[2]) return false; + inline bool contain(const Vec3f& p) const { + if (p[0] < min_[0] || p[0] > max_[0]) return false; + if (p[1] < min_[1] || p[1] > max_[1]) return false; + if (p[2] < min_[2] || p[2] > max_[2]) return false; return true; } - + /// @brief Check whether two AABB are overlap - inline bool overlap(const AABB& other) const - { - if(min_[0] > other.max_[0]) return false; - if(min_[1] > other.max_[1]) return false; - if(min_[2] > other.max_[2]) return false; + inline bool overlap(const AABB& other) const { + if (min_[0] > other.max_[0]) return false; + if (min_[1] > other.max_[1]) return false; + if (min_[2] > other.max_[2]) return false; - if(max_[0] < other.min_[0]) return false; - if(max_[1] < other.min_[1]) return false; - if(max_[2] < other.min_[2]) return false; + if (max_[0] < other.min_[0]) return false; + if (max_[1] < other.min_[1]) return false; + if (max_[2] < other.min_[2]) return false; return true; - } + } /// @brief Check whether two AABB are overlap bool overlap(const AABB& other, const CollisionRequest& request, @@ -141,140 +124,116 @@ class HPP_FCL_DLLAPI AABB /// @brief Distance between two AABBs FCL_REAL distance(const AABB& other) const; - /// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points + /// @brief Distance between two AABBs; P and Q, should not be NULL, return the + /// nearest points FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const; /// @brief Merge the AABB and a point - inline AABB& operator += (const Vec3f& p) - { + inline AABB& operator+=(const Vec3f& p) { min_ = min_.cwiseMin(p); max_ = max_.cwiseMax(p); return *this; } /// @brief Merge the AABB and another AABB - inline AABB& operator += (const AABB& other) - { + inline AABB& operator+=(const AABB& other) { min_ = min_.cwiseMin(other.min_); max_ = max_.cwiseMax(other.max_); return *this; } /// @brief Return the merged AABB of current AABB and the other one - inline AABB operator + (const AABB& other) const - { + inline AABB operator+(const AABB& other) const { AABB res(*this); return res += other; } /// @brief Size of the AABB (used in BV_Splitter to order two AABBs) - inline FCL_REAL size() const - { - return (max_ - min_).squaredNorm(); - } + inline FCL_REAL size() const { return (max_ - min_).squaredNorm(); } /// @brief Center of the AABB - inline Vec3f center() const - { - return (min_ + max_) * 0.5; - } + inline Vec3f center() const { return (min_ + max_) * 0.5; } /// @brief Width of the AABB - inline FCL_REAL width() const - { - return max_[0] - min_[0]; - } + inline FCL_REAL width() const { return max_[0] - min_[0]; } /// @brief Height of the AABB - inline FCL_REAL height() const - { - return max_[1] - min_[1]; - } + inline FCL_REAL height() const { return max_[1] - min_[1]; } /// @brief Depth of the AABB - inline FCL_REAL depth() const - { - return max_[2] - min_[2]; - } + inline FCL_REAL depth() const { return max_[2] - min_[2]; } - /// @brief Volume of the AABB - inline FCL_REAL volume() const - { - return width() * height() * depth(); - } + /// @brief Volume of the AABB + inline FCL_REAL volume() const { return width() * height() * depth(); } /// @} /// @brief Check whether the AABB contains another AABB - inline bool contain(const AABB& other) const - { - return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); + inline bool contain(const AABB& other) const { + return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && + (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && + (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); } /// @brief Check whether two AABB are overlap and return the overlap part - inline bool overlap(const AABB& other, AABB& overlap_part) const - { - if(!overlap(other)) - { + inline bool overlap(const AABB& other, AABB& overlap_part) const { + if (!overlap(other)) { return false; } - + overlap_part.min_ = min_.cwiseMax(other.min_); overlap_part.max_ = max_.cwiseMin(other.max_); return true; } - + /// @brief Check whether two AABB are overlapped along specific axis - inline bool axisOverlap(const AABB & other, int axis_id) const - { - if(min_[axis_id] > other.max_[axis_id]) return false; - if(max_[axis_id] < other.min_[axis_id]) return false; + inline bool axisOverlap(const AABB& other, int axis_id) const { + if (min_[axis_id] > other.max_[axis_id]) return false; + if (max_[axis_id] < other.min_[axis_id]) return false; return true; } - /// @brief expand the half size of the AABB by delta, and keep the center unchanged. - inline AABB& expand(const Vec3f& delta) - { + /// @brief expand the half size of the AABB by delta, and keep the center + /// unchanged. + inline AABB& expand(const Vec3f& delta) { min_ -= delta; max_ += delta; return *this; } - - /// @brief expand the half size of the AABB by a scalar delta, and keep the center unchanged. - inline AABB& expand(const FCL_REAL delta) - { + + /// @brief expand the half size of the AABB by a scalar delta, and keep the + /// center unchanged. + inline AABB& expand(const FCL_REAL delta) { min_.array() -= delta; max_.array() += delta; return *this; } /// @brief expand the aabb by increase the thickness of the plate by a ratio - inline AABB& expand(const AABB& core, FCL_REAL ratio) - { + inline AABB& expand(const AABB& core, FCL_REAL ratio) { min_ = min_ * ratio - core.min_; max_ = max_ * ratio - core.max_; return *this; } - + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief translate the center of AABB by t -static inline AABB translate(const AABB& aabb, const Vec3f& t) -{ +static inline AABB translate(const AABB& aabb, const Vec3f& t) { AABB res(aabb); res.min_ += t; res.max_ += t; return res; } -static inline AABB rotate(const AABB& aabb, const Matrix3f& R) -{ - AABB res (R * aabb.min_); - Vec3f corner (aabb.min_); - const Eigen::DenseIndex bit[3] = { 1, 2, 4 }; - for (Eigen::DenseIndex ic = 1; ic < 8; ++ic) { // ic = 0 corresponds to aabb.min_. Skip it. +static inline AABB rotate(const AABB& aabb, const Matrix3f& R) { + AABB res(R * aabb.min_); + Vec3f corner(aabb.min_); + const Eigen::DenseIndex bit[3] = {1, 2, 4}; + for (Eigen::DenseIndex ic = 1; ic < 8; + ++ic) { // ic = 0 corresponds to aabb.min_. Skip it. for (Eigen::DenseIndex i = 0; i < 3; ++i) { corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i]; } @@ -283,16 +242,18 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& R) return res; } -/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity. +/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) +/// and b2 is in identity. HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2); -/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity. +/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) +/// and b2 is in identity. HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound); -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h index 416457db3..b42316ea0 100644 --- a/include/hpp/fcl/BV/BV.h +++ b/include/hpp/fcl/BV/BV.h @@ -38,7 +38,6 @@ #ifndef HPP_FCL_BV_H #define HPP_FCL_BV_H - #include #include #include @@ -48,124 +47,99 @@ #include /** @brief Main namespace */ -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @cond IGNORE -namespace details -{ +namespace details { -/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a bounding volume of type BV2 in I configuration. -template -struct Converter -{ +/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a +/// bounding volume of type BV2 in I configuration. +template +struct Converter { static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2); static void convert(const BV1& bv1, BV2& bv2); }; - /// @brief Convert from AABB to AABB, not very tight but is fast. -template<> -struct Converter -{ - static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) - { +template <> +struct Converter { + static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) { const Vec3f& center = bv1.center(); FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5; const Vec3f center2 = tf1.transform(center); bv2.min_ = center2 - Vec3f::Constant(r); bv2.max_ = center2 + Vec3f::Constant(r); } - - static void convert(const AABB& bv1, AABB& bv2) - { - bv2 = bv1; - } + + static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; } }; -template<> -struct Converter -{ - static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) - { +template <> +struct Converter { + static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) { bv2.To = tf1.transform(bv1.center()); bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; bv2.axes = tf1.getRotation(); } - - static void convert(const AABB& bv1, OBB& bv2) - { + + static void convert(const AABB& bv1, OBB& bv2) { bv2.To = bv1.center(); bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; bv2.axes.setIdentity(); } }; -template<> -struct Converter -{ - static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2) - { +template <> +struct Converter { + static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2) { bv2.extent = bv1.extent; bv2.To = tf1.transform(bv1.To); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; } - - static void convert(const OBB& bv1, OBB& bv2) - { - bv2 = bv1; - } + + static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; } }; -template<> -struct Converter -{ - static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2) - { +template <> +struct Converter { + static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2) { Converter::convert(bv1.obb, tf1, bv2); } - - static void convert(const OBBRSS& bv1, OBB& bv2) - { + + static void convert(const OBBRSS& bv1, OBB& bv2) { Converter::convert(bv1.obb, bv2); } }; -template<> -struct Converter -{ - static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) - { - bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius); +template <> +struct Converter { + static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) { + bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, + bv1.length[1] * 0.5 + bv1.radius, bv1.radius); bv2.To = tf1.transform(bv1.Tr); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; } - - static void convert(const RSS& bv1, OBB& bv2) - { - bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius); + + static void convert(const RSS& bv1, OBB& bv2) { + bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, + bv1.length[1] * 0.5 + bv1.radius, bv1.radius); bv2.To = bv1.Tr; bv2.axes = bv1.axes; } }; - -template -struct Converter -{ - static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) - { +template +struct Converter { + static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) { const Vec3f& center = bv1.center(); FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; const Vec3f center2 = tf1.transform(center); bv2.min_ = center2 - Vec3f::Constant(r); bv2.max_ = center2 + Vec3f::Constant(r); } - - static void convert(const BV1& bv1, AABB& bv2) - { + + static void convert(const BV1& bv1, AABB& bv2) { const Vec3f& center = bv1.center(); FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; bv2.min_ = center - Vec3f::Constant(r); @@ -173,53 +147,45 @@ struct Converter } }; -template -struct Converter -{ - static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2) - { +template +struct Converter { + static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2) { AABB bv; Converter::convert(bv1, bv); Converter::convert(bv, tf1, bv2); } - - static void convert(const BV1& bv1, OBB& bv2) - { + + static void convert(const BV1& bv1, OBB& bv2) { AABB bv; Converter::convert(bv1, bv); Converter::convert(bv, bv2); } }; -template<> -struct Converter -{ - static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2) - { +template <> +struct Converter { + static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.To); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; - + bv2.radius = bv1.extent[2]; bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius); bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius); } - - static void convert(const OBB& bv1, RSS& bv2) - { + + static void convert(const OBB& bv1, RSS& bv2) { bv2.Tr = bv1.To; bv2.axes = bv1.axes; - + bv2.radius = bv1.extent[2]; bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius); bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius); } }; -template<> -struct Converter -{ - static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2) - { +template <> +struct Converter { + static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.Tr); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; @@ -227,53 +193,42 @@ struct Converter bv2.length[0] = bv1.length[0]; bv2.length[1] = bv1.length[1]; } - - static void convert(const RSS& bv1, RSS& bv2) - { - bv2 = bv1; - } + + static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; } }; -template<> -struct Converter -{ - static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2) - { +template <> +struct Converter { + static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2) { Converter::convert(bv1.rss, tf1, bv2); } - - static void convert(const OBBRSS& bv1, RSS& bv2) - { + + static void convert(const OBBRSS& bv1, RSS& bv2) { Converter::convert(bv1.rss, bv2); } }; -template<> -struct Converter -{ - static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2) - { +template <> +struct Converter { + static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.center()); /// Sort the AABB edges so that AABB extents are ordered. - FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() }; + FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth()}; Eigen::DenseIndex id[3] = {0, 1, 2}; - for(Eigen::DenseIndex i = 1; i < 3; ++i) - { - for(Eigen::DenseIndex j = i; j > 0; --j) - { - if(d[j] > d[j-1]) - { + for (Eigen::DenseIndex i = 1; i < 3; ++i) { + for (Eigen::DenseIndex j = i; j > 0; --j) { + if (d[j] > d[j - 1]) { { FCL_REAL tmp = d[j]; - d[j] = d[j-1]; - d[j-1] = tmp; + d[j] = d[j - 1]; + d[j - 1] = tmp; } { Eigen::DenseIndex tmp = id[j]; - id[j] = id[j-1]; - id[j-1] = tmp; + id[j] = id[j - 1]; + id[j - 1] = tmp; } } } @@ -286,55 +241,52 @@ struct Converter const Matrix3f& R = tf1.getRotation(); const bool left_hand = (id[0] == (id[1] + 1) % 3); - if (left_hand) bv2.axes.col(0) = -R.col(id[0]); - else bv2.axes.col(0) = R.col(id[0]); + if (left_hand) + bv2.axes.col(0) = -R.col(id[0]); + else + bv2.axes.col(0) = R.col(id[0]); bv2.axes.col(1) = R.col(id[1]); bv2.axes.col(2) = R.col(id[2]); } - - static void convert(const AABB& bv1, RSS& bv2) - { + + static void convert(const AABB& bv1, RSS& bv2) { convert(bv1, Transform3f(), bv2); } }; -template<> -struct Converter -{ - static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2) - { +template <> +struct Converter { + static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2) { Converter::convert(bv1, tf1, bv2.obb); Converter::convert(bv1, tf1, bv2.rss); } - - static void convert(const AABB& bv1, OBBRSS& bv2) - { + + static void convert(const AABB& bv1, OBBRSS& bv2) { Converter::convert(bv1, bv2.obb); Converter::convert(bv1, bv2.rss); } }; -} +} // namespace details -/// @endcond +/// @endcond - -/// @brief Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity configuration. -template -static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2) -{ +/// @brief Convert a bounding volume of type BV1 in configuration tf1 to +/// bounding volume of type BV2 in identity configuration. +template +static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2) { details::Converter::convert(bv1, tf1, bv2); } -/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2 in identity configuration. -template -static inline void convertBV(const BV1& bv1, BV2& bv2) -{ +/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2 +/// in identity configuration. +template +static inline void convertBV(const BV1& bv1, BV2& bv2) { details::Converter::convert(bv1, bv2); } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h index 2397c3bc9..8ba57ea26 100644 --- a/include/hpp/fcl/BV/BV_node.h +++ b/include/hpp/fcl/BV/BV_node.h @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_BV_NODE_H #define HPP_FCL_BV_NODE_H @@ -44,102 +43,95 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @defgroup Construction_Of_BVH Construction of BVHModel /// Classes which are used to build a BVHModel (Bounding Volume Hierarchy) /// @{ /// @brief BVNodeBase encodes the tree structure for BVH -struct HPP_FCL_DLLAPI BVNodeBase -{ +struct HPP_FCL_DLLAPI BVNodeBase { /// @brief An index for first child node or primitive /// If the value is positive, it is the index of the first child bv node /// If the value is negative, it is -(primitive index + 1) /// Zero is not used. int first_child; - /// @brief The start id the primitive belonging to the current node. The index is referred to the primitive_indices in BVHModel and from that - /// we can obtain the primitive's index in original data indirectly. + /// @brief The start id the primitive belonging to the current node. The index + /// is referred to the primitive_indices in BVHModel and from that we can + /// obtain the primitive's index in original data indirectly. unsigned int first_primitive; - /// @brief The number of primitives belonging to the current node + /// @brief The number of primitives belonging to the current node unsigned int num_primitives; - + /// @brief Default constructor BVNodeBase() - : first_child(0) - , first_primitive((std::numeric_limits::max)()) // value we should help to raise an issue - , num_primitives(0) - {} - + : first_child(0), + first_primitive( + (std::numeric_limits::max)()) // value we should help + // to raise an issue + , + num_primitives(0) {} + /// @brief Equality operator - bool operator==(const BVNodeBase & other) const - { - return - first_child == other.first_child - && first_primitive == other.first_primitive - && num_primitives == other.num_primitives; + bool operator==(const BVNodeBase& other) const { + return first_child == other.first_child && + first_primitive == other.first_primitive && + num_primitives == other.num_primitives; } - + /// @brief Difference operator - bool operator!=(const BVNodeBase & other) const - { - return !(*this == other); - } + bool operator!=(const BVNodeBase& other) const { return !(*this == other); } - /// @brief Whether current node is a leaf node (i.e. contains a primitive index + /// @brief Whether current node is a leaf node (i.e. contains a primitive + /// index inline bool isLeaf() const { return first_child < 0; } - /// @brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel + /// @brief Return the primitive index. The index is referred to the original + /// data (i.e. vertices or tri_indices) in BVHModel inline int primitiveId() const { return -(first_child + 1); } - /// @brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel + /// @brief Return the index of the first child. The index is referred to the + /// bounding volume array (i.e. bvs) in BVHModel inline int leftChild() const { return first_child; } - /// @brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel + /// @brief Return the index of the second child. The index is referred to the + /// bounding volume array (i.e. bvs) in BVHModel inline int rightChild() const { return first_child + 1; } }; -/// @brief A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and also the geometry data provided in BV template parameter. -template -struct HPP_FCL_DLLAPI BVNode : public BVNodeBase -{ +/// @brief A class describing a bounding volume node. It includes the tree +/// structure providing in BVNodeBase and also the geometry data provided in BV +/// template parameter. +template +struct HPP_FCL_DLLAPI BVNode : public BVNodeBase { typedef BVNodeBase Base; - + /// @brief bounding volume storing the geometry BV bv; - + /// @brief Equality operator - bool operator==(const BVNode & other) const - { + bool operator==(const BVNode& other) const { return Base::operator==(other) && bv == other.bv; } - + /// @brief Difference operator - bool operator!=(const BVNode & other) const - { - return !(*this == other); - } + bool operator!=(const BVNode& other) const { return !(*this == other); } /// @brief Check whether two BVNode collide - bool overlap(const BVNode& other) const - { - return bv.overlap(other.bv); - } + bool overlap(const BVNode& other) const { return bv.overlap(other.bv); } /// @brief Check whether two BVNode collide bool overlap(const BVNode& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const - { + FCL_REAL& sqrDistLowerBound) const { return bv.overlap(other.bv, request, sqrDistLowerBound); } - /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distance, return the nearest points. - FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const - { + /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and + /// the underlying BV supports distance, return the nearest points. + FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, + Vec3f* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } @@ -147,8 +139,7 @@ struct HPP_FCL_DLLAPI BVNode : public BVNodeBase Vec3f getCenter() const { return bv.center(); } /// @brief Access to the orientation of the BV - const Matrix3f& getOrientation() const - { + const Matrix3f& getOrientation() const { static const Matrix3f id3 = Matrix3f::Identity(); return id3; } @@ -158,27 +149,23 @@ struct HPP_FCL_DLLAPI BVNode : public BVNodeBase /// \endcond }; -template<> -inline const Matrix3f& BVNode::getOrientation() const -{ +template <> +inline const Matrix3f& BVNode::getOrientation() const { return bv.axes; } -template<> -inline const Matrix3f& BVNode::getOrientation() const -{ +template <> +inline const Matrix3f& BVNode::getOrientation() const { return bv.axes; } -template<> -inline const Matrix3f& BVNode::getOrientation() const -{ +template <> +inline const Matrix3f& BVNode::getOrientation() const { return bv.obb.axes; } +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h index bb4e7911c..9d765a712 100644 --- a/include/hpp/fcl/BV/OBB.h +++ b/include/hpp/fcl/BV/OBB.h @@ -40,10 +40,8 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { struct CollisionRequest; @@ -51,48 +49,39 @@ struct CollisionRequest; /// @{ /// @brief Oriented bounding box class -struct HPP_FCL_DLLAPI OBB -{ - /// @brief Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i-th principle direction of the box. - /// We assume that axis[0] corresponds to the axis with the longest box edge, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one. +struct HPP_FCL_DLLAPI OBB { + /// @brief Orientation of OBB. axis[i] is the ith column of the orientation + /// matrix for the box; it is also the i-th principle direction of the box. We + /// assume that axis[0] corresponds to the axis with the longest box edge, + /// axis[1] corresponds to the shorter one and axis[2] corresponds to the + /// shortest one. Matrix3f axes; /// @brief Center of OBB Vec3f To; - + /// @brief Half dimensions of OBB Vec3f extent; - - OBB() - : axes(Matrix3f::Zero()) - , To(Vec3f::Zero()) - , extent(Vec3f::Zero()) - {} - + + OBB() : axes(Matrix3f::Zero()), To(Vec3f::Zero()), extent(Vec3f::Zero()) {} + /// @brief Equality operator - bool operator==(const OBB & other) const - { - return - axes == other.axes - && To == other.To - && extent == other.extent; + bool operator==(const OBB& other) const { + return axes == other.axes && To == other.To && extent == other.extent; } - + /// @brief Difference operator - bool operator!=(const OBB & other) const - { - return !(*this == other); - } + bool operator!=(const OBB& other) const { return !(*this == other); } /// @brief Check whether the OBB contains a point. bool contain(const Vec3f& p) const; /// Check collision between two OBB - /// @return true if collision happens. + /// @return true if collision happens. bool overlap(const OBB& other) const; /// Check collision between two OBB - /// @return true if collision happens. + /// @return true if collision happens. /// @retval sqrDistLowerBound squared lower bound on distance between boxes if /// they do not overlap. bool overlap(const OBB& other, const CollisionRequest& request, @@ -101,70 +90,53 @@ struct HPP_FCL_DLLAPI OBB /// @brief Distance between two OBBs, not implemented. FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; - /// @brief A simple way to merge the OBB and a point (the result is not compact). - OBB& operator += (const Vec3f& p); + /// @brief A simple way to merge the OBB and a point (the result is not + /// compact). + OBB& operator+=(const Vec3f& p); /// @brief Merge the OBB and another OBB (the result is not compact). - OBB& operator += (const OBB& other) - { - *this = *this + other; - return *this; + OBB& operator+=(const OBB& other) { + *this = *this + other; + return *this; } - /// @brief Return the merged OBB of current OBB and the other one (the result is not compact). - OBB operator + (const OBB& other) const; + /// @brief Return the merged OBB of current OBB and the other one (the result + /// is not compact). + OBB operator+(const OBB& other) const; /// @brief Size of the OBB (used in BV_Splitter to order two OBBs) - inline FCL_REAL size() const - { - return extent.squaredNorm(); - } + inline FCL_REAL size() const { return extent.squaredNorm(); } /// @brief Center of the OBB - inline const Vec3f& center() const - { - return To; - } + inline const Vec3f& center() const { return To; } /// @brief Width of the OBB. - inline FCL_REAL width() const - { - return 2 * extent[0]; - } + inline FCL_REAL width() const { return 2 * extent[0]; } /// @brief Height of the OBB. - inline FCL_REAL height() const - { - return 2 * extent[1]; - } + inline FCL_REAL height() const { return 2 * extent[1]; } /// @brief Depth of the OBB - inline FCL_REAL depth() const - { - return 2 * extent[2]; - } + inline FCL_REAL depth() const { return 2 * extent[2]; } /// @brief Volume of the OBB - inline FCL_REAL volume() const - { - return width() * height() * depth(); - } + inline FCL_REAL volume() const { return width() * height() * depth(); } }; - /// @brief Translate the OBB bv HPP_FCL_DLLAPI OBB translate(const OBB& bv, const Vec3f& t); -/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, - const OBB& b1, const OBB& b2); +/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, + const OBB& b2); -/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity. +/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and +/// b2 is in identity. HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound); - /// Check collision between two boxes /// @param B, T orientation and position of first box, /// @param a half dimensions of first box, @@ -172,8 +144,8 @@ HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, /// The second box is in identity configuration. HPP_FCL_DLLAPI bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b); -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h index 69c23e857..e5fb8a7f4 100644 --- a/include/hpp/fcl/BV/OBBRSS.h +++ b/include/hpp/fcl/BV/OBBRSS.h @@ -38,135 +38,99 @@ #ifndef HPP_FCL_OBBRSS_H #define HPP_FCL_OBBRSS_H - #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { - struct CollisionRequest; +struct CollisionRequest; /// @addtogroup Bounding_Volume /// @{ -/// @brief Class merging the OBB and RSS, can handle collision and distance simultaneously -struct HPP_FCL_DLLAPI OBBRSS -{ - +/// @brief Class merging the OBB and RSS, can handle collision and distance +/// simultaneously +struct HPP_FCL_DLLAPI OBBRSS { /// @brief OBB member, for rotation OBB obb; /// @brief RSS member, for distance RSS rss; - + /// @brief Equality operator - bool operator==(const OBBRSS & other) const - { - return - obb == other.obb - && rss == other.rss; + bool operator==(const OBBRSS& other) const { + return obb == other.obb && rss == other.rss; } - + /// @brief Difference operator - bool operator!=(const OBBRSS & other) const - { - return !(*this == other); - } + bool operator!=(const OBBRSS& other) const { return !(*this == other); } /// @brief Check whether the OBBRSS contains a point - inline bool contain(const Vec3f& p) const - { - return obb.contain(p); - } + inline bool contain(const Vec3f& p) const { return obb.contain(p); } /// @brief Check collision between two OBBRSS - bool overlap(const OBBRSS& other) const - { - return obb.overlap(other.obb); - } + bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); } /// Check collision between two OBBRSS /// @retval sqrDistLowerBound squared lower bound on distance between /// objects if they do not overlap. bool overlap(const OBBRSS& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const - { + FCL_REAL& sqrDistLowerBound) const { return obb.overlap(other.obb, request, sqrDistLowerBound); } - /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points - FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const - { + /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the + /// nearest points + FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, + Vec3f* Q = NULL) const { return rss.distance(other.rss, P, Q); } /// @brief Merge the OBBRSS and a point - OBBRSS& operator += (const Vec3f& p) - { + OBBRSS& operator+=(const Vec3f& p) { obb += p; rss += p; return *this; } /// @brief Merge two OBBRSS - OBBRSS& operator += (const OBBRSS& other) - { + OBBRSS& operator+=(const OBBRSS& other) { *this = *this + other; return *this; } /// @brief Merge two OBBRSS - OBBRSS operator + (const OBBRSS& other) const - { + OBBRSS operator+(const OBBRSS& other) const { OBBRSS result; result.obb = obb + other.obb; result.rss = rss + other.rss; return result; } - /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS) - inline FCL_REAL size() const - { - return obb.size(); - } + /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS) + inline FCL_REAL size() const { return obb.size(); } /// @brief Center of the OBBRSS - inline const Vec3f& center() const - { - return obb.center(); - } + inline const Vec3f& center() const { return obb.center(); } /// @brief Width of the OBRSS - inline FCL_REAL width() const - { - return obb.width(); - } + inline FCL_REAL width() const { return obb.width(); } /// @brief Height of the OBBRSS - inline FCL_REAL height() const - { - return obb.height(); - } + inline FCL_REAL height() const { return obb.height(); } /// @brief Depth of the OBBRSS - inline FCL_REAL depth() const - { - return obb.depth(); - } + inline FCL_REAL depth() const { return obb.depth(); } /// @brief Volume of the OBBRSS - inline FCL_REAL volume() const - { - return obb.volume(); - } + inline FCL_REAL volume() const { return obb.volume(); } }; -/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity -inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2) -{ +/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) +/// and b2 is in indentity +inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, + const OBBRSS& b2) { return overlap(R0, T0, b1.obb, b2.obb); } @@ -174,23 +138,23 @@ inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const /// @param R0, T0 configuration of b1 /// @param b1 first OBBRSS in configuration (R0, T0) /// @param b2 second OBBRSS in identity position -/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do not overlap. +/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do +/// not overlap. inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) -{ + FCL_REAL& sqrDistLowerBound) { return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound); } -/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points -inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL) -{ +/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) +/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points +inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, + const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL) { return distance(R0, T0, b1.rss, b2.rss, P, Q); } -} - +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h index f625db0e4..44a10a004 100644 --- a/include/hpp/fcl/BV/RSS.h +++ b/include/hpp/fcl/BV/RSS.h @@ -41,10 +41,8 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { struct CollisionRequest; @@ -52,10 +50,12 @@ struct CollisionRequest; /// @{ /// @brief A class for rectangle sphere-swept bounding volume -struct HPP_FCL_DLLAPI RSS -{ - /// @brief Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i-th principle direction of the RSS. - /// We assume that axis[0] corresponds to the axis with the longest length, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one. +struct HPP_FCL_DLLAPI RSS { + /// @brief Orientation of RSS. axis[i] is the ith column of the orientation + /// matrix for the RSS; it is also the i-th principle direction of the RSS. We + /// assume that axis[0] corresponds to the axis with the longest length, + /// axis[1] corresponds to the shorter one and axis[2] corresponds to the + /// shortest one. Matrix3f axes; /// @brief Origin of the rectangle in RSS @@ -68,30 +68,20 @@ struct HPP_FCL_DLLAPI RSS FCL_REAL radius; /// @brief Default constructor with default values - RSS() - : axes(Matrix3f::Zero()) - , Tr(Vec3f::Zero()) - , radius(-1) - { - length[0] = 0; length[1] = 0; + RSS() : axes(Matrix3f::Zero()), Tr(Vec3f::Zero()), radius(-1) { + length[0] = 0; + length[1] = 0; } - + /// @brief Equality operator - bool operator==(const RSS & other) const - { - return - axes == other.axes - && Tr == other.Tr - && length[0] == other.length[0] - && length[1] == other.length[1] - && radius == other.radius; + bool operator==(const RSS& other) const { + return axes == other.axes && Tr == other.Tr && + length[0] == other.length[0] && length[1] == other.length[1] && + radius == other.radius; } - + /// @brief Difference operator - bool operator!=(const RSS & other) const - { - return !(*this == other); - } + bool operator!=(const RSS& other) const { return !(*this == other); } /// @brief Check whether the RSS contains a point bool contain(const Vec3f& p) const; @@ -101,95 +91,83 @@ struct HPP_FCL_DLLAPI RSS /// Not implemented bool overlap(const RSS& other, const CollisionRequest&, - FCL_REAL& sqrDistLowerBound) const - { - sqrDistLowerBound = sqrt (-1); - return overlap (other); + FCL_REAL& sqrDistLowerBound) const { + sqrDistLowerBound = sqrt(-1); + return overlap(other); } - /// @brief the distance between two RSS; P and Q, if not NULL, return the nearest points + /// @brief the distance between two RSS; P and Q, if not NULL, return the + /// nearest points FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; /// @brief A simple way to merge the RSS and a point, not compact. /// @todo This function may have some bug. - RSS& operator += (const Vec3f& p); + RSS& operator+=(const Vec3f& p); /// @brief Merge the RSS and another RSS - inline RSS& operator += (const RSS& other) - { + inline RSS& operator+=(const RSS& other) { *this = *this + other; return *this; } /// @brief Return the merged RSS of current RSS and the other one - RSS operator + (const RSS& other) const; - + RSS operator+(const RSS& other) const; /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) - inline FCL_REAL size() const - { - return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + 2 * radius); + inline FCL_REAL size() const { + return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + + 2 * radius); } /// @brief The RSS center - inline const Vec3f& center() const - { - return Tr; - } + inline const Vec3f& center() const { return Tr; } /// @brief Width of the RSS - inline FCL_REAL width() const - { - return length[0] + 2 * radius; - } + inline FCL_REAL width() const { return length[0] + 2 * radius; } /// @brief Height of the RSS - inline FCL_REAL height() const - { - return length[1] + 2 * radius; - } + inline FCL_REAL height() const { return length[1] + 2 * radius; } /// @brief Depth of the RSS - inline FCL_REAL depth() const - { - return 2 * radius; - } + inline FCL_REAL depth() const { return 2 * radius; } /// @brief Volume of the RSS - inline FCL_REAL volume() const - { - return (length[0] * length[1] * 2 * radius + 4 * boost::math::constants::pi() * radius * radius * radius); + inline FCL_REAL volume() const { + return (length[0] * length[1] * 2 * radius + + 4 * boost::math::constants::pi() * radius * radius * + radius); } /// @brief Check collision between two RSS and return the overlap part. - /// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS. - bool overlap(const RSS& other, RSS& /*overlap_part*/) const - { + /// For RSS, we return nothing, as the overlap part of two RSSs usually is not + /// a RSS. + bool overlap(const RSS& other, RSS& /*overlap_part*/) const { return overlap(other); } }; /// @brief distance between two RSS bounding volumes -/// P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points -/// Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1) +/// P and Q (optional return values) are the closest points in the rectangles, +/// not the RSS. But the direction P - Q is the correct direction for cloest +/// points Notice that P and Q are both in the local frame of the first RSS (not +/// global frame and not even the local frame of object 1) HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, - const RSS& b1, const RSS& b2, - Vec3f* P = NULL, Vec3f* Q = NULL); - - -/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, - const RSS& b1, const RSS& b2); - -/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, - const RSS& b1, const RSS& b2, - const CollisionRequest& request, + const RSS& b1, const RSS& b2, Vec3f* P = NULL, + Vec3f* Q = NULL); + +/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, + const RSS& b2); + +/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, + const RSS& b2, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound); -} - +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h index fcd382de8..e8e6ba328 100644 --- a/include/hpp/fcl/BV/kDOP.h +++ b/include/hpp/fcl/BV/kDOP.h @@ -41,19 +41,20 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { struct CollisionRequest; - /// @addtogroup Bounding_Volume - /// @{ +/// @addtogroup Bounding_Volume +/// @{ -/// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 -/// The KDOP structure is defined by some pairs of parallel planes defined by some axes. -/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: +/// @brief KDOP class describes the KDOP collision structures. K is set as the +/// template parameter, which should be 16, 18, or 24 +/// The KDOP structure is defined by some pairs of parallel planes defined by +/// some axes. +/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off +/// some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 8 /// (0,-1,0) and (0,1,0) -> indices 1 and 9 /// (0,0,-1) and (0,0,1) -> indices 2 and 10 @@ -62,7 +63,8 @@ struct CollisionRequest; /// (0,-1,-1) and (0,1,1) -> indices 5 and 13 /// (-1,1,0) and (1,-1,0) -> indices 6 and 14 /// (-1,0,1) and (1,0,-1) -> indices 7 and 15 -/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: +/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off +/// some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 9 /// (0,-1,0) and (0,1,0) -> indices 1 and 10 /// (0,0,-1) and (0,0,1) -> indices 2 and 11 @@ -72,7 +74,8 @@ struct CollisionRequest; /// (-1,1,0) and (1,-1,0) -> indices 6 and 15 /// (-1,0,1) and (1,0,-1) -> indices 7 and 16 /// (0,-1,1) and (0,1,-1) -> indices 8 and 17 -/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: +/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off +/// some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 12 /// (0,-1,0) and (0,1,0) -> indices 1 and 13 /// (0,0,-1) and (0,0,1) -> indices 2 and 14 @@ -85,15 +88,13 @@ struct CollisionRequest; /// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21 /// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 /// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 -template -class HPP_FCL_DLLAPI KDOP -{ -protected: +template +class HPP_FCL_DLLAPI KDOP { + protected: /// @brief Origin's distances to N KDOP planes Eigen::Array dist_; -public: - + public: /// @brief Creating kDOP containing nothing KDOP(); @@ -102,118 +103,94 @@ class HPP_FCL_DLLAPI KDOP /// @brief Creating kDOP containing two points KDOP(const Vec3f& a, const Vec3f& b); - + /// @brief Equality operator - bool operator==(const KDOP & other) const - { + bool operator==(const KDOP& other) const { return (dist_ == other.dist_).all(); } - + /// @brief Difference operator - bool operator!=(const KDOP & other) const - { + bool operator!=(const KDOP& other) const { return (dist_ != other.dist_).any(); } - + /// @brief Check whether two KDOPs overlap. bool overlap(const KDOP& other) const; /// @brief Check whether two KDOPs overlap. - /// @return true if collision happens. + /// @return true if collision happens. /// @retval sqrDistLowerBound squared lower bound on distance between boxes if /// they do not overlap. bool overlap(const KDOP& other, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) const; - /// @brief The distance between two KDOP. Not implemented. - FCL_REAL distance(const KDOP& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; + /// @brief The distance between two KDOP. Not implemented. + FCL_REAL distance(const KDOP& other, Vec3f* P = NULL, + Vec3f* Q = NULL) const; /// @brief Merge the point and the KDOP - KDOP& operator += (const Vec3f& p); + KDOP& operator+=(const Vec3f& p); /// @brief Merge two KDOPs - KDOP& operator += (const KDOP& other); + KDOP& operator+=(const KDOP& other); /// @brief Create a KDOP by mergin two KDOPs - KDOP operator + (const KDOP& other) const; + KDOP operator+(const KDOP& other) const; - /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) - inline FCL_REAL size() const - { + /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) + inline FCL_REAL size() const { return width() * width() + height() * height() + depth() * depth(); } /// @brief The (AABB) center - inline Vec3f center() const - { - return (dist_.template head<3>() + dist_.template segment<3>(N/2)) / 2; + inline Vec3f center() const { + return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2; } - /// @brief The (AABB) width - inline FCL_REAL width() const - { - return dist_[N / 2] - dist_[0]; - } + inline FCL_REAL width() const { return dist_[N / 2] - dist_[0]; } /// @brief The (AABB) height - inline FCL_REAL height() const - { - return dist_[N / 2 + 1] - dist_[1]; - } + inline FCL_REAL height() const { return dist_[N / 2 + 1] - dist_[1]; } /// @brief The (AABB) depth - inline FCL_REAL depth() const - { - return dist_[N / 2 + 2] - dist_[2]; - } + inline FCL_REAL depth() const { return dist_[N / 2 + 2] - dist_[2]; } /// @brief The (AABB) volume - inline FCL_REAL volume() const - { - return width() * height() * depth(); - } + inline FCL_REAL volume() const { return width() * height() * depth(); } - inline FCL_REAL dist(short i) const - { - return dist_[i]; - } + inline FCL_REAL dist(short i) const { return dist_[i]; } - inline FCL_REAL& dist(short i) - { - return dist_[i]; - } + inline FCL_REAL& dist(short i) { return dist_[i]; } //// @brief Check whether one point is inside the KDOP bool inside(const Vec3f& p) const; - public: + public: /// \cond EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// \endcond }; -template -bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, - const KDOP& /*b1*/, const KDOP& /*b2*/) -{ - throw std::logic_error ("not implemented"); +template +bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, + const KDOP& /*b2*/) { + throw std::logic_error("not implemented"); } -template -bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, - const KDOP& /*b1*/, const KDOP& /*b2*/, - const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/) -{ - throw std::logic_error ("not implemented"); +template +bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, + const KDOP& /*b2*/, const CollisionRequest& /*request*/, + FCL_REAL& /*sqrDistLowerBound*/) { + throw std::logic_error("not implemented"); } /// @brief translate the KDOP BV -template +template HPP_FCL_DLLAPI KDOP translate(const KDOP& bv, const Vec3f& t); -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h index a089ccbd3..35dc5f211 100644 --- a/include/hpp/fcl/BV/kIOS.h +++ b/include/hpp/fcl/BV/kIOS.h @@ -40,86 +40,73 @@ #include - -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { struct CollisionRequest; /// @addtogroup Bounding_Volume /// @{ -/// @brief A class describing the kIOS collision structure, which is a set of spheres. -class HPP_FCL_DLLAPI kIOS -{ +/// @brief A class describing the kIOS collision structure, which is a set of +/// spheres. +class HPP_FCL_DLLAPI kIOS { /// @brief One sphere in kIOS - struct HPP_FCL_DLLAPI kIOS_Sphere - { + struct HPP_FCL_DLLAPI kIOS_Sphere { Vec3f o; FCL_REAL r; - - bool operator==(const kIOS_Sphere & other) const - { + + bool operator==(const kIOS_Sphere& other) const { return o == other.o && r == other.r; } - - bool operator!=(const kIOS_Sphere & other) const - { + + bool operator!=(const kIOS_Sphere& other) const { return !(*this == other); } }; /// @brief generate one sphere enclosing two spheres - static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1) - { + static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, + const kIOS_Sphere& s1) { Vec3f d = s1.o - s0.o; FCL_REAL dist2 = d.squaredNorm(); FCL_REAL diff_r = s1.r - s0.r; - + /** The sphere with the larger radius encloses the other */ - if(diff_r * diff_r >= dist2) - { - if(s1.r > s0.r) return s1; - else return s0; - } - else /** spheres partially overlapping or disjoint */ + if (diff_r * diff_r >= dist2) { + if (s1.r > s0.r) + return s1; + else + return s0; + } else /** spheres partially overlapping or disjoint */ { float dist = (float)std::sqrt(dist2); kIOS_Sphere s; s.r = dist + s0.r + s1.r; - if(dist > 0) + if (dist > 0) s.o = s0.o + d * ((s.r - s0.r) / dist); else s.o = s0.o; return s; } } -public: - + + public: /// @brief Equality operator - bool operator==(const kIOS & other) const - { + bool operator==(const kIOS& other) const { bool res = obb == other.obb && num_spheres == other.num_spheres; - if(!res) - return false; - - for(size_t k = 0; k < num_spheres; ++k) - { - if(spheres[k] != other.spheres[k]) - return false; + if (!res) return false; + + for (size_t k = 0; k < num_spheres; ++k) { + if (spheres[k] != other.spheres[k]) return false; } - + return true; } - + /// @brief Difference operator - bool operator!=(const kIOS & other) const - { - return !(*this == other); - } - + bool operator!=(const kIOS& other) const { return !(*this == other); } + /// @brief The (at most) five spheres for intersection kIOS_Sphere spheres[5]; @@ -143,26 +130,22 @@ class HPP_FCL_DLLAPI kIOS FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; /// @brief A simple way to merge the kIOS and a point - kIOS& operator += (const Vec3f& p); + kIOS& operator+=(const Vec3f& p); /// @brief Merge the kIOS and another kIOS - kIOS& operator += (const kIOS& other) - { + kIOS& operator+=(const kIOS& other) { *this = *this + other; return *this; } /// @brief Return the merged kIOS of current kIOS and the other one - kIOS operator + (const kIOS& other) const; + kIOS operator+(const kIOS& other) const; /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) FCL_REAL size() const; /// @brief Center of the kIOS - const Vec3f& center() const - { - return spheres[0].o; - } + const Vec3f& center() const { return spheres[0].o; } /// @brief Width of the kIOS FCL_REAL width() const; @@ -177,20 +160,20 @@ class HPP_FCL_DLLAPI kIOS FCL_REAL volume() const; }; - /// @brief Translate the kIOS BV HPP_FCL_DLLAPI kIOS translate(const kIOS& bv, const Vec3f& t); -/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. +/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) +/// and b2 is in identity. /// @todo Not efficient -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, - const kIOS& b1, const kIOS& b2); +HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, + const kIOS& b2); -/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. +/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) +/// and b2 is in identity. /// @todo Not efficient -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, - const kIOS& b1, const kIOS& b2, - const CollisionRequest& request, +HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, + const kIOS& b2, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound); /// @brief Approximate distance between two kIOS bounding volumes @@ -199,8 +182,8 @@ HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL); -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BVH/BVH_front.h b/include/hpp/fcl/BVH/BVH_front.h index cd250186d..9a3a99eea 100644 --- a/include/hpp/fcl/BVH/BVH_front.h +++ b/include/hpp/fcl/BVH/BVH_front.h @@ -38,48 +38,42 @@ #ifndef HPP_FCL_BVH_FRONT_H #define HPP_FCL_BVH_FRONT_H - #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Front list acceleration for collision /// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where -/// the traversal terminates while performing a query during a given time instance. The front list reflects the subset of a -/// BVTT that is traversed for that particular proximity query. -struct HPP_FCL_DLLAPI BVHFrontNode -{ - /// @brief The nodes to start in the future, i.e. the wave front of the traversal tree. +/// the traversal terminates while performing a query during a given time +/// instance. The front list reflects the subset of a BVTT that is traversed for +/// that particular proximity query. +struct HPP_FCL_DLLAPI BVHFrontNode { + /// @brief The nodes to start in the future, i.e. the wave front of the + /// traversal tree. unsigned int left, right; - /// @brief The front node is not valid when collision is detected on the front node. + /// @brief The front node is not valid when collision is detected on the front + /// node. bool valid; BVHFrontNode(unsigned int left_, unsigned int right_) - : left(left_) - , right(right_) - , valid(true) - { - } + : left(left_), right(right_), valid(true) {} }; /// @brief BVH front list is a list of front nodes. typedef std::list BVHFrontList; /// @brief Add new front node into the front list -inline void updateFrontList(BVHFrontList* front_list, unsigned int b1, unsigned int b2) -{ - if(front_list) front_list->push_back(BVHFrontNode(b1, b2)); +inline void updateFrontList(BVHFrontList* front_list, unsigned int b1, + unsigned int b2) { + if (front_list) front_list->push_back(BVHFrontNode(b1, b2)); } +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BVH/BVH_internal.h b/include/hpp/fcl/BVH/BVH_internal.h index d0a233739..407a430ca 100644 --- a/include/hpp/fcl/BVH/BVH_internal.h +++ b/include/hpp/fcl/BVH/BVH_internal.h @@ -40,50 +40,51 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief States for BVH construction /// empty->begun->processed ->replace_begun->processed -> ...... /// | /// |-> update_begun -> updated -> ..... -enum BVHBuildState - { - BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor - BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry primitives - BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use - BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for updating geometry primitives - BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, ready for ccd use - BVH_BUILD_STATE_REPLACE_BEGUN /// after beginReplaceModel(), state for replacing geometry primitives - }; +enum BVHBuildState { + BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor + BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry + /// primitives + BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use + BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for + /// updating geometry primitives + BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, + /// ready for ccd use + BVH_BUILD_STATE_REPLACE_BEGUN /// after beginReplaceModel(), state for + /// replacing geometry primitives +}; -/// @brief Error code for BVH -enum BVHReturnCode - { - BVH_OK = 0, /// BVH is valid - BVH_ERR_MODEL_OUT_OF_MEMORY = -1, /// Cannot allocate memory for vertices and triangles - BVH_ERR_BUILD_OUT_OF_SEQUENCE = -2, /// BVH construction does not follow correct sequence - BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared - BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -4, /// BVH geometry in previous frame is not prepared - BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported - BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed - BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid - BVH_ERR_UNKNOWN = -8 /// Unknown failure - }; +/// @brief Error code for BVH +enum BVHReturnCode { + BVH_OK = 0, /// BVH is valid + BVH_ERR_MODEL_OUT_OF_MEMORY = + -1, /// Cannot allocate memory for vertices and triangles + BVH_ERR_BUILD_OUT_OF_SEQUENCE = + -2, /// BVH construction does not follow correct sequence + BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared + BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = + -4, /// BVH geometry in previous frame is not prepared + BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported + BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed + BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid + BVH_ERR_UNKNOWN = -8 /// Unknown failure +}; /// @brief BVH model type -enum BVHModelType - { - BVH_MODEL_UNKNOWN, /// @brief unknown model type - BVH_MODEL_TRIANGLES, /// @brief triangle model - BVH_MODEL_POINTCLOUD /// @brief point cloud model - }; +enum BVHModelType { + BVH_MODEL_UNKNOWN, /// @brief unknown model type + BVH_MODEL_TRIANGLES, /// @brief triangle model + BVH_MODEL_POINTCLOUD /// @brief point cloud model +}; +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h index da073d446..cc88f0fb0 100644 --- a/include/hpp/fcl/BVH/BVH_model.h +++ b/include/hpp/fcl/BVH/BVH_model.h @@ -45,25 +45,23 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @addtogroup Construction_Of_BVH /// @{ class ConvexBase; -template class BVFitter; -template class BVSplitter; - -/// @brief A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) -class HPP_FCL_DLLAPI BVHModelBase -: public CollisionGeometry -{ -public: +template +class BVFitter; +template +class BVSplitter; +/// @brief A base class describing the bounding hierarchy of a mesh model or a +/// point cloud model (which is viewed as a degraded version of mesh) +class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry { + public: /// @brief Geometry point data Vec3f* vertices; @@ -83,14 +81,13 @@ class HPP_FCL_DLLAPI BVHModelBase BVHBuildState build_state; /// @brief Convex representation of this object - shared_ptr< ConvexBase > convex; + shared_ptr convex; /// @brief Model type described by the instance - BVHModelType getModelType() const - { - if(num_tris && num_vertices) + BVHModelType getModelType() const { + if (num_tris && num_vertices) return BVH_MODEL_TRIANGLES; - else if(num_vertices) + else if (num_vertices) return BVH_MODEL_POINTCLOUD; else return BVH_MODEL_UNKNOWN; @@ -103,11 +100,10 @@ class HPP_FCL_DLLAPI BVHModelBase BVHModelBase(const BVHModelBase& other); /// @brief deconstruction, delete mesh data related. - virtual ~BVHModelBase () - { - delete [] vertices; - delete [] tri_indices; - delete [] prev_vertices; + virtual ~BVHModelBase() { + delete[] vertices; + delete[] tri_indices; + delete[] prev_vertices; } /// @brief Get the object type: it is a BVH @@ -121,26 +117,29 @@ class HPP_FCL_DLLAPI BVHModelBase /// @brief Add one point in the new BVH model int addVertex(const Vec3f& p); - + /// @brief Add points in the new BVH model - int addVertices(const Matrixx3f & points); - + int addVertices(const Matrixx3f& points); + /// @brief Add triangles in the new BVH model - int addTriangles(const Matrixx3i & triangles); + int addTriangles(const Matrixx3i& triangles); /// @brief Add one triangle in the new BVH model int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); /// @brief Add a set of triangles in the new BVH model - int addSubModel(const std::vector& ps, const std::vector& ts); + int addSubModel(const std::vector& ps, + const std::vector& ts); /// @brief Add a set of points in the new BVH model int addSubModel(const std::vector& ps); - /// @brief End BVH model construction, will build the bounding volume hierarchy + /// @brief End BVH model construction, will build the bounding volume + /// hierarchy int endModel(); - /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame) + /// @brief Replace the geometry information of current frame (i.e. should have + /// the same mesh topology with the previous frame) int beginReplaceModel(); /// @brief Replace one point in the old BVH model @@ -152,11 +151,13 @@ class HPP_FCL_DLLAPI BVHModelBase /// @brief Replace a set of points in the old BVH model int replaceSubModel(const std::vector& ps); - /// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy + /// @brief End BVH model replacement, will also refit or rebuild the bounding + /// volume hierarchy int endReplaceModel(bool refit = true, bool bottomup = true); - /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame). - /// The current frame will be saved as the previous frame in prev_vertices. + /// @brief Replace the geometry information of current frame (i.e. should have + /// the same mesh topology with the previous frame). The current frame will be + /// saved as the previous frame in prev_vertices. int beginUpdateModel(); /// @brief Update one point in the old BVH model @@ -168,12 +169,13 @@ class HPP_FCL_DLLAPI BVHModelBase /// @brief Update a set of points in the old BVH model int updateSubModel(const std::vector& ps); - /// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy + /// @brief End BVH model update, will also refit or rebuild the bounding + /// volume hierarchy int endUpdateModel(bool refit = true, bool bottomup = true); - /// @brief Build this \ref Convex "Convex" representation of this model. - /// The result is stored in attribute \ref convex. - /// \note this only takes the points of this model. It does not check that the + /// @brief Build this \ref Convex "Convex" representation of this + /// model. The result is stored in attribute \ref convex. \note this only + /// takes the points of this model. It does not check that the /// object is convex. It does not compute a convex hull. void buildConvexRepresentation(bool share_memory); @@ -192,61 +194,60 @@ class HPP_FCL_DLLAPI BVHModelBase virtual int memUsage(const bool msg = false) const = 0; - /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent - /// BV node. When traversing the BVH, this can save one matrix transformation. + /// @brief This is a special acceleration: BVH_model default stores the BV's + /// transform in world coordinate. However, we can also store each BV's + /// transform related to its parent BV node. When traversing the BVH, this can + /// save one matrix transformation. virtual void makeParentRelative() = 0; - Vec3f computeCOM() const - { + Vec3f computeCOM() const { FCL_REAL vol = 0; - Vec3f com(0,0,0); - for(unsigned int i = 0; i < num_tris; ++i) - { + Vec3f com(0, 0, 0); + for (unsigned int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; - FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); + FCL_REAL d_six_vol = + (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); vol += d_six_vol; - com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol; + com += + (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol; } return com / (vol * 4); } - FCL_REAL computeVolume() const - { + FCL_REAL computeVolume() const { FCL_REAL vol = 0; - for(unsigned int i = 0; i < num_tris; ++i) - { + for (unsigned int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; - FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); + FCL_REAL d_six_vol = + (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); vol += d_six_vol; } return vol / 6; } - Matrix3f computeMomentofInertia() const - { + Matrix3f computeMomentofInertia() const { Matrix3f C = Matrix3f::Zero(); Matrix3f C_canonical; - C_canonical << 1/60.0, 1/120.0, 1/120.0, - 1/120.0, 1/60.0, 1/120.0, - 1/120.0, 1/120.0, 1/60.0; + C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, + 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0; - for(unsigned int i = 0; i < num_tris; ++i) - { + for (unsigned int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; const Vec3f& v1 = vertices[tri[0]]; const Vec3f& v2 = vertices[tri[1]]; const Vec3f& v3 = vertices[tri[2]]; - Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); + Matrix3f A; + A << v1.transpose(), v2.transpose(), v3.transpose(); C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); } return C.trace() * Matrix3f::Identity() - C; } -protected: + protected: virtual void deleteBVs() = 0; virtual bool allocateBVs() = 0; @@ -258,22 +259,21 @@ class HPP_FCL_DLLAPI BVHModelBase unsigned int num_tris_allocated; unsigned int num_vertices_allocated; - unsigned int num_vertex_updated; /// for ccd vertex update - -protected: - + unsigned int num_vertex_updated; /// for ccd vertex update + + protected: /// \brief Comparison operators - virtual bool isEqual(const CollisionGeometry & other) const; + virtual bool isEqual(const CollisionGeometry& other) const; }; -/// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) -/// \tparam BV one of the bounding volume class in \ref Bounding_Volume. -template -class HPP_FCL_DLLAPI BVHModel -: public BVHModelBase -{ +/// @brief A class describing the bounding hierarchy of a mesh model or a point +/// cloud model (which is viewed as a degraded version of mesh) \tparam BV one +/// of the bounding volume class in \ref Bounding_Volume. +template +class HPP_FCL_DLLAPI BVHModel : public BVHModelBase { typedef BVHModelBase Base; -public: + + public: /// @brief Split rule to split one BV node into two children shared_ptr > bv_splitter; @@ -288,38 +288,33 @@ class HPP_FCL_DLLAPI BVHModel /// \param[in] other BVHModel to copy. /// BVHModel(const BVHModel& other); - + /// @brief Clone *this into a new BVHModel - virtual BVHModel * clone() const { return new BVHModel(*this); } + virtual BVHModel* clone() const { return new BVHModel(*this); } /// @brief deconstruction, delete mesh data related. - ~BVHModel() - { - delete [] bvs; - delete [] primitive_indices; + ~BVHModel() { + delete[] bvs; + delete[] primitive_indices; } - /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here - + /// @brief We provide getBV() and getNumBVs() because BVH may be compressed + /// (in future), so we must provide some flexibility here + /// @brief Access the bv giving the its index - const BVNode& getBV(unsigned int i) const - { - assert (i < num_bvs); + const BVNode& getBV(unsigned int i) const { + assert(i < num_bvs); return bvs[i]; } /// @brief Access the bv giving the its index - BVNode& getBV(unsigned int i) - { - assert (i < num_bvs); + BVNode& getBV(unsigned int i) { + assert(i < num_bvs); return bvs[i]; } /// @brief Get the number of bv in the BVH - unsigned int getNumBVs() const - { - return num_bvs; - } + unsigned int getNumBVs() const { return num_bvs; } /// @brief Get the BV type: default is unknown NODE_TYPE getNodeType() const { return BV_UNKNOWN; } @@ -327,15 +322,16 @@ class HPP_FCL_DLLAPI BVHModel /// @brief Check the number of memory used int memUsage(const bool msg) const; - /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent - /// BV node. When traversing the BVH, this can save one matrix transformation. - void makeParentRelative() - { - Matrix3f I (Matrix3f::Identity()); + /// @brief This is a special acceleration: BVH_model default stores the BV's + /// transform in world coordinate. However, we can also store each BV's + /// transform related to its parent BV node. When traversing the BVH, this can + /// save one matrix transformation. + void makeParentRelative() { + Matrix3f I(Matrix3f::Identity()); makeParentRelativeRecurse(0, I, Vec3f()); } -protected: + protected: void deleteBVs(); bool allocateBVs(); @@ -354,48 +350,50 @@ class HPP_FCL_DLLAPI BVHModel /// @brief Refit the bounding volume hierarchy int refitTree(bool bottomup); - /// @brief Refit the bounding volume hierarchy in a top-down way (slow but more compact) + /// @brief Refit the bounding volume hierarchy in a top-down way (slow but + /// more compact) int refitTree_topdown(); - /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but less compact) + /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but + /// less compact) int refitTree_bottomup(); /// @brief Recursive kernel for hierarchy construction - int recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives); + int recursiveBuildTree(int bv_id, unsigned int first_primitive, + unsigned int num_primitives); - /// @brief Recursive kernel for bottomup refitting + /// @brief Recursive kernel for bottomup refitting int recursiveRefitTree_bottomup(int bv_id); - /// @ recursively compute each bv's transform related to its parent. For default BV, only the translation works. - /// For oriented BV (OBB, RSS, OBBRSS), special implementation is provided. - void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c) - { - if(!bvs[bv_id].isLeaf()) - { - makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, bvs[bv_id].getCenter()); - - makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, bvs[bv_id].getCenter()); + /// @ recursively compute each bv's transform related to its parent. For + /// default BV, only the translation works. For oriented BV (OBB, RSS, + /// OBBRSS), special implementation is provided. + void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, + const Vec3f& parent_c) { + if (!bvs[bv_id].isLeaf()) { + makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, + bvs[bv_id].getCenter()); + + makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, + bvs[bv_id].getCenter()); } bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c); } - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const BVHModel * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const BVHModel & other = *other_ptr; - + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const BVHModel* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const BVHModel& other = *other_ptr; + bool res = Base::isEqual(other); - if(!res) - return false; + if (!res) return false; // unsigned int other_num_primitives = 0; // if(other.primitive_indices) // { - + // switch(other.getModelType()) // { // case BVH_MODEL_TRIANGLES: @@ -408,85 +406,85 @@ class HPP_FCL_DLLAPI BVHModel // ; // } // } - -// unsigned int num_primitives = 0; -// if(primitive_indices) -// { -// -// switch(other.getModelType()) -// { -// case BVH_MODEL_TRIANGLES: -// num_primitives = num_tris; -// break; -// case BVH_MODEL_POINTCLOUD: -// num_primitives = num_vertices; -// break; -// default: -// ; -// } -// } -// -// if(num_primitives != other_num_primitives) -// return false; -// -// for(int k = 0; k < num_primitives; ++k) -// { -// if(primitive_indices[k] != other.primitive_indices[k]) -// return false; -// } - - if(num_bvs != other.num_bvs) - return false; - - for(unsigned int k = 0; k < num_bvs; ++k) - { - if(bvs[k] != other.bvs[k]) - return false; + + // unsigned int num_primitives = 0; + // if(primitive_indices) + // { + // + // switch(other.getModelType()) + // { + // case BVH_MODEL_TRIANGLES: + // num_primitives = num_tris; + // break; + // case BVH_MODEL_POINTCLOUD: + // num_primitives = num_vertices; + // break; + // default: + // ; + // } + // } + // + // if(num_primitives != other_num_primitives) + // return false; + // + // for(int k = 0; k < num_primitives; ++k) + // { + // if(primitive_indices[k] != other.primitive_indices[k]) + // return false; + // } + + if (num_bvs != other.num_bvs) return false; + + for (unsigned int k = 0; k < num_bvs; ++k) { + if (bvs[k] != other.bvs[k]) return false; } - + return true; } }; /// @} -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c); - -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c); +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, + const Vec3f& parent_c); -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c); +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, + const Vec3f& parent_c); +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, + Matrix3f& parent_axes, + const Vec3f& parent_c); /// @brief Specialization of getNodeType() for BVHModel with different BV types -template<> +template <> NODE_TYPE BVHModel::getNodeType() const; -template<> +template <> NODE_TYPE BVHModel::getNodeType() const; -template<> +template <> NODE_TYPE BVHModel::getNodeType() const; -template<> +template <> NODE_TYPE BVHModel::getNodeType() const; -template<> +template <> NODE_TYPE BVHModel::getNodeType() const; -template<> +template <> NODE_TYPE BVHModel >::getNodeType() const; -template<> +template <> NODE_TYPE BVHModel >::getNodeType() const; -template<> +template <> NODE_TYPE BVHModel >::getNodeType() const; -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/hpp/fcl/BVH/BVH_utility.h index 4abe809c3..508af2fec 100644 --- a/include/hpp/fcl/BVH/BVH_utility.h +++ b/include/hpp/fcl/BVH/BVH_utility.h @@ -35,64 +35,85 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_BVH_UTILITY_H #define HPP_FCL_BVH_UTILITY_H #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Extract the part of the BVHModel that is inside an AABB. /// A triangle in collision with the AABB is considered inside. -template -HPP_FCL_DLLAPI -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb); +template +HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); -template<> -HPP_FCL_DLLAPI -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb); -template<> -HPP_FCL_DLLAPI -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb); -template<> -HPP_FCL_DLLAPI -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb); -template<> -HPP_FCL_DLLAPI -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb); -template<> -HPP_FCL_DLLAPI -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb); -template<> -HPP_FCL_DLLAPI -BVHModel >* BVHExtract(const BVHModel >& model, const Transform3f& pose, const AABB& aabb); -template<> -HPP_FCL_DLLAPI -BVHModel >* BVHExtract(const BVHModel >& model, const Transform3f& pose, const AABB& aabb); -template<> -HPP_FCL_DLLAPI -BVHModel >* BVHExtract(const BVHModel >& model, const Transform3f& pose, const AABB& aabb); +template <> +HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); +template <> +HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); +template <> +HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); +template <> +HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); +template <> +HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); +template <> +HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3f& pose, + const AABB& aabb); +template <> +HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3f& pose, + const AABB& aabb); +template <> +HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3f& pose, + const AABB& aabb); -/// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles -HPP_FCL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, Matrix3f& M); +/// @brief Compute the covariance matrix for a set or subset of points. if ts = +/// null, then indices refer to points directly; otherwise refer to triangles +HPP_FCL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + Matrix3f& M); -/// @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises. -HPP_FCL_DLLAPI void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r); +/// @brief Compute the RSS bounding volume parameters: radius, rectangle size +/// and the origin, given the BV axises. +HPP_FCL_DLLAPI void getRadiusAndOriginAndRectangleSize( + Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, + const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r); -/// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises. -HPP_FCL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, Matrix3f& axes, Vec3f& center, Vec3f& extent); +/// @brief Compute the bounding volume extent and center for a set or subset of +/// points, given the BV axises. +HPP_FCL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + Matrix3f& axes, Vec3f& center, + Vec3f& extent); /// @brief Compute the center and radius for a triangle's circumcircle -HPP_FCL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius); +HPP_FCL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b, + const Vec3f& c, Vec3f& center, + FCL_REAL& radius); -/// @brief Compute the maximum distance from a given center point to a point cloud -HPP_FCL_DLLAPI FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, const Vec3f& query); +/// @brief Compute the maximum distance from a given center point to a point +/// cloud +HPP_FCL_DLLAPI FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + const Vec3f& query); -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_SSaP.h b/include/hpp/fcl/broadphase/broadphase_SSaP.h index 5f102fc3b..b99eb90c8 100644 --- a/include/hpp/fcl/broadphase/broadphase_SSaP.h +++ b/include/hpp/fcl/broadphase/broadphase_SSaP.h @@ -31,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 Jia Pan */ @@ -41,19 +41,15 @@ #include #include "hpp/fcl/broadphase/broadphase_collision_manager.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -/// @brief Simple SAP collision manager -class HPP_FCL_DLLAPI SSaPCollisionManager -: public BroadPhaseCollisionManager -{ -public: +/// @brief Simple SAP collision manager +class HPP_FCL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { + public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; - + SSaPCollisionManager(); /// @brief remove one object from the manager @@ -74,42 +70,56 @@ class HPP_FCL_DLLAPI SSaPCollisionManager /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; - /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase * callback) const; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase * callback) const; + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(CollisionCallBackBase * callback) const; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(DistanceCallBackBase * callback) const; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase * callback) const; + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase * callback) const; + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; -protected: - /// @brief check collision between one object and a list of objects, return value is whether stop is possible - bool checkColl(typename std::vector::const_iterator pos_start, typename std::vector::const_iterator pos_end, - CollisionObject* obj, CollisionCallBackBase * callback) const; - - /// @brief check distance between one object and a list of objects, return value is whether stop is possible - bool checkDis(typename std::vector::const_iterator pos_start, typename std::vector::const_iterator pos_end, - CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const; + protected: + /// @brief check collision between one object and a list of objects, return + /// value is whether stop is possible + bool checkColl( + typename std::vector::const_iterator pos_start, + typename std::vector::const_iterator pos_end, + CollisionObject* obj, CollisionCallBackBase* callback) const; + + /// @brief check distance between one object and a list of objects, return + /// value is whether stop is possible + bool checkDis( + typename std::vector::const_iterator pos_start, + typename std::vector::const_iterator pos_end, + CollisionObject* obj, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const; + + bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; - bool collide_(CollisionObject* obj, CollisionCallBackBase * callback) const; - - bool distance_(CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const; + bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const; static int selectOptimalAxis( const std::vector& objs_x, @@ -127,11 +137,12 @@ class HPP_FCL_DLLAPI SSaPCollisionManager /// @brief Objects sorted according to lower z value std::vector objs_z; - /// @brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly + /// @brief tag about whether the environment is maintained suitably (i.e., the + /// objs_x, objs_y, objs_z are sorted correctly bool setup_; }; -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/hpp/fcl/broadphase/broadphase_SaP.h index 2e7e19ced..0751401d1 100644 --- a/include/hpp/fcl/broadphase/broadphase_SaP.h +++ b/include/hpp/fcl/broadphase/broadphase_SaP.h @@ -31,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 Jia Pan */ @@ -43,19 +43,15 @@ #include "hpp/fcl/broadphase/broadphase_collision_manager.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Rigorous SAP collision manager -class HPP_FCL_DLLAPI SaPCollisionManager -: public BroadPhaseCollisionManager -{ -public: +class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { + public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; - + SaPCollisionManager(); ~SaPCollisionManager(); @@ -87,37 +83,41 @@ class HPP_FCL_DLLAPI SaPCollisionManager /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; - /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase * callback) const; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase * callback) const; + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(CollisionCallBackBase * callback) const; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(DistanceCallBackBase * callback) const; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase * callback) const; + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase * callback) const; + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; -protected: - + protected: struct EndPoint; /// @brief SAP interval for one object - struct SaPAABB - { + struct SaPAABB { /// @brief object CollisionObject* obj; @@ -132,9 +132,9 @@ class HPP_FCL_DLLAPI SaPCollisionManager }; /// @brief End point for an interval - struct EndPoint - { - /// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi + struct EndPoint { + /// @brief tag for whether it is a lower bound or higher bound of an + /// interval, 0 for lo, and 1 for hi char minmax; /// @brief back pointer to SAP interval @@ -150,46 +150,44 @@ class HPP_FCL_DLLAPI SaPCollisionManager const Vec3f& getVal() const; /// @brief set the value of the end point - Vec3f& getVal(); + Vec3f& getVal(); FCL_REAL getVal(size_t i) const; FCL_REAL& getVal(size_t i); - }; - /// @brief A pair of objects that are not culling away and should further check collision - struct SaPPair - { + /// @brief A pair of objects that are not culling away and should further + /// check collision + struct SaPPair { SaPPair(CollisionObject* a, CollisionObject* b); CollisionObject* obj1; CollisionObject* obj2; - bool operator == (const SaPPair& other) const; + bool operator==(const SaPPair& other) const; }; /// @brief Functor to help unregister one object - class HPP_FCL_DLLAPI isUnregistered - { + class HPP_FCL_DLLAPI isUnregistered { CollisionObject* obj; - public: + public: isUnregistered(CollisionObject* obj_); - bool operator() (const SaPPair& pair) const; + bool operator()(const SaPPair& pair) const; }; - /// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away) - class HPP_FCL_DLLAPI isNotValidPair - { + /// @brief Functor to help remove collision pairs no longer valid (i.e., + /// should be culled away) + class HPP_FCL_DLLAPI isNotValidPair { CollisionObject* obj1; CollisionObject* obj2; - public: + public: isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_); - bool operator() (const SaPPair& pair); + bool operator()(const SaPPair& pair); }; void update_(SaPAABB* updated_aabb); @@ -198,7 +196,7 @@ class HPP_FCL_DLLAPI SaPCollisionManager /// @brief End point list for x, y, z coordinates EndPoint* elist[3]; - + /// @brief vector version of elist, for acceleration std::vector velist[3]; @@ -212,16 +210,17 @@ class HPP_FCL_DLLAPI SaPCollisionManager std::map obj_aabb_map; - bool distance_(CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const; + bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const; - bool collide_(CollisionObject* obj, CollisionCallBackBase * callback) const; + bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; void addToOverlapPairs(const SaPPair& p); void removeFromOverlapPairs(const SaPPair& p); }; -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_bruteforce.h b/include/hpp/fcl/broadphase/broadphase_bruteforce.h index 9cbc3f7f6..734c74fb7 100644 --- a/include/hpp/fcl/broadphase/broadphase_bruteforce.h +++ b/include/hpp/fcl/broadphase/broadphase_bruteforce.h @@ -31,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 Jia Pan */ @@ -41,19 +41,15 @@ #include #include "hpp/fcl/broadphase/broadphase_collision_manager.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Brute force N-body collision manager -class HPP_FCL_DLLAPI NaiveCollisionManager -: public BroadPhaseCollisionManager -{ -public: +class HPP_FCL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager { + public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; - + NaiveCollisionManager(); /// @brief add objects to the manager @@ -77,38 +73,43 @@ class HPP_FCL_DLLAPI NaiveCollisionManager /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; - /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase * callback) const; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase * callback) const; + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(CollisionCallBackBase * callback) const; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(DistanceCallBackBase * callback) const; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase * callback) const; + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase * callback) const; + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; -protected: - + protected: /// @brief objects belonging to the manager are stored in a list structure std::list objs; }; -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_callbacks.h b/include/hpp/fcl/broadphase/broadphase_callbacks.h index 0bc5fa823..b9889ce0e 100644 --- a/include/hpp/fcl/broadphase/broadphase_callbacks.h +++ b/include/hpp/fcl/broadphase/broadphase_callbacks.h @@ -44,49 +44,55 @@ namespace hpp { namespace fcl { /// @brief Base callback class for collision queries. -/// This class can be supersed by child classes to provide desired behaviors according to the application (e.g, only listing the potential CollisionObjects in collision). -struct HPP_FCL_DLLAPI CollisionCallBackBase -{ - /// @brief Initialization of the callback before running the collision broadphase manager. - virtual void init() {}; - +/// This class can be supersed by child classes to provide desired behaviors +/// according to the application (e.g, only listing the potential +/// CollisionObjects in collision). +struct HPP_FCL_DLLAPI CollisionCallBackBase { + /// @brief Initialization of the callback before running the collision + /// broadphase manager. + virtual void init(){}; + /// @brief Collision evaluation between two objects in collision. - /// This callback will cause the broadphase evaluation to stop if it returns true. + /// This callback will cause the broadphase evaluation to stop if it + /// returns true. /// /// @param[in] o1 Collision object #1. /// @param[in] o2 Collision object #2. virtual bool collide(CollisionObject* o1, CollisionObject* o2) = 0; - + /// @brief Functor call associated to the collide operation. - virtual bool operator()(CollisionObject* o1, CollisionObject* o2) - { - return collide(o1,o2); + virtual bool operator()(CollisionObject* o1, CollisionObject* o2) { + return collide(o1, o2); } }; /// @brief Base callback class for distance queries. -/// This class can be supersed by child classes to provide desired behaviors according to the application (e.g, only listing the potential CollisionObjects in collision). -struct HPP_FCL_DLLAPI DistanceCallBackBase -{ - /// @brief Initialization of the callback before running the collision broadphase manager. - virtual void init() {}; - +/// This class can be supersed by child classes to provide desired behaviors +/// according to the application (e.g, only listing the potential +/// CollisionObjects in collision). +struct HPP_FCL_DLLAPI DistanceCallBackBase { + /// @brief Initialization of the callback before running the collision + /// broadphase manager. + virtual void init(){}; + /// @brief Distance evaluation between two objects in collision. - /// This callback will cause the broadphase evaluation to stop if it returns true. + /// This callback will cause the broadphase evaluation to stop if it + /// returns true. /// /// @param[in] o1 Collision object #1. /// @param[in] o2 Collision object #2. /// @param[out] dist Distance between the two collision geometries. - virtual bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL & dist) = 0; - + virtual bool distance(CollisionObject* o1, CollisionObject* o2, + FCL_REAL& dist) = 0; + /// @brief Functor call associated to the distance operation. - virtual bool operator()(CollisionObject* o1, CollisionObject* o2, FCL_REAL & dist) - { - return distance(o1,o2,dist); + virtual bool operator()(CollisionObject* o1, CollisionObject* o2, + FCL_REAL& dist) { + return distance(o1, o2, dist); } }; } // namespace fcl -} // namespace hpp +} // namespace hpp #endif // HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H diff --git a/include/hpp/fcl/broadphase/broadphase_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_collision_manager.h index 16bb8b80f..750907b28 100644 --- a/include/hpp/fcl/broadphase/broadphase_collision_manager.h +++ b/include/hpp/fcl/broadphase/broadphase_collision_manager.h @@ -31,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 Jia Pan */ @@ -45,17 +45,14 @@ #include "hpp/fcl/collision_object.h" #include "hpp/fcl/broadphase/broadphase_callbacks.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Base class for broad phase collision. It helps to accelerate the /// collision/distance between N objects. Also support self collision, self /// distance and collision/distance with another M objects. -class HPP_FCL_DLLAPI BroadPhaseCollisionManager -{ -public: +class HPP_FCL_DLLAPI BroadPhaseCollisionManager { + public: BroadPhaseCollisionManager(); virtual ~BroadPhaseCollisionManager(); @@ -86,52 +83,59 @@ class HPP_FCL_DLLAPI BroadPhaseCollisionManager /// @brief return the objects managed by the manager virtual void getObjects(std::vector& objs) const = 0; - + /// @brief return the objects managed by the manager - virtual std::vector getObjects() const - { + virtual std::vector getObjects() const { std::vector res(size()); getObjects(res); return res; }; - /// @brief perform collision test between one object and all the objects belonging to the manager - virtual void collide(CollisionObject* obj, CollisionCallBackBase * callback) const = 0; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + virtual void collide(CollisionObject* obj, + CollisionCallBackBase* callback) const = 0; + + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + virtual void distance(CollisionObject* obj, + DistanceCallBackBase* callback) const = 0; - /// @brief perform distance computation between one object and all the objects belonging to the manager - virtual void distance(CollisionObject* obj, DistanceCallBackBase * callback) const = 0; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + virtual void collide(CollisionCallBackBase* callback) const = 0; - /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - virtual void collide(CollisionCallBackBase * callback) const = 0; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + virtual void distance(DistanceCallBackBase* callback) const = 0; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - virtual void distance(DistanceCallBackBase * callback) const = 0; - /// @brief perform collision test with objects belonging to another manager - virtual void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase * callback) const = 0; - + virtual void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const = 0; + /// @brief perform distance test with objects belonging to another manager - virtual void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase * callback) const = 0; + virtual void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const = 0; /// @brief whether the manager is empty virtual bool empty() const = 0; - + /// @brief the number of objects managed by the manager virtual size_t size() const = 0; -protected: - - /// @brief tools help to avoid repeating collision or distance callback for the pairs of objects tested before. It can be useful for some of the broadphase algorithms. + protected: + /// @brief tools help to avoid repeating collision or distance callback for + /// the pairs of objects tested before. It can be useful for some of the + /// broadphase algorithms. mutable std::set > tested_set; mutable bool enable_tested_set_; bool inTestedSet(CollisionObject* a, CollisionObject* b) const; void insertTestedSet(CollisionObject* a, CollisionObject* b) const; - }; -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h index 562ee2e8b..93691b200 100644 --- a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h @@ -45,35 +45,27 @@ namespace hpp { namespace fcl { //============================================================================== -BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() -{ +BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() { // Do nothing } //============================================================================== - -BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() -{ +BroadPhaseContinuousCollisionManager::~BroadPhaseContinuousCollisionManager() { // Do nothing } //============================================================================== - void BroadPhaseContinuousCollisionManager::registerObjects( - const std::vector& other_objs) -{ - for(size_t i = 0; i < other_objs.size(); ++i) - registerObject(other_objs[i]); + const std::vector& other_objs) { + for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); } //============================================================================== - void BroadPhaseContinuousCollisionManager::update( - ContinuousCollisionObject* updated_obj) -{ + ContinuousCollisionObject* updated_obj) { HPP_FCL_UNUSED_VARIABLE(updated_obj); update(); @@ -81,17 +73,15 @@ void BroadPhaseContinuousCollisionManager::update( //============================================================================== - void BroadPhaseContinuousCollisionManager::update( - const std::vector& updated_objs) -{ + const std::vector& updated_objs) { HPP_FCL_UNUSED_VARIABLE(updated_objs); update(); } -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h index 8ea39af78..79bfe6aa6 100644 --- a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h +++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h @@ -31,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 Jia Pan */ @@ -42,38 +42,36 @@ #include "hpp/fcl/collision_object.h" #include "hpp/fcl/narrowphase/continuous_collision_object.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Callback for continuous collision between two objects. Return value /// is whether can stop now. template -using ContinuousCollisionCallBack = bool (*)( - ContinuousCollisionObject* o1, - ContinuousCollisionObject* o2, void* cdata); +using ContinuousCollisionCallBack = bool (*)(ContinuousCollisionObject* o1, + ContinuousCollisionObject* o2, + void* cdata); /// @brief Callback for continuous distance between two objects, Return value is /// whether can stop now, also return the minimum distance till now. template -using ContinuousDistanceCallBack = bool (*)( - ContinuousCollisionObject* o1, - ContinuousCollisionObject* o2, S& dist); +using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1, + ContinuousCollisionObject* o2, + S& dist); /// @brief Base class for broad phase continuous collision. It helps to /// accelerate the continuous collision/distance between N objects. Also support /// self collision, self distance and collision/distance with another M objects. template -class HPP_FCL_DLLAPI BroadPhaseContinuousCollisionManager -{ -public: +class HPP_FCL_DLLAPI BroadPhaseContinuousCollisionManager { + public: BroadPhaseContinuousCollisionManager(); virtual ~BroadPhaseContinuousCollisionManager(); /// @brief add objects to the manager - virtual void registerObjects(const std::vector& other_objs); + virtual void registerObjects( + const std::vector& other_objs); /// @brief add one object to the manager virtual void registerObject(ContinuousCollisionObject* obj) = 0; @@ -91,45 +89,57 @@ class HPP_FCL_DLLAPI BroadPhaseContinuousCollisionManager virtual void update(ContinuousCollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update - virtual void update(const std::vector& updated_objs); + virtual void update( + const std::vector& updated_objs); /// @brief clear the manager virtual void clear() = 0; /// @brief return the objects managed by the manager - virtual void getObjects(std::vector& objs) const = 0; + virtual void getObjects( + std::vector& objs) const = 0; - /// @brief perform collision test between one object and all the objects belonging to the manager - virtual void collide(ContinuousCollisionObject* obj, CollisionCallBackBase * callback) const = 0; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + virtual void collide(ContinuousCollisionObject* obj, + CollisionCallBackBase* callback) const = 0; - /// @brief perform distance computation between one object and all the objects belonging to the manager - virtual void distance(ContinuousCollisionObject* obj, DistanceCallBackBase * callback) const = 0; + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + virtual void distance(ContinuousCollisionObject* obj, + DistanceCallBackBase* callback) const = 0; - /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - virtual void collide(CollisionCallBackBase * callback) const = 0; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + virtual void collide(CollisionCallBackBase* callback) const = 0; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - virtual void distance(DistanceCallBackBase * callback) const = 0; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + virtual void distance(DistanceCallBackBase* callback) const = 0; /// @brief perform collision test with objects belonging to another manager - virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, CollisionCallBackBase * callback) const = 0; + virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, + CollisionCallBackBase* callback) const = 0; /// @brief perform distance test with objects belonging to another manager - virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, DistanceCallBackBase * callback) const = 0; + virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, + DistanceCallBackBase* callback) const = 0; /// @brief whether the manager is empty virtual bool empty() const = 0; - + /// @brief the number of objects managed by the manager virtual size_t size() const = 0; }; -using BroadPhaseContinuousCollisionManagerf = BroadPhaseContinuousCollisionManager; -using BroadPhaseContinuousCollisionManagerd = BroadPhaseContinuousCollisionManager; +using BroadPhaseContinuousCollisionManagerf = + BroadPhaseContinuousCollisionManager; +using BroadPhaseContinuousCollisionManagerd = + BroadPhaseContinuousCollisionManager; -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp #include "hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h" diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h index 956f107a9..0ef013214 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -60,55 +60,46 @@ namespace dynamic_AABB_tree { //============================================================================== template bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, + const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Eigen::MatrixBase& translation2, - CollisionCallBackBase * callback) -{ - if(!root2) - { - if(root1->isLeaf()) - { + CollisionCallBackBase* callback) { + if (!root2) { + if (root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); - if(!obj1->collisionGeometry()->isFree()) - { + if (!obj1->collisionGeometry()->isFree()) { const AABB& root2_bv_t = translate(root2_bv, translation2); - if(root1->bv.overlap(root2_bv_t)) - { + if (root1->bv.overlap(root2_bv_t)) { Box* box = new Box(); Transform3f box_tf; Transform3f tf2 = Transform3f::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); - box->cost_density = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain + box->cost_density = + tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); } } - } - else - { - if(collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, translation2, callback)) + } else { + if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, + translation2, callback)) return true; - if(collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, translation2, callback)) + if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, + translation2, callback)) return true; } return false; - } - else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { + } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); - if(!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) - { + if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { const AABB& root2_bv_t = translate(root2_bv, translation2); - if(root1->bv.overlap(root2_bv_t)) - { + if (root1->bv.overlap(root2_bv_t)) { Box* box = new Box(); Transform3f box_tf; Transform3f tf2 = Transform3f::Identity(); @@ -120,40 +111,38 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); - } - else return false; - } - else return false; + } else + return false; + } else + return false; } const AABB& root2_bv_t = translate(root2_bv, translation2); - if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; + if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, callback)) + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { + if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv, + translation2, callback)) return true; - if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, callback)) + if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv, + translation2, callback)) return true; - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(root1, tree2, child, child_bv, translation2, callback)) + if (collisionRecurse_(root1, tree2, child, child_bv, translation2, + callback)) return true; - } - else - { + } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(root1, tree2, nullptr, child_bv, translation2, callback)) + if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2, + callback)) return true; } } @@ -163,73 +152,61 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== template -bool distanceRecurse_( - DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Eigen::MatrixBase& translation2, - DistanceCallBackBase * callback, - FCL_REAL& min_dist) -{ - if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - if(tree2->isNodeOccupied(root2)) - { +bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Eigen::MatrixBase& translation2, + DistanceCallBackBase* callback, FCL_REAL& min_dist) { + if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { + if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3f box_tf; Transform3f tf2 = Transform3f::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); - return (*callback)(static_cast(root1->data), &obj, min_dist); - } - else return false; + return (*callback)(static_cast(root1->data), &obj, + min_dist); + } else + return false; } - if(!tree2->isNodeOccupied(root2)) return false; + if (!tree2->isNodeOccupied(root2)) return false; - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, translation2); FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, + translation2, callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, + translation2, callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, + translation2, callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, + translation2, callback, min_dist)) return true; } } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -237,9 +214,9 @@ bool distanceRecurse_( FCL_REAL d = root1->bv.distance(aabb2); - if(d < min_dist) - { - if(distanceRecurse_(root1, tree2, child, child_bv, translation2, callback, min_dist)) + if (d < min_dist) { + if (distanceRecurse_(root1, tree2, child, child_bv, translation2, + callback, min_dist)) return true; } } @@ -251,9 +228,9 @@ bool distanceRecurse_( #endif -} // namespace dynamic_AABB_tree -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace dynamic_AABB_tree +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h index 9bae5dd24..7c80aea85 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -31,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 Jia Pan */ @@ -48,20 +48,18 @@ #include "hpp/fcl/broadphase/broadphase_collision_manager.h" #include "hpp/fcl/broadphase/detail/hierarchy_tree.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager -: public BroadPhaseCollisionManager -{ -public: + : public BroadPhaseCollisionManager { + public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; using DynamicAABBNode = detail::NodeBase; - using DynamicAABBTable = std::unordered_map ; + using DynamicAABBTable = + std::unordered_map; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; @@ -76,7 +74,7 @@ class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); - + /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -101,37 +99,43 @@ class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; - /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase * callback) const; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase * callback) const; + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(CollisionCallBackBase * callback) const; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(DistanceCallBackBase * callback) const; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase * callback) const; + void collide(BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase * callback) const; - + void distance(BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const; + /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; /// @brief returns the AABB tree structure. const detail::HierarchyTree& getTree() const; - + /// @brief returns the AABB tree structure. - detail::HierarchyTree & getTree(); + detail::HierarchyTree& getTree(); -private: + private: detail::HierarchyTree dtree{}; std::unordered_map table; @@ -140,9 +144,9 @@ class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager void update_(CollisionObject* updated_obj); }; -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h" diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index ec4df3bd3..439f93b11 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -45,14 +45,10 @@ #include "hpp/fcl/octree.h" #endif -namespace hpp -{ -namespace fcl -{ -namespace detail -{ -namespace dynamic_AABB_tree_array -{ +namespace hpp { +namespace fcl { +namespace detail { +namespace dynamic_AABB_tree_array { #if HPP_FCL_HAVE_OCTOMAP @@ -60,25 +56,18 @@ namespace dynamic_AABB_tree_array template bool collisionRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, - size_t root1_id, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Eigen::MatrixBase& translation2, - CollisionCallBackBase * callback) -{ - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; - if(!root2) - { - if(root1->isLeaf()) - { + size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Eigen::MatrixBase& translation2, + CollisionCallBackBase* callback) { + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = + nodes1 + root1_id; + if (!root2) { + if (root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); - if(!obj1->collisionGeometry()->isFree()) - { + if (!obj1->collisionGeometry()->isFree()) { const AABB& root_bv_t = translate(root2_bv, translation2); - if(root1->bv.overlap(root_bv_t)) - { + if (root1->bv.overlap(root_bv_t)) { Box* box = new Box(); Transform3f box_tf; Transform3f tf2 = Transform3f::Identity(); @@ -91,25 +80,21 @@ bool collisionRecurse_( return (*callback)(obj1, &obj2); } } - } - else - { - if(collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, root2_bv, translation2, callback)) + } else { + if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, + root2_bv, translation2, callback)) return true; - if(collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, root2_bv, translation2, callback)) + if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, + root2_bv, translation2, callback)) return true; } return false; - } - else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { + } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); - if(!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) - { + if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { const AABB& root_bv_t = translate(root2_bv, translation2); - if(root1->bv.overlap(root_bv_t)) - { + if (root1->bv.overlap(root_bv_t)) { Box* box = new Box(); Transform3f box_tf; Transform3f tf2 = Transform3f::Identity(); @@ -121,40 +106,38 @@ bool collisionRecurse_( CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); - } - else return false; - } - else return false; + } else + return false; + } else + return false; } const AABB& root_bv_t = translate(root2_bv, translation2); - if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; + if (tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, callback)) + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { + if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + translation2, callback)) return true; - if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, callback)) + if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + translation2, callback)) return true; - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, callback)) + if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, + translation2, callback)) return true; - } - else - { + } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, translation2, callback)) + if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, + translation2, callback)) return true; } } @@ -167,74 +150,62 @@ bool collisionRecurse_( template bool distanceRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, - size_t root1_id, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Eigen::MatrixBase& translation2, - DistanceCallBackBase * callback, - FCL_REAL& min_dist) -{ - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; - if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - if(tree2->isNodeOccupied(root2)) - { + size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Eigen::MatrixBase& translation2, + DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = + nodes1 + root1_id; + if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { + if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3f box_tf; Transform3f tf2 = Transform3f::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); - return (*callback)(static_cast(root1->data), &obj, min_dist); - } - else return false; + return (*callback)(static_cast(root1->data), &obj, + min_dist); + } else + return false; } - if(!tree2->isNodeOccupied(root2)) return false; + if (!tree2->isNodeOccupied(root2)) return false; - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, translation2); FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + translation2, callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + translation2, callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + translation2, callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + translation2, callback, min_dist)) return true; } } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -242,9 +213,9 @@ bool distanceRecurse_( const AABB& aabb2 = translate(child_bv, translation2); FCL_REAL d = root1->bv.distance(aabb2); - if(d < min_dist) - { - if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, callback, min_dist)) + if (d < min_dist) { + if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, + translation2, callback, min_dist)) return true; } } @@ -254,12 +225,11 @@ bool distanceRecurse_( return false; } - #endif -} // namespace dynamic_AABB_tree_array -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace dynamic_AABB_tree_array +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h index dd9e06d79..8761b72a8 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -31,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 Jia Pan */ @@ -49,15 +49,12 @@ #include "hpp/fcl/broadphase/broadphase_collision_manager.h" #include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager -: public BroadPhaseCollisionManager -{ -public: + : public BroadPhaseCollisionManager { + public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; @@ -72,12 +69,12 @@ class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager bool octree_as_geometry_collide; bool octree_as_geometry_distance; - + DynamicAABBTreeArrayCollisionManager(); /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); - + /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -102,33 +99,39 @@ class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; - /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase * callback) const; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase * callback) const; + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(CollisionCallBackBase * callback) const; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(DistanceCallBackBase * callback) const; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase * callback) const; + void collide(BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase * callback) const; - + void distance(BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const; + /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; const detail::implementation_array::HierarchyTree& getTree() const; -private: + private: detail::implementation_array::HierarchyTree dtree{}; std::unordered_map table; @@ -137,9 +140,9 @@ class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager void update_(CollisionObject* updated_obj); }; -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h" diff --git a/include/hpp/fcl/broadphase/broadphase_interval_tree.h b/include/hpp/fcl/broadphase/broadphase_interval_tree.h index 680ffea1d..78b1037d0 100644 --- a/include/hpp/fcl/broadphase/broadphase_interval_tree.h +++ b/include/hpp/fcl/broadphase/broadphase_interval_tree.h @@ -31,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 Jia Pan */ @@ -44,19 +44,16 @@ #include "hpp/fcl/broadphase/broadphase_collision_manager.h" #include "hpp/fcl/broadphase/detail/interval_tree.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Collision manager based on interval tree class HPP_FCL_DLLAPI IntervalTreeCollisionManager -: public BroadPhaseCollisionManager -{ -public: + : public BroadPhaseCollisionManager { + public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; - + IntervalTreeCollisionManager(); ~IntervalTreeCollisionManager(); @@ -85,51 +82,56 @@ class HPP_FCL_DLLAPI IntervalTreeCollisionManager /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; - /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase * callback) const; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, DistanceCallBackBase * callback) const; + /// @brief perform distance computation between one object and all the objects + /// belonging to the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(CollisionCallBackBase * callback) const; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e., N^2 self collision) + void collide(CollisionCallBackBase* callback) const; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(DistanceCallBackBase * callback) const; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase * callback) const; + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase * callback) const; + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; -protected: - + protected: /// @brief SAP end point /// @brief SAP end point - struct HPP_FCL_DLLAPI EndPoint - { + struct HPP_FCL_DLLAPI EndPoint { /// @brief object related with the end point CollisionObject* obj; /// @brief end point value FCL_REAL value; - /// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi + /// @brief tag for whether it is a lower bound or higher bound of an + /// interval, 0 for lo, and 1 for hi char minmax; - bool operator<(const EndPoint &p) const; + bool operator<(const EndPoint& p) const; }; - /// @brief Extention interval tree's interval to SAP interval, adding more information - struct HPP_FCL_DLLAPI SAPInterval : public detail::SimpleInterval - { + /// @brief Extention interval tree's interval to SAP interval, adding more + /// information + struct HPP_FCL_DLLAPI SAPInterval : public detail::SimpleInterval { CollisionObject* obj; SAPInterval(FCL_REAL low_, FCL_REAL high_, CollisionObject* obj_); @@ -138,19 +140,18 @@ class HPP_FCL_DLLAPI IntervalTreeCollisionManager bool checkColl( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, - CollisionObject* obj, - CollisionCallBackBase * callback) const; + CollisionObject* obj, CollisionCallBackBase* callback) const; bool checkDist( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, - CollisionObject* obj, - DistanceCallBackBase * callback, + CollisionObject* obj, DistanceCallBackBase* callback, FCL_REAL& min_dist) const; - bool collide_(CollisionObject* obj, CollisionCallBackBase * callback) const; + bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; - bool distance_(CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const; + bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const; /// @brief vector stores all the end points std::vector endpoints[3]; @@ -164,9 +165,8 @@ class HPP_FCL_DLLAPI IntervalTreeCollisionManager bool setup_; }; +} // namespace fcl -} // namespace fcl - -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h index 957fdd714..29c462c3e 100644 --- a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h @@ -31,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 Jia Pan */ @@ -40,50 +40,40 @@ #include "hpp/fcl/broadphase/broadphase_spatialhash.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { //============================================================================== -template +template SpatialHashingCollisionManager::SpatialHashingCollisionManager( - FCL_REAL cell_size, - const Vec3f& scene_min, - const Vec3f& scene_max, + FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size) - : scene_limit(AABB(scene_min, scene_max)), - hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) -{ + : scene_limit(AABB(scene_min, scene_max)), + hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) { hash_table->init(default_table_size); } //============================================================================== -template -SpatialHashingCollisionManager::~SpatialHashingCollisionManager() -{ +template +SpatialHashingCollisionManager::~SpatialHashingCollisionManager() { delete hash_table; } //============================================================================== -template +template void SpatialHashingCollisionManager::registerObject( - CollisionObject* obj) -{ + CollisionObject* obj) { objs.push_back(obj); const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { - if(!scene_limit.contain(obj_aabb)) + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { + if (!scene_limit.contain(obj_aabb)) objs_partially_penetrating_scene_limit.push_back(obj); hash_table->insert(overlap_aabb, obj); - } - else - { + } else { objs_outside_scene_limit.push_back(obj); } @@ -91,23 +81,20 @@ void SpatialHashingCollisionManager::registerObject( } //============================================================================== -template -void SpatialHashingCollisionManager::unregisterObject(CollisionObject* obj) -{ +template +void SpatialHashingCollisionManager::unregisterObject( + CollisionObject* obj) { objs.remove(obj); const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { - if(!scene_limit.contain(obj_aabb)) + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { + if (!scene_limit.contain(obj_aabb)) objs_partially_penetrating_scene_limit.remove(obj); hash_table->remove(overlap_aabb, obj); - } - else - { + } else { objs_outside_scene_limit.remove(obj); } @@ -115,35 +102,29 @@ void SpatialHashingCollisionManager::unregisterObject(CollisionObject } //============================================================================== -template -void SpatialHashingCollisionManager::setup() -{ +template +void SpatialHashingCollisionManager::setup() { // Do nothing } //============================================================================== -template -void SpatialHashingCollisionManager::update() -{ +template +void SpatialHashingCollisionManager::update() { hash_table->clear(); objs_partially_penetrating_scene_limit.clear(); objs_outside_scene_limit.clear(); - for(auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) - { + for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) { CollisionObject* obj = *it; const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { - if(!scene_limit.contain(obj_aabb)) + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { + if (!scene_limit.contain(obj_aabb)) objs_partially_penetrating_scene_limit.push_back(obj); hash_table->insert(overlap_aabb, obj); - } - else - { + } else { objs_outside_scene_limit.push_back(obj); } @@ -152,43 +133,37 @@ void SpatialHashingCollisionManager::update() } //============================================================================== -template -void SpatialHashingCollisionManager::update(CollisionObject* updated_obj) -{ +template +void SpatialHashingCollisionManager::update( + CollisionObject* updated_obj) { const AABB& new_aabb = updated_obj->getAABB(); const AABB& old_aabb = obj_aabb_map[updated_obj]; AABB old_overlap_aabb; - const auto is_old_aabb_overlapping - = scene_limit.overlap(old_aabb, old_overlap_aabb); - if(is_old_aabb_overlapping) + const auto is_old_aabb_overlapping = + scene_limit.overlap(old_aabb, old_overlap_aabb); + if (is_old_aabb_overlapping) hash_table->remove(old_overlap_aabb, updated_obj); AABB new_overlap_aabb; - const auto is_new_aabb_overlapping - = scene_limit.overlap(new_aabb, new_overlap_aabb); - if(is_new_aabb_overlapping) + const auto is_new_aabb_overlapping = + scene_limit.overlap(new_aabb, new_overlap_aabb); + if (is_new_aabb_overlapping) hash_table->insert(new_overlap_aabb, updated_obj); ObjectStatus old_status; - if(is_old_aabb_overlapping) - { - if(scene_limit.contain(old_aabb)) + if (is_old_aabb_overlapping) { + if (scene_limit.contain(old_aabb)) old_status = Inside; else old_status = PartiallyPenetrating; - } - else - { + } else { old_status = Outside; } - if(is_new_aabb_overlapping) - { - if(scene_limit.contain(new_aabb)) - { - if (old_status == PartiallyPenetrating) - { + if (is_new_aabb_overlapping) { + if (scene_limit.contain(new_aabb)) { + if (old_status == PartiallyPenetrating) { // Status change: PartiallyPenetrating --> Inside // Required action(s): // - remove object from "objs_partially_penetrating_scene_limit" @@ -197,65 +172,51 @@ void SpatialHashingCollisionManager::update(CollisionObject* updated_ objs_partially_penetrating_scene_limit.end(), updated_obj); objs_partially_penetrating_scene_limit.erase(find_it); - } - else if (old_status == Outside) - { + } else if (old_status == Outside) { // Status change: Outside --> Inside // Required action(s): // - remove object from "objs_outside_scene_limit" auto find_it = std::find(objs_outside_scene_limit.begin(), - objs_outside_scene_limit.end(), - updated_obj); + objs_outside_scene_limit.end(), updated_obj); objs_outside_scene_limit.erase(find_it); } - } - else - { - if (old_status == Inside) - { + } else { + if (old_status == Inside) { // Status change: Inside --> PartiallyPenetrating // Required action(s): // - add object to "objs_partially_penetrating_scene_limit" objs_partially_penetrating_scene_limit.push_back(updated_obj); - } - else if (old_status == Outside) - { + } else if (old_status == Outside) { // Status change: Outside --> PartiallyPenetrating // Required action(s): // - remove object from "objs_outside_scene_limit" // - add object to "objs_partially_penetrating_scene_limit" auto find_it = std::find(objs_outside_scene_limit.begin(), - objs_outside_scene_limit.end(), - updated_obj); + objs_outside_scene_limit.end(), updated_obj); objs_outside_scene_limit.erase(find_it); objs_partially_penetrating_scene_limit.push_back(updated_obj); } } - } - else - { - if (old_status == Inside) - { + } else { + if (old_status == Inside) { // Status change: Inside --> Outside // Required action(s): // - add object to "objs_outside_scene_limit" objs_outside_scene_limit.push_back(updated_obj); - } - else if (old_status == PartiallyPenetrating) - { + } else if (old_status == PartiallyPenetrating) { // Status change: PartiallyPenetrating --> Outside // Required action(s): // - remove object from "objs_partially_penetrating_scene_limit" // - add object to "objs_outside_scene_limit" - auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(), - objs_partially_penetrating_scene_limit.end(), - updated_obj); + auto find_it = + std::find(objs_partially_penetrating_scene_limit.begin(), + objs_partially_penetrating_scene_limit.end(), updated_obj); objs_partially_penetrating_scene_limit.erase(find_it); objs_outside_scene_limit.push_back(updated_obj); @@ -266,17 +227,15 @@ void SpatialHashingCollisionManager::update(CollisionObject* updated_ } //============================================================================== -template -void SpatialHashingCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0; i < updated_objs.size(); ++i) - update(updated_objs[i]); +template +void SpatialHashingCollisionManager::update( + const std::vector& updated_objs) { + for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); } //============================================================================== -template -void SpatialHashingCollisionManager::clear() -{ +template +void SpatialHashingCollisionManager::clear() { objs.clear(); hash_table->clear(); objs_outside_scene_limit.clear(); @@ -284,80 +243,63 @@ void SpatialHashingCollisionManager::clear() } //============================================================================== -template -void SpatialHashingCollisionManager::getObjects(std::vector& objs_) const -{ +template +void SpatialHashingCollisionManager::getObjects( + std::vector& objs_) const { objs_.resize(objs.size()); std::copy(objs.begin(), objs.end(), objs_.begin()); } //============================================================================== -template -void SpatialHashingCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase * callback) const -{ - if(size() == 0) return; +template +void SpatialHashingCollisionManager::collide( + CollisionObject* obj, CollisionCallBackBase* callback) const { + if (size() == 0) return; collide_(obj, callback); } //============================================================================== -template -void SpatialHashingCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase * callback) const -{ - if(size() == 0) return; +template +void SpatialHashingCollisionManager::distance( + CollisionObject* obj, DistanceCallBackBase* callback) const { + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== -template +template bool SpatialHashingCollisionManager::collide_( - CollisionObject* obj, CollisionCallBackBase * callback) const -{ + CollisionObject* obj, CollisionCallBackBase* callback) const { const auto& obj_aabb = obj->getAABB(); AABB overlap_aabb; - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { const auto query_result = hash_table->query(overlap_aabb); - for(const auto& obj2 : query_result) - { - if(obj == obj2) - continue; + for (const auto& obj2 : query_result) { + if (obj == obj2) continue; - if((*callback)(obj, obj2)) - return true; + if ((*callback)(obj, obj2)) return true; } - if(!scene_limit.contain(obj_aabb)) - { - for(const auto& obj2 : objs_outside_scene_limit) - { - if(obj == obj2) - continue; + if (!scene_limit.contain(obj_aabb)) { + for (const auto& obj2 : objs_outside_scene_limit) { + if (obj == obj2) continue; - if((*callback)(obj, obj2)) - return true; + if ((*callback)(obj, obj2)) return true; } } - } - else - { - for(const auto& obj2 : objs_partially_penetrating_scene_limit) - { - if(obj == obj2) - continue; - - if((*callback)(obj, obj2)) - return true; + } else { + for (const auto& obj2 : objs_partially_penetrating_scene_limit) { + if (obj == obj2) continue; + + if ((*callback)(obj, obj2)) return true; } - for(const auto& obj2 : objs_outside_scene_limit) - { - if(obj == obj2) - continue; + for (const auto& obj2 : objs_outside_scene_limit) { + if (obj == obj2) continue; - if((*callback)(obj, obj2)) - return true; + if ((*callback)(obj, obj2)) return true; } } @@ -365,15 +307,14 @@ bool SpatialHashingCollisionManager::collide_( } //============================================================================== -template +template bool SpatialHashingCollisionManager::distance_( - CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const -{ + CollisionObject* obj, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const { auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; auto aabb = obj->getAABB(); - if(min_dist < (std::numeric_limits::max)()) - { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + if (min_dist < (std::numeric_limits::max)()) { + Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } @@ -382,67 +323,49 @@ bool SpatialHashingCollisionManager::distance_( auto status = 1; FCL_REAL old_min_distance; - while(1) - { + while (1) { old_min_distance = min_dist; - if(scene_limit.overlap(aabb, overlap_aabb)) - { - if (distanceObjectToObjects( - obj, hash_table->query(overlap_aabb), callback, min_dist)) - { + if (scene_limit.overlap(aabb, overlap_aabb)) { + if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb), + callback, min_dist)) { return true; } - if(!scene_limit.contain(aabb)) - { - if (distanceObjectToObjects( - obj, objs_outside_scene_limit, callback, min_dist)) - { + if (!scene_limit.contain(aabb)) { + if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, + min_dist)) { return true; } } - } - else - { - if (distanceObjectToObjects( - obj, objs_partially_penetrating_scene_limit, callback, min_dist)) - { + } else { + if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit, + callback, min_dist)) { return true; } - if (distanceObjectToObjects( - obj, objs_outside_scene_limit, callback, min_dist)) - { + if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, + min_dist)) { return true; } } - if(status == 1) - { - if(old_min_distance < (std::numeric_limits::max)()) - { + if (status == 1) { + if (old_min_distance < (std::numeric_limits::max)()) { break; - } - else - { - if(min_dist < old_min_distance) - { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + } else { + if (min_dist < old_min_distance) { + Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; - } - else - { - if(aabb == obj->getAABB()) + } else { + if (aabb == obj->getAABB()) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } - } - else if(status == 0) - { + } else if (status == 0) { break; } } @@ -451,59 +374,40 @@ bool SpatialHashingCollisionManager::distance_( } //============================================================================== -template +template void SpatialHashingCollisionManager::collide( - CollisionCallBackBase * callback) const -{ - if(size() == 0) - return; + CollisionCallBackBase* callback) const { + if (size() == 0) return; - for(const auto& obj1 : objs) - { + for (const auto& obj1 : objs) { const auto& obj_aabb = obj1->getAABB(); AABB overlap_aabb; - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { + if (scene_limit.overlap(obj_aabb, overlap_aabb)) { auto query_result = hash_table->query(overlap_aabb); - for(const auto& obj2 : query_result) - { - if(obj1 < obj2) - { - if((*callback)(obj1, obj2)) - return; + for (const auto& obj2 : query_result) { + if (obj1 < obj2) { + if ((*callback)(obj1, obj2)) return; } } - if(!scene_limit.contain(obj_aabb)) - { - for(const auto& obj2 : objs_outside_scene_limit) - { - if(obj1 < obj2) - { - if((*callback)(obj1, obj2)) - return; + if (!scene_limit.contain(obj_aabb)) { + for (const auto& obj2 : objs_outside_scene_limit) { + if (obj1 < obj2) { + if ((*callback)(obj1, obj2)) return; } } } - } - else - { - for(const auto& obj2 : objs_partially_penetrating_scene_limit) - { - if(obj1 < obj2) - { - if((*callback)(obj1, obj2)) - return; + } else { + for (const auto& obj2 : objs_partially_penetrating_scene_limit) { + if (obj1 < obj2) { + if ((*callback)(obj1, obj2)) return; } } - for(const auto& obj2 : objs_outside_scene_limit) - { - if(obj1 < obj2) - { - if((*callback)(obj1, obj2)) - return; + for (const auto& obj2 : objs_outside_scene_limit) { + if (obj1 < obj2) { + if ((*callback)(obj1, obj2)) return; } } } @@ -511,22 +415,18 @@ void SpatialHashingCollisionManager::collide( } //============================================================================== -template +template void SpatialHashingCollisionManager::distance( - DistanceCallBackBase * callback) const -{ - if(size() == 0) - return; + DistanceCallBackBase* callback) const { + if (size() == 0) return; this->enable_tested_set_ = true; this->tested_set.clear(); FCL_REAL min_dist = (std::numeric_limits::max)(); - for(const auto& obj : objs) - { - if(distance_(obj, callback, min_dist)) - break; + for (const auto& obj : objs) { + if (distance_(obj, callback, min_dist)) break; } this->enable_tested_set_ = false; @@ -534,124 +434,97 @@ void SpatialHashingCollisionManager::distance( } //============================================================================== -template -void SpatialHashingCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase * callback) const -{ - auto* other_manager = static_cast* >(other_manager_); +template +void SpatialHashingCollisionManager::collide( + BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const { + auto* other_manager = + static_cast*>(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) - return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { collide(callback); return; } - if(this->size() < other_manager->size()) - { - for(const auto& obj : objs) - { - if(other_manager->collide_(obj, callback)) - return; + if (this->size() < other_manager->size()) { + for (const auto& obj : objs) { + if (other_manager->collide_(obj, callback)) return; } - } - else - { - for(const auto& obj : other_manager->objs) - { - if(collide_(obj, callback)) - return; + } else { + for (const auto& obj : other_manager->objs) { + if (collide_(obj, callback)) return; } } } //============================================================================== -template -void SpatialHashingCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase * callback) const -{ - auto* other_manager = static_cast* >(other_manager_); +template +void SpatialHashingCollisionManager::distance( + BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const { + auto* other_manager = + static_cast*>(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) - return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { distance(callback); return; } FCL_REAL min_dist = (std::numeric_limits::max)(); - if(this->size() < other_manager->size()) - { - for(const auto& obj : objs) - if(other_manager->distance_(obj, callback, min_dist)) return; - } - else - { - for(const auto& obj : other_manager->objs) - if(distance_(obj, callback, min_dist)) return; + if (this->size() < other_manager->size()) { + for (const auto& obj : objs) + if (other_manager->distance_(obj, callback, min_dist)) return; + } else { + for (const auto& obj : other_manager->objs) + if (distance_(obj, callback, min_dist)) return; } } //============================================================================== -template -bool SpatialHashingCollisionManager::empty() const -{ +template +bool SpatialHashingCollisionManager::empty() const { return objs.empty(); } //============================================================================== -template -size_t SpatialHashingCollisionManager::size() const -{ +template +size_t SpatialHashingCollisionManager::size() const { return objs.size(); } //============================================================================== -template +template void SpatialHashingCollisionManager::computeBound( - std::vector& objs,Vec3f& l,Vec3f& u) -{ + std::vector& objs, Vec3f& l, Vec3f& u) { AABB bound; - for(unsigned int i = 0; i < objs.size(); ++i) - bound += objs[i]->getAABB(); + for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB(); l = bound.min_; u = bound.max_; } //============================================================================== -template -template +template +template bool SpatialHashingCollisionManager::distanceObjectToObjects( - CollisionObject* obj, - const Container& objs, - DistanceCallBackBase * callback, - FCL_REAL& min_dist) const -{ - for(auto& obj2 : objs) - { - if(obj == obj2) - continue; - - if(!this->enable_tested_set_) - { - if(obj->getAABB().distance(obj2->getAABB()) < min_dist) - { - if((*callback)(obj, obj2, min_dist)) - return true; + CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const { + for (auto& obj2 : objs) { + if (obj == obj2) continue; + + if (!this->enable_tested_set_) { + if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { + if ((*callback)(obj, obj2, min_dist)) return true; } - } - else - { - if(!this->inTestedSet(obj, obj2)) - { - if(obj->getAABB().distance(obj2->getAABB()) < min_dist) - { - if((*callback)(obj, obj2, min_dist)) - return true; + } else { + if (!this->inTestedSet(obj, obj2)) { + if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { + if ((*callback)(obj, obj2, min_dist)) return true; } this->insertTestedSet(obj, obj2); @@ -662,8 +535,8 @@ bool SpatialHashingCollisionManager::distanceObjectToObjects( return false; } -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash.h b/include/hpp/fcl/broadphase/broadphase_spatialhash.h index cfa24a00e..606b82ada 100644 --- a/include/hpp/fcl/broadphase/broadphase_spatialhash.h +++ b/include/hpp/fcl/broadphase/broadphase_spatialhash.h @@ -31,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 Jia Pan */ @@ -46,23 +46,18 @@ #include "hpp/fcl/broadphase/detail/sparse_hash_table.h" #include "hpp/fcl/broadphase/detail/spatial_hash.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief spatial hashing collision mananger -template > -class SpatialHashingCollisionManager -: public BroadPhaseCollisionManager -{ -public: +template > +class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { + public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; - - SpatialHashingCollisionManager(FCL_REAL cell_size, - const Vec3f& scene_min, + + SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000); @@ -92,23 +87,29 @@ class SpatialHashingCollisionManager /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; - /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, CollisionCallBackBase * callback) const; + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + void collide(CollisionObject* obj, CollisionCallBackBase* callback) const; - /// @brief perform distance computation between one object and all the objects belonging ot the manager - void distance(CollisionObject* obj, DistanceCallBackBase * callback) const; + /// @brief perform distance computation between one object and all the objects + /// belonging ot the manager + void distance(CollisionObject* obj, DistanceCallBackBase* callback) const; - /// @brief perform collision test for the objects belonging to the manager (i.e, N^2 self collision) - void collide(CollisionCallBackBase * callback) const; + /// @brief perform collision test for the objects belonging to the manager + /// (i.e, N^2 self collision) + void collide(CollisionCallBackBase* callback) const; - /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(DistanceCallBackBase * callback) const; + /// @brief perform distance test for the objects belonging to the manager + /// (i.e., N^2 self distance) + void distance(DistanceCallBackBase* callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, CollisionCallBackBase * callback) const; + void collide(BroadPhaseCollisionManager* other_manager, + CollisionCallBackBase* callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase * callback) const; + void distance(BroadPhaseCollisionManager* other_manager, + DistanceCallBackBase* callback) const; /// @brief whether the manager is empty bool empty() const; @@ -117,15 +118,18 @@ class SpatialHashingCollisionManager size_t size() const; /// @brief compute the bound for the environent - static void computeBound(std::vector& objs,Vec3f& l,Vec3f& u); - -protected: + static void computeBound(std::vector& objs, Vec3f& l, + Vec3f& u); - /// @brief perform collision test between one object and all the objects belonging to the manager - bool collide_(CollisionObject* obj, CollisionCallBackBase * callback) const; + protected: + /// @brief perform collision test between one object and all the objects + /// belonging to the manager + bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; - /// @brief perform distance computation between one object and all the objects belonging ot the manager - bool distance_(CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const; + /// @brief perform distance computation between one object and all the objects + /// belonging ot the manager + bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const; /// @brief all objects in the scene std::list objs; @@ -140,33 +144,26 @@ class SpatialHashingCollisionManager /// @brief the size of the scene AABB scene_limit; - /// @brief store the map between objects and their aabbs. will make update more convenient + /// @brief store the map between objects and their aabbs. will make update + /// more convenient std::map obj_aabb_map; - /// @brief objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table + /// @brief objects in the scene limit (given by scene_min and scene_max) are + /// in the spatial hash table HashTable* hash_table; -private: - - enum ObjectStatus - { - Inside, - PartiallyPenetrating, - Outside - }; + private: + enum ObjectStatus { Inside, PartiallyPenetrating, Outside }; template - bool distanceObjectToObjects( - CollisionObject* obj, - const Container& objs, - DistanceCallBackBase * callback, - FCL_REAL& min_dist) const; - + bool distanceObjectToObjects(CollisionObject* obj, const Container& objs, + DistanceCallBackBase* callback, + FCL_REAL& min_dist) const; }; -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp #include "hpp/fcl/broadphase/broadphase_spatialhash-inl.h" diff --git a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h index 841722ec3..df9fd4048 100644 --- a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h +++ b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h @@ -51,13 +51,10 @@ namespace hpp { namespace fcl { -/// @brief Collision data stores the collision request and the result given by collision algorithm. -struct CollisionData -{ - CollisionData() - { - done = false; - } +/// @brief Collision data stores the collision request and the result given by +/// collision algorithm. +struct CollisionData { + CollisionData() { done = false; } /// @brief Collision request CollisionRequest request; @@ -69,14 +66,10 @@ struct CollisionData bool done; }; - -/// @brief Distance data stores the distance request and the result given by distance algorithm. -struct DistanceData -{ - DistanceData() - { - done = false; - } +/// @brief Distance data stores the distance request and the result given by +/// distance algorithm. +struct DistanceData { + DistanceData() { done = false; } /// @brief Distance request DistanceRequest request; @@ -86,7 +79,6 @@ struct DistanceData /// @brief Whether the distance iteration can stop bool done; - }; /// @brief Provides a simple callback for the collision query in the @@ -111,23 +103,20 @@ struct DistanceData /// @param data A non-null pointer to a CollisionData instance. /// @return `true` if the broadphase evaluation should stop. /// @tparam S The scalar type with which the computation will be performed. -bool defaultCollisionFunction(CollisionObject* o1, - CollisionObject* o2, +bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* data); - /// @brief Collision data for use with the DefaultContinuousCollisionFunction. /// It stores the collision request and the result given by the collision /// algorithm (and stores the conclusion of whether further evaluation of the /// broadphase collision manager has been deemed unnecessary). -//struct DefaultContinuousCollisionData { -// ContinuousCollisionRequest request; -// ContinuousCollisionResult result; +// struct DefaultContinuousCollisionData { +// ContinuousCollisionRequest request; +// ContinuousCollisionResult result; // -// /// If `true`, requests that the broadphase evaluation stop. -// bool done{false}; -//}; - +// /// If `true`, requests that the broadphase evaluation stop. +// bool done{false}; +// }; /// @brief Provides a simple callback for the continuous collision query in the /// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and @@ -151,20 +140,20 @@ bool defaultCollisionFunction(CollisionObject* o1, /// @param data A non-null pointer to a CollisionData instance. /// @return True if the broadphase evaluation should stop. /// @tparam S The scalar type with which the computation will be performed. -//bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1, -// ContinuousCollisionObject* o2, -// void* data) { -// assert(data != nullptr); -// auto* cdata = static_cast(data); +// bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1, +// ContinuousCollisionObject* o2, +// void* data) { +// assert(data != nullptr); +// auto* cdata = static_cast(data); // -// if (cdata->done) return true; +// if (cdata->done) return true; // -// const ContinuousCollisionRequest& request = cdata->request; -// ContinuousCollisionResult& result = cdata->result; -// collide(o1, o2, request, result); +// const ContinuousCollisionRequest& request = cdata->request; +// ContinuousCollisionResult& result = cdata->result; +// collide(o1, o2, request, result); // -// return cdata->done; -//} +// return cdata->done; +// } /// @brief Provides a simple callback for the distance query in the /// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and @@ -188,67 +177,59 @@ bool defaultCollisionFunction(CollisionObject* o1, /// @param dist The distance computed by distance(). /// @return `true` if the broadphase evaluation should stop. /// @tparam S The scalar type with which the computation will be performed. -bool defaultDistanceFunction(CollisionObject* o1, - CollisionObject* o2, - void* data, FCL_REAL & dist); - +bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, + void* data, FCL_REAL& dist); -/// @brief Default collision callback to check collision between collision objects. -struct HPP_FCL_DLLAPI CollisionCallBackDefault -: CollisionCallBackBase -{ +/// @brief Default collision callback to check collision between collision +/// objects. +struct HPP_FCL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase { bool collide(CollisionObject* o1, CollisionObject* o2); - + CollisionData data; - - virtual ~CollisionCallBackDefault() {}; + + virtual ~CollisionCallBackDefault(){}; }; -/// @brief Default distance callback to check collision between collision objects. -struct HPP_FCL_DLLAPI DistanceCallBackDefault -: DistanceCallBackBase -{ - bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL & dist); - +/// @brief Default distance callback to check collision between collision +/// objects. +struct HPP_FCL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { + bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist); + DistanceData data; - - virtual ~DistanceCallBackDefault() {}; + + virtual ~DistanceCallBackDefault(){}; }; /// @brief Collision callback to collect collision pairs potentially in contacts -struct HPP_FCL_DLLAPI CollisionCallBackCollect -: CollisionCallBackBase -{ +struct HPP_FCL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase { typedef std::pair CollisionPair; - + /// @brief Default constructor. CollisionCallBackCollect(const size_t max_size); - + bool collide(CollisionObject* o1, CollisionObject* o2); - + /// @brief Returns the number of registered collision pairs size_t numCollisionPairs() const; - + /// @brief Returns a const reference to the active collision_pairs to check - const std::vector & getCollisionPairs() const; - + const std::vector& getCollisionPairs() const; + /// @brief Reset the callback void init(); - + /// @brief Check wether a collision pair exists - bool exist(const CollisionPair & pair) const; - - virtual ~CollisionCallBackCollect() {}; - -protected: - + bool exist(const CollisionPair& pair) const; + + virtual ~CollisionCallBackCollect(){}; + + protected: std::vector collision_pairs; size_t max_size; - }; } // namespace fcl -} // namespace hpp +} // namespace hpp #endif // HPP_FCL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h index 5700ea487..1ce6228e4 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h @@ -40,18 +40,14 @@ #include "hpp/fcl/broadphase/detail/hierarchy_tree.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { //============================================================================== -template -HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) -{ +template +HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { root_node = nullptr; n_leaves = 0; free_node = nullptr; @@ -62,39 +58,36 @@ HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) } //============================================================================== -template -HierarchyTree::~HierarchyTree() -{ +template +HierarchyTree::~HierarchyTree() { clear(); } //============================================================================== -template -void HierarchyTree::init(std::vector& leaves, int level) -{ - switch(level) - { - case 0: - init_0(leaves); - break; - case 1: - init_1(leaves); - break; - case 2: - init_2(leaves); - break; - case 3: - init_3(leaves); - break; - default: - init_0(leaves); +template +void HierarchyTree::init(std::vector& leaves, int level) { + switch (level) { + case 0: + init_0(leaves); + break; + case 1: + init_1(leaves); + break; + case 2: + init_2(leaves); + break; + case 3: + init_3(leaves); + break; + default: + init_0(leaves); } } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::insert(const BV& bv, void* data) -{ +template +typename HierarchyTree::Node* HierarchyTree::insert(const BV& bv, + void* data) { Node* leaf = createNode(nullptr, bv, data); insertLeaf(root_node, leaf); ++n_leaves; @@ -102,20 +95,17 @@ typename HierarchyTree::Node* HierarchyTree::insert(const BV& bv, void* } //============================================================================== -template -void HierarchyTree::remove(Node* leaf) -{ +template +void HierarchyTree::remove(Node* leaf) { removeLeaf(leaf); deleteNode(leaf); --n_leaves; } //============================================================================== -template -void HierarchyTree::clear() -{ - if(root_node) - recurseDeleteNode(root_node); +template +void HierarchyTree::clear() { + if (root_node) recurseDeleteNode(root_node); n_leaves = 0; delete free_node; free_node = nullptr; @@ -124,16 +114,14 @@ void HierarchyTree::clear() } //============================================================================== -template -bool HierarchyTree::empty() const -{ +template +bool HierarchyTree::empty() const { return (nullptr == root_node); } //============================================================================== -template -void HierarchyTree::update(Node* leaf, int lookahead_level) -{ +template +void HierarchyTree::update(Node* leaf, int lookahead_level) { // TODO(DamrongGuoy): Since we update a leaf node by removing and // inserting the same leaf node, it is likely to change the structure of // the tree even if no object's pose has changed. In the future, @@ -144,15 +132,12 @@ void HierarchyTree::update(Node* leaf, int lookahead_level) // The `sub_root` variable is the root of the subtree containing nodes // whose BVs were adjusted due to node removal. typename HierarchyTree::Node* sub_root = removeLeaf(leaf); - if(sub_root) - { - if(lookahead_level > 0) - { + if (sub_root) { + if (lookahead_level > 0) { // For positive `lookahead_level`, we move the `sub_root` variable up. - for(int i = 0; (i < lookahead_level) && sub_root->parent; ++i) + for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i) sub_root = sub_root->parent; - } - else + } else // By default, lookahead_level = -1, and we reset the `sub_root` variable // to the root of the entire tree. sub_root = root_node; @@ -163,70 +148,57 @@ void HierarchyTree::update(Node* leaf, int lookahead_level) } //============================================================================== -template -bool HierarchyTree::update(Node* leaf, const BV& bv) -{ - if(leaf->bv.contain(bv)) return false; +template +bool HierarchyTree::update(Node* leaf, const BV& bv) { + if (leaf->bv.contain(bv)) return false; update_(leaf, bv); return true; } //============================================================================== template -struct UpdateImpl -{ - static bool run( - const HierarchyTree& tree, - typename HierarchyTree::Node* leaf, - const BV& bv, - const Vec3f& /*vel*/, - FCL_REAL /*margin*/) - { - if(leaf->bv.contain(bv)) return false; +struct UpdateImpl { + static bool run(const HierarchyTree& tree, + typename HierarchyTree::Node* leaf, const BV& bv, + const Vec3f& /*vel*/, FCL_REAL /*margin*/) { + if (leaf->bv.contain(bv)) return false; tree.update_(leaf, bv); return true; } - static bool run( - const HierarchyTree& tree, - typename HierarchyTree::Node* leaf, - const BV& bv, - const Vec3f& /*vel*/) - { - if(leaf->bv.contain(bv)) return false; + static bool run(const HierarchyTree& tree, + typename HierarchyTree::Node* leaf, const BV& bv, + const Vec3f& /*vel*/) { + if (leaf->bv.contain(bv)) return false; tree.update_(leaf, bv); return true; } }; //============================================================================== -template -bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin) -{ +template +bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel, + FCL_REAL margin) { return UpdateImpl::run(*this, leaf, bv, vel, margin); } //============================================================================== -template -bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel) -{ +template +bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel) { return UpdateImpl::run(*this, leaf, bv, vel); } //============================================================================== -template -size_t HierarchyTree::getMaxHeight() const -{ - if(!root_node) - return 0; +template +size_t HierarchyTree::getMaxHeight() const { + if (!root_node) return 0; return getMaxHeight(root_node); } //============================================================================== -template -size_t HierarchyTree::getMaxDepth() const -{ - if(!root_node) return 0; +template +size_t HierarchyTree::getMaxDepth() const { + if (!root_node) return 0; size_t max_depth; getMaxDepth(root_node, 0, max_depth); @@ -234,11 +206,9 @@ size_t HierarchyTree::getMaxDepth() const } //============================================================================== -template -void HierarchyTree::balanceBottomup() -{ - if(root_node) - { +template +void HierarchyTree::balanceBottomup() { + if (root_node) { std::vector leaves; leaves.reserve(n_leaves); fetchLeaves(root_node, leaves); @@ -248,11 +218,9 @@ void HierarchyTree::balanceBottomup() } //============================================================================== -template -void HierarchyTree::balanceTopdown() -{ - if(root_node) - { +template +void HierarchyTree::balanceTopdown() { + if (root_node) { std::vector leaves; leaves.reserve(n_leaves); fetchLeaves(root_node, leaves); @@ -261,20 +229,16 @@ void HierarchyTree::balanceTopdown() } //============================================================================== -template -void HierarchyTree::balanceIncremental(int iterations) -{ - if(iterations < 0) iterations = (int)n_leaves; - if(root_node && (iterations > 0)) - { - for(int i = 0; i < iterations; ++i) - { +template +void HierarchyTree::balanceIncremental(int iterations) { + if (iterations < 0) iterations = (int)n_leaves; + if (root_node && (iterations > 0)) { + for (int i = 0; i < iterations; ++i) { Node* node = root_node; unsigned int bit = 0; - while(!node->isLeaf()) - { - node = sort(node, root_node)->children[(opath>>bit)&1]; - bit = (bit+1)&(sizeof(unsigned int) * 8-1); + while (!node->isLeaf()) { + node = sort(node, root_node)->children[(opath >> bit) & 1]; + bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); } update(node); ++opath; @@ -283,80 +247,66 @@ void HierarchyTree::balanceIncremental(int iterations) } //============================================================================== -template -void HierarchyTree::refit() -{ - if(root_node) - recurseRefit(root_node); +template +void HierarchyTree::refit() { + if (root_node) recurseRefit(root_node); } //============================================================================== -template -void HierarchyTree::extractLeaves(const Node* root, std::vector& leaves) const -{ - if(!root->isLeaf()) - { +template +void HierarchyTree::extractLeaves(const Node* root, + std::vector& leaves) const { + if (!root->isLeaf()) { extractLeaves(root->children[0], leaves); extractLeaves(root->children[1], leaves); - } - else + } else leaves.push_back(root); } //============================================================================== -template -size_t HierarchyTree::size() const -{ +template +size_t HierarchyTree::size() const { return n_leaves; } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::getRoot() const -{ +template +typename HierarchyTree::Node* HierarchyTree::getRoot() const { return root_node; } //============================================================================== -template -typename HierarchyTree::Node*& HierarchyTree::getRoot() -{ +template +typename HierarchyTree::Node*& HierarchyTree::getRoot() { return root_node; } //============================================================================== -template -void HierarchyTree::print(Node* root, int depth) -{ - for(int i = 0; i < depth; ++i) - std::cout << " "; - std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; - if(root->isLeaf()) - { - } - else - { - print(root->children[0], depth+1); - print(root->children[1], depth+1); +template +void HierarchyTree::print(Node* root, int depth) { + for (int i = 0; i < depth; ++i) std::cout << " "; + std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " + << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " + << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; + if (root->isLeaf()) { + } else { + print(root->children[0], depth + 1); + print(root->children[1], depth + 1); } } //============================================================================== -template -void HierarchyTree::bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ +template +void HierarchyTree::bottomup(const NodeVecIterator lbeg, + const NodeVecIterator lend) { NodeVecIterator lcur_end = lend; - while(lbeg < lcur_end - 1) - { + while (lbeg < lcur_end - 1) { NodeVecIterator min_it1, min_it2; FCL_REAL min_size = (std::numeric_limits::max)(); - for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) - { - for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) - { + for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) { + for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) { FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size(); - if(cur_size < min_size) - { + if (cur_size < min_size) { min_size = cur_size; min_it1 = it1; min_it2 = it2; @@ -379,70 +329,63 @@ void HierarchyTree::bottomup(const NodeVecIterator lbeg, const NodeVecIterat } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::topdown(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ - switch(topdown_level) - { - case 0: - return topdown_0(lbeg, lend); - break; - case 1: - return topdown_1(lbeg, lend); - break; - default: - return topdown_0(lbeg, lend); +template +typename HierarchyTree::Node* HierarchyTree::topdown( + const NodeVecIterator lbeg, const NodeVecIterator lend) { + switch (topdown_level) { + case 0: + return topdown_0(lbeg, lend); + break; + case 1: + return topdown_1(lbeg, lend); + break; + default: + return topdown_0(lbeg, lend); } } //============================================================================== -template -size_t HierarchyTree::getMaxHeight(Node* node) const -{ - if(!node->isLeaf()) - { +template +size_t HierarchyTree::getMaxHeight(Node* node) const { + if (!node->isLeaf()) { size_t height1 = getMaxHeight(node->children[0]); size_t height2 = getMaxHeight(node->children[1]); return std::max(height1, height2) + 1; - } - else + } else return 0; } //============================================================================== -template -void HierarchyTree::getMaxDepth(Node* node, size_t depth, size_t& max_depth) const -{ - if(!node->isLeaf()) - { - getMaxDepth(node->children[0], depth+1, max_depth); - getMaxDepth(node->children[1], depth+1, max_depth); - } - else +template +void HierarchyTree::getMaxDepth(Node* node, size_t depth, + size_t& max_depth) const { + if (!node->isLeaf()) { + getMaxDepth(node->children[0], depth + 1, max_depth); + getMaxDepth(node->children[1], depth + 1, max_depth); + } else max_depth = std::max(max_depth, depth); } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ +template +typename HierarchyTree::Node* HierarchyTree::topdown_0( + const NodeVecIterator lbeg, const NodeVecIterator lend) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(num_leaves > bu_threshold) - { + if (num_leaves > 1) { + if (num_leaves > bu_threshold) { BV vol = (*lbeg)->bv; - for(NodeVecIterator it = lbeg + 1; it < lend; ++it) - vol += (*it)->bv; + for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv; int best_axis = 0; FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; - if(extent[1] > extent[0]) best_axis = 1; - if(extent[2] > extent[best_axis]) best_axis = 2; + if (extent[1] > extent[0]) best_axis = 1; + if (extent[2] > extent[best_axis]) best_axis = 2; // compute median NodeVecIterator lcenter = lbeg + num_leaves / 2; - std::nth_element(lbeg, lcenter, lend, std::bind(&nodeBaseLess, std::placeholders::_1, std::placeholders::_2, std::ref(best_axis))); + std::nth_element(lbeg, lcenter, lend, + std::bind(&nodeBaseLess, std::placeholders::_1, + std::placeholders::_2, std::ref(best_axis))); Node* node = createNode(nullptr, vol, nullptr); node->children[0] = topdown_0(lbeg, lcenter); @@ -450,9 +393,7 @@ typename HierarchyTree::Node* HierarchyTree::topdown_0(const NodeVecIter node->children[0]->parent = node; node->children[1]->parent = node; return node; - } - else - { + } else { bottomup(lbeg, lend); return *lbeg; } @@ -461,54 +402,44 @@ typename HierarchyTree::Node* HierarchyTree::topdown_0(const NodeVecIter } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ +template +typename HierarchyTree::Node* HierarchyTree::topdown_1( + const NodeVecIterator lbeg, const NodeVecIterator lend) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(num_leaves > bu_threshold) - { - Vec3f split_p = (*lbeg)->bv.center(); + if (num_leaves > 1) { + if (num_leaves > bu_threshold) { + Vec3f split_p = (*lbeg)->bv.center(); BV vol = (*lbeg)->bv; NodeVecIterator it; - for(it = lbeg + 1; it < lend; ++it) - { + for (it = lbeg + 1; it < lend; ++it) { split_p += (*it)->bv.center(); vol += (*it)->bv; } split_p /= static_cast(num_leaves); int best_axis = -1; long bestmidp = num_leaves; - int splitcount[3][2] = {{0,0}, {0,0}, {0,0}}; - for(it = lbeg; it < lend; ++it) - { - Vec3f x = (*it)->bv.center() - split_p; - for(int j = 0; j < 3; ++j) - ++splitcount[j][x[j] > 0 ? 1 : 0]; + int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; + for (it = lbeg; it < lend; ++it) { + Vec3f x = (*it)->bv.center() - split_p; + for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; } - for(int i = 0; i < 3; ++i) - { - if((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) - { + for (int i = 0; i < 3; ++i) { + if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { long midp = std::abs(splitcount[i][0] - splitcount[i][1]); - if(midp < bestmidp) - { + if (midp < bestmidp) { best_axis = i; bestmidp = midp; } } } - if(best_axis < 0) best_axis = 0; + if (best_axis < 0) best_axis = 0; FCL_REAL split_value = split_p[best_axis]; NodeVecIterator lcenter = lbeg; - for(it = lbeg; it < lend; ++it) - { - if((*it)->bv.center()[best_axis] < split_value) - { + for (it = lbeg; it < lend; ++it) { + if ((*it)->bv.center()[best_axis] < split_value) { Node* temp = *it; *it = *lcenter; *lcenter = temp; @@ -522,9 +453,7 @@ typename HierarchyTree::Node* HierarchyTree::topdown_1(const NodeVecIter node->children[0]->parent = node; node->children[1]->parent = node; return node; - } - else - { + } else { bottomup(lbeg, lend); return *lbeg; } @@ -533,9 +462,8 @@ typename HierarchyTree::Node* HierarchyTree::topdown_1(const NodeVecIter } //============================================================================== -template -void HierarchyTree::init_0(std::vector& leaves) -{ +template +void HierarchyTree::init_0(std::vector& leaves) { clear(); root_node = topdown(leaves.begin(), leaves.end()); n_leaves = leaves.size(); @@ -544,24 +472,22 @@ void HierarchyTree::init_0(std::vector& leaves) } //============================================================================== -template -void HierarchyTree::init_1(std::vector& leaves) -{ +template +void HierarchyTree::init_1(std::vector& leaves) { clear(); BV bound_bv; - if(leaves.size() > 0) - bound_bv = leaves[0]->bv; - for(size_t i = 1; i < leaves.size(); ++i) - bound_bv += leaves[i]->bv; + if (leaves.size() > 0) bound_bv = leaves[0]->bv; + for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); - for(size_t i = 0; i < leaves.size(); ++i) + for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); - root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1); + root_node = mortonRecurse_0(leaves.begin(), leaves.end(), + (1 << (coder.bits() - 1)), coder.bits() - 1); refit(); n_leaves = leaves.size(); @@ -570,24 +496,22 @@ void HierarchyTree::init_1(std::vector& leaves) } //============================================================================== -template -void HierarchyTree::init_2(std::vector& leaves) -{ +template +void HierarchyTree::init_2(std::vector& leaves) { clear(); BV bound_bv; - if(leaves.size() > 0) - bound_bv = leaves[0]->bv; - for(size_t i = 1; i < leaves.size(); ++i) - bound_bv += leaves[i]->bv; + if (leaves.size() > 0) bound_bv = leaves[0]->bv; + for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); - for(size_t i = 0; i < leaves.size(); ++i) + for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); - root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1); + root_node = mortonRecurse_1(leaves.begin(), leaves.end(), + (1 << (coder.bits() - 1)), coder.bits() - 1); refit(); n_leaves = leaves.size(); @@ -596,19 +520,16 @@ void HierarchyTree::init_2(std::vector& leaves) } //============================================================================== -template -void HierarchyTree::init_3(std::vector& leaves) -{ +template +void HierarchyTree::init_3(std::vector& leaves) { clear(); BV bound_bv; - if(leaves.size() > 0) - bound_bv = leaves[0]->bv; - for(size_t i = 1; i < leaves.size(); ++i) - bound_bv += leaves[i]->bv; + if (leaves.size() > 0) bound_bv = leaves[0]->bv; + for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); - for(size_t i = 0; i < leaves.size(); ++i) + for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); @@ -622,30 +543,25 @@ void HierarchyTree::init_3(std::vector& leaves) } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits) -{ +template +typename HierarchyTree::Node* HierarchyTree::mortonRecurse_0( + const NodeVecIterator lbeg, const NodeVecIterator lend, + const uint32_t& split, int bits) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(bits > 0) - { + if (num_leaves > 1) { + if (bits > 0) { Node dummy; dummy.code = split; - NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); + NodeVecIterator lcenter = + std::lower_bound(lbeg, lend, &dummy, SortByMorton()); - if(lcenter == lbeg) - { + if (lcenter == lbeg) { uint32_t split2 = split | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split2, bits - 1); - } - else if(lcenter == lend) - { + } else if (lcenter == lend) { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split1, bits - 1); - } - else - { + } else { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); uint32_t split2 = split | (1 << (bits - 1)); @@ -658,42 +574,34 @@ typename HierarchyTree::Node* HierarchyTree::mortonRecurse_0(const NodeV child2->parent = node; return node; } - } - else - { + } else { Node* node = topdown(lbeg, lend); return node; } - } - else + } else return *lbeg; } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits) -{ +template +typename HierarchyTree::Node* HierarchyTree::mortonRecurse_1( + const NodeVecIterator lbeg, const NodeVecIterator lend, + const uint32_t& split, int bits) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(bits > 0) - { + if (num_leaves > 1) { + if (bits > 0) { Node dummy; dummy.code = split; - NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); + NodeVecIterator lcenter = + std::lower_bound(lbeg, lend, &dummy, SortByMorton()); - if(lcenter == lbeg) - { + if (lcenter == lbeg) { uint32_t split2 = split | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split2, bits - 1); - } - else if(lcenter == lend) - { + } else if (lcenter == lend) { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split1, bits - 1); - } - else - { + } else { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); uint32_t split2 = split | (1 << (bits - 1)); @@ -706,9 +614,7 @@ typename HierarchyTree::Node* HierarchyTree::mortonRecurse_1(const NodeV child2->parent = node; return node; } - } - else - { + } else { Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); Node* node = createNode(nullptr, nullptr); @@ -718,18 +624,16 @@ typename HierarchyTree::Node* HierarchyTree::mortonRecurse_1(const NodeV child2->parent = node; return node; } - } - else + } else return *lbeg; } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ +template +typename HierarchyTree::Node* HierarchyTree::mortonRecurse_2( + const NodeVecIterator lbeg, const NodeVecIterator lend) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { + if (num_leaves > 1) { Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); Node* node = createNode(nullptr, nullptr); @@ -738,24 +642,19 @@ typename HierarchyTree::Node* HierarchyTree::mortonRecurse_2(const NodeV child1->parent = node; child2->parent = node; return node; - } - else + } else return *lbeg; } //============================================================================== -template -void HierarchyTree::update_(Node* leaf, const BV& bv) -{ +template +void HierarchyTree::update_(Node* leaf, const BV& bv) { Node* root = removeLeaf(leaf); - if(root) - { - if(max_lookahead_level >= 0) - { - for(int i = 0; (i < max_lookahead_level) && root->parent; ++i) + if (root) { + if (max_lookahead_level >= 0) { + for (int i = 0; (i < max_lookahead_level) && root->parent; ++i) root = root->parent; - } - else + } else root = root_node; } @@ -764,17 +663,18 @@ void HierarchyTree::update_(Node* leaf, const BV& bv) } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::sort(Node* n, Node*& r) -{ +template +typename HierarchyTree::Node* HierarchyTree::sort(Node* n, Node*& r) { Node* p = n->parent; - if(p > n) - { + if (p > n) { size_t i = indexOf(n); size_t j = 1 - i; Node* s = p->children[j]; Node* q = p->parent; - if(q) q->children[indexOf(p)] = n; else r = n; + if (q) + q->children[indexOf(p)] = n; + else + r = n; s->parent = n; p->parent = n; n->parent = q; @@ -791,9 +691,8 @@ typename HierarchyTree::Node* HierarchyTree::sort(Node* n, Node*& r) } //============================================================================== -template -void HierarchyTree::insertLeaf(Node* const sub_root, - Node* const leaf) +template +void HierarchyTree::insertLeaf(Node* const sub_root, Node* const leaf) // Attempts to insert `leaf` into a subtree rooted at `sub_root`. // 1. If the whole tree is empty, then `leaf` simply becomes the tree. // 2. Otherwise, a leaf node called `sibling` of the subtree rooted at @@ -802,8 +701,7 @@ void HierarchyTree::insertLeaf(Node* const sub_root, // two children, `sibling` and `leaf`. The bounding volumes are updated as // necessary. { - if(!root_node) - { + if (!root_node) { // If the entire tree is empty, the node pointed by `leaf` variable will // become the root of the tree. root_node = leaf; @@ -814,8 +712,7 @@ void HierarchyTree::insertLeaf(Node* const sub_root, // node that we call `sibling`. The `sibling` node will eventually become // the sibling of the given `leaf` node. Node* sibling = sub_root; - while(!sibling->isLeaf()) - { + while (!sibling->isLeaf()) { sibling = sibling->children[select(*leaf, *(sibling->children[0]), *(sibling->children[1]))]; } @@ -823,7 +720,7 @@ void HierarchyTree::insertLeaf(Node* const sub_root, // Create a new `node` that later will become the new parent of both the // `sibling` and the given `leaf`. Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr); - if(prev) + if (prev) // If the parent `prev` of the `sibling` is an interior node, we will // replace the `sibling` with the subtree {node {`sibling`, leaf}} like // this: @@ -837,22 +734,22 @@ void HierarchyTree::insertLeaf(Node* const sub_root, // sibling leaf { prev->children[indexOf(sibling)] = node; - node->children[0] = sibling; sibling->parent = node; - node->children[1] = leaf; leaf->parent = node; + node->children[0] = sibling; + sibling->parent = node; + node->children[1] = leaf; + leaf->parent = node; // Now that we've inserted `leaf` some of the existing bounding // volumes might not fully enclose their children. Walk up the tree // looking for parents that don't already enclose their children // and create a new tight-fitting bounding volume for those. - do - { - if(!prev->bv.contain(node->bv)) + do { + if (!prev->bv.contain(node->bv)) prev->bv = prev->children[0]->bv + prev->children[1]->bv; else break; node = prev; } while (nullptr != (prev = node->parent)); - } - else + } else // If the `sibling` has no parent, i.e., the tree is a singleton, // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like // this: @@ -861,8 +758,10 @@ void HierarchyTree::insertLeaf(Node* const sub_root, // ╱ ╲ // sibling leaf { - node->children[0] = sibling; sibling->parent = node; - node->children[1] = leaf; leaf->parent = node; + node->children[0] = sibling; + sibling->parent = node; + node->children[1] = leaf; + leaf->parent = node; root_node = node; } @@ -874,23 +773,21 @@ void HierarchyTree::insertLeaf(Node* const sub_root, //============================================================================== template -typename HierarchyTree::Node* -HierarchyTree::removeLeaf(Node* const leaf) { -// Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling, -// and its parent with just its sibling. It then "tightens" the ancestor -// bounding volumes. Returns a pointer to the parent of the highest node whose -// BV changed due to the removal. - if(leaf == root_node) - { +typename HierarchyTree::Node* HierarchyTree::removeLeaf( + Node* const leaf) { + // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling, + // and its parent with just its sibling. It then "tightens" the ancestor + // bounding volumes. Returns a pointer to the parent of the highest node whose + // BV changed due to the removal. + if (leaf == root_node) { // If the `leaf` node is the only node in the tree, the tree becomes empty. root_node = nullptr; return nullptr; } Node* parent = leaf->parent; Node* prev = parent->parent; - Node* sibling = parent->children[1-indexOf(leaf)]; - if(prev) - { + Node* sibling = parent->children[1 - indexOf(leaf)]; + if (prev) { // If the parent node is not the root node, the sibling node will // replace the parent node like this: // @@ -911,21 +808,17 @@ HierarchyTree::removeLeaf(Node* const leaf) { sibling->parent = prev; deleteNode(parent); // Step 2: tighten up the BVs of the ancestor nodes. - while(prev) - { + while (prev) { BV new_bv = prev->children[0]->bv + prev->children[1]->bv; - if(!(new_bv == prev->bv)) - { + if (!(new_bv == prev->bv)) { prev->bv = new_bv; prev = prev->parent; - } - else break; + } else + break; } return prev ? prev : root_node; - } - else - { + } else { // If the parent node is the root node, the sibling node will become the // root of the whole tree like this: // @@ -944,63 +837,55 @@ HierarchyTree::removeLeaf(Node* const leaf) { } //============================================================================== -template -void HierarchyTree::fetchLeaves(Node* root, std::vector& leaves, int depth) -{ - if((!root->isLeaf()) && depth) - { - fetchLeaves(root->children[0], leaves, depth-1); - fetchLeaves(root->children[1], leaves, depth-1); +template +void HierarchyTree::fetchLeaves(Node* root, std::vector& leaves, + int depth) { + if ((!root->isLeaf()) && depth) { + fetchLeaves(root->children[0], leaves, depth - 1); + fetchLeaves(root->children[1], leaves, depth - 1); deleteNode(root); - } - else - { + } else { leaves.push_back(root); } } //============================================================================== -template -size_t HierarchyTree::indexOf(Node* node) -{ +template +size_t HierarchyTree::indexOf(Node* node) { // node cannot be nullptr return (node->parent->children[1] == node); } //============================================================================== -template +template typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, - const BV& bv, - void* data) -{ + const BV& bv, + void* data) { Node* node = createNode(parent, data); node->bv = bv; return node; } //============================================================================== -template +template typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, - const BV& bv1, - const BV& bv2, - void* data) -{ + const BV& bv1, + const BV& bv2, + void* data) { Node* node = createNode(parent, data); node->bv = bv1 + bv2; return node; } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, void* data) -{ +template +typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, + void* data) { Node* node = nullptr; - if(free_node) - { + if (free_node) { node = free_node; free_node = nullptr; - } - else + } else node = new Node(); node->parent = parent; node->data = data; @@ -1009,132 +894,106 @@ typename HierarchyTree::Node* HierarchyTree::createNode(Node* parent, vo } //============================================================================== -template -void HierarchyTree::deleteNode(Node* node) -{ - if(free_node != node) - { +template +void HierarchyTree::deleteNode(Node* node) { + if (free_node != node) { delete free_node; free_node = node; } } //============================================================================== -template -void HierarchyTree::recurseDeleteNode(Node* node) -{ - if(!node->isLeaf()) - { +template +void HierarchyTree::recurseDeleteNode(Node* node) { + if (!node->isLeaf()) { recurseDeleteNode(node->children[0]); recurseDeleteNode(node->children[1]); } - if(node == root_node) root_node = nullptr; + if (node == root_node) root_node = nullptr; deleteNode(node); } //============================================================================== -template -void HierarchyTree::recurseRefit(Node* node) -{ - if(!node->isLeaf()) - { +template +void HierarchyTree::recurseRefit(Node* node) { + if (!node->isLeaf()) { recurseRefit(node->children[0]); recurseRefit(node->children[1]); node->bv = node->children[0]->bv + node->children[1]->bv; - } - else + } else return; } //============================================================================== -template -bool nodeBaseLess(NodeBase* a, NodeBase* b, int d) -{ - if(a->bv.center()[d] < b->bv.center()[d]) return true; +template +bool nodeBaseLess(NodeBase* a, NodeBase* b, int d) { + if (a->bv.center()[d] < b->bv.center()[d]) return true; return false; } //============================================================================== template -struct SelectImpl -{ - static std::size_t run( - const NodeBase& /*query*/, - const NodeBase& /*node1*/, - const NodeBase& /*node2*/) - { +struct SelectImpl { + static std::size_t run(const NodeBase& /*query*/, + const NodeBase& /*node1*/, + const NodeBase& /*node2*/) { return 0; } - static std::size_t run( - const BV& /*query*/, - const NodeBase& /*node1*/, - const NodeBase& /*node2*/) - { + static std::size_t run(const BV& /*query*/, const NodeBase& /*node1*/, + const NodeBase& /*node2*/) { return 0; } }; //============================================================================== -template -size_t select( - const NodeBase& query, - const NodeBase& node1, - const NodeBase& node2) -{ +template +size_t select(const NodeBase& query, const NodeBase& node1, + const NodeBase& node2) { return SelectImpl::run(query, node1, node2); } //============================================================================== -template -size_t select( - const BV& query, - const NodeBase& node1, - const NodeBase& node2) -{ +template +size_t select(const BV& query, const NodeBase& node1, + const NodeBase& node2) { return SelectImpl::run(query, node1, node2); } //============================================================================== template -struct SelectImpl -{ - static std::size_t run( - const NodeBase& node, - const NodeBase& node1, - const NodeBase& node2) - { +struct SelectImpl { + static std::size_t run(const NodeBase& node, + const NodeBase& node1, + const NodeBase& node2) { const AABB& bv = node.bv; const AABB& bv1 = node1.bv; const AABB& bv2 = node2.bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); + Vec3f v = bv.min_ + bv.max_; + Vec3f v1 = v - (bv1.min_ + bv1.max_); + Vec3f v2 = v - (bv2.min_ + bv2.max_); FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } - static std::size_t run( - const AABB& query, - const NodeBase& node1, - const NodeBase& node2) - { + static std::size_t run(const AABB& query, const NodeBase& node1, + const NodeBase& node2) { const AABB& bv = query; const AABB& bv1 = node1.bv; const AABB& bv2 = node2.bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); + Vec3f v = bv.min_ + bv.max_; + Vec3f v1 = v - (bv1.min_ + bv1.max_); + Vec3f v2 = v - (bv2.min_ + bv2.max_); FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } }; -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h index 1a41a3212..54cddd079 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h @@ -47,31 +47,28 @@ #include "hpp/fcl/broadphase/detail/morton.h" #include "hpp/fcl/broadphase/detail/node_base.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { /// @brief Class for hierarchy tree structure -template -class HierarchyTree -{ -public: - +template +class HierarchyTree { + public: typedef NodeBase Node; /// @brief Create hierarchy tree with suitable setting. - /// bu_threshold decides the height of tree node to start bottom-up construction / optimization; - /// topdown_level decides different methods to construct tree in topdown manner. - /// lower level method constructs tree with better quality but is slower. + /// bu_threshold decides the height of tree node to start bottom-up + /// construction / optimization; topdown_level decides different methods to + /// construct tree in topdown manner. lower level method constructs tree with + /// better quality but is slower. HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); - - /// @brief Initialize the tree by a set of leaves using algorithm with a given level. + + /// @brief Initialize the tree by a set of leaves using algorithm with a given + /// level. void init(std::vector& leaves, int level = 0); /// @brief Insest a node @@ -80,10 +77,10 @@ class HierarchyTree /// @brief Remove a leaf node void remove(Node* leaf); - /// @brief Clear the tree + /// @brief Clear the tree void clear(); - /// @brief Whether the tree is empty + /// @brief Whether the tree is empty bool empty() const; /// @brief Updates a `leaf` node. A use case is when the bounding volume @@ -99,13 +96,14 @@ class HierarchyTree /// node. void update(Node* leaf, int lookahead_level = -1); - /// @brief update the tree when the bounding volume of a given leaf has changed + /// @brief update the tree when the bounding volume of a given leaf has + /// changed bool update(Node* leaf, const BV& bv); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(Node* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(Node* leaf, const BV& bv, const Vec3f& vel); /// @brief get the max height of the tree @@ -114,19 +112,20 @@ class HierarchyTree /// @brief get the max depth of the tree size_t getMaxDepth() const; - /// @brief balance the tree from bottom + /// @brief balance the tree from bottom void balanceBottomup(); - /// @brief balance the tree from top + /// @brief balance the tree from top void balanceTopdown(); - - /// @brief balance the tree in an incremental way + + /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); - - /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner + + /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, + /// update the entire tree in a bottom-up manner void refit(); - /// @brief extract all the leaves of the tree + /// @brief extract all the leaves of the tree void extractLeaves(const Node* root, std::vector& leaves) const; /// @brief number of leaves in the tree @@ -140,23 +139,21 @@ class HierarchyTree /// @brief print the tree in a recursive way void print(Node* root, int depth); -private: - - typedef typename std::vector* >::iterator NodeVecIterator; - typedef typename std::vector* >::const_iterator NodeVecConstIterator; + private: + typedef typename std::vector*>::iterator NodeVecIterator; + typedef + typename std::vector*>::const_iterator NodeVecConstIterator; - struct SortByMorton - { - bool operator() (const Node* a, const Node* b) const - { + struct SortByMorton { + bool operator()(const Node* a, const Node* b) const { return a->code < b->code; } }; - /// @brief construct a tree for a set of leaves from bottom -- very heavy way + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief construct a tree for a set of leaves from top + /// @brief construct a tree for a set of leaves from top Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief compute the maximum height of a subtree rooted from a given node @@ -165,43 +162,56 @@ class HierarchyTree /// @brief compute the maximum depth of a subtree rooted from a given node void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const; - /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. - /// During construction, first compute the best split axis as the axis along with the longest AABB edge. - /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold. + /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a + /// topdown manner. During construction, first compute the best split axis as + /// the axis along with the longest AABB edge. Then compute the median of all + /// nodes' center projection onto the axis and using it as the split + /// threshold. Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. - /// During construction, first compute the best split thresholds for different axes as the average of all nodes' center. - /// Then choose the split axis as the axis whose threshold can divide the nodes into two parts with almost similar size. - /// This construction is more expensive then topdown_0, but also can provide tree with better quality. + /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a + /// topdown manner. During construction, first compute the best split + /// thresholds for different axes as the average of all nodes' center. Then + /// choose the split axis as the axis whose threshold can divide the nodes + /// into two parts with almost similar size. This construction is more + /// expensive then topdown_0, but also can provide tree with better quality. Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief init tree from leaves in the topdown manner (topdown_0 or topdown_1) + /// @brief init tree from leaves in the topdown manner (topdown_0 or + /// topdown_1) void init_0(std::vector& leaves); - /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we use bottomup method to construct the subtree, which is slow but can construct tree with high quality. + /// @brief init tree from leaves using morton code. It uses morton_0, i.e., + /// for nodes which is of depth more than the maximum bits of the morton code, + /// we use bottomup method to construct the subtree, which is slow but can + /// construct tree with high quality. void init_1(std::vector& leaves); - /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the node index. + /// @brief init tree from leaves using morton code. It uses morton_0, i.e., + /// for nodes which is of depth more than the maximum bits of the morton code, + /// we split the leaves into two parts with the same size simply using the + /// node index. void init_2(std::vector& leaves); - /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. + /// @brief init tree from leaves using morton code. It uses morton_2, i.e., + /// for all nodes, we simply divide the leaves into parts with the same size + /// simply using the node index. void init_3(std::vector& leaves); - - Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits); - Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits); + Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, + const uint32_t& split, int bits); + + Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, + const uint32_t& split, int bits); Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief update one leaf node's bounding volume + /// @brief update one leaf node's bounding volume void update_(Node* leaf, const BV& bv); - /// @brief sort node n and its parent according to their memory position + /// @brief sort node n and its parent according to their memory position Node* sort(Node* n, Node*& r); - + /// @brief Insert a leaf node and also update its ancestors. Maintain the /// tree as a full binary tree (every interior node has exactly two children). /// Furthermore, adjust the BV of interior nodes so that each parent's BV @@ -219,23 +229,18 @@ class HierarchyTree // adjusted. Node* removeLeaf(Node* const leaf); - /// @brief Delete all internal nodes and return all leaves nodes with given depth from root + /// @brief Delete all internal nodes and return all leaves nodes with given + /// depth from root void fetchLeaves(Node* root, std::vector& leaves, int depth = -1); static size_t indexOf(Node* node); - /// @brief create one node (leaf or internal) - Node* createNode(Node* parent, - const BV& bv, - void* data); + /// @brief create one node (leaf or internal) + Node* createNode(Node* parent, const BV& bv, void* data); - Node* createNode(Node* parent, - const BV& bv1, - const BV& bv2, - void* data); - - Node* createNode(Node* parent, - void* data); + Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data); + + Node* createNode(Node* parent, void* data); void deleteNode(Node* node); @@ -243,19 +248,20 @@ class HierarchyTree void recurseRefit(Node* node); -protected: + protected: Node* root_node; size_t n_leaves; unsigned int opath; - /// This is a one Node cache, the reason is that we need to remove a node and then add it again frequently. - Node* free_node; + /// This is a one Node cache, the reason is that we need to remove a node and + /// then add it again frequently. + Node* free_node; int max_lookahead_level; - -public: + + public: /// @brief decide which topdown algorithm to use int topdown_level; @@ -264,26 +270,24 @@ class HierarchyTree }; /// @brief Compare two nodes accoording to the d-th dimension of node center -template +template bool nodeBaseLess(NodeBase* a, NodeBase* b, int d); /// @brief select from node1 and node2 which is close to a given query. 0 for /// node1 and 1 for node2 -template -size_t select( - const NodeBase& query, - const NodeBase& node1, - const NodeBase& node2); +template +size_t select(const NodeBase& query, const NodeBase& node1, + const NodeBase& node2); /// @brief select from node1 and node2 which is close to a given query bounding /// volume. 0 for node1 and 1 for node2 -template -size_t select( - const BV& query, const NodeBase& node1, const NodeBase& node2); +template +size_t select(const BV& query, const NodeBase& node1, + const NodeBase& node2); -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #include "hpp/fcl/broadphase/detail/hierarchy_tree-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h index 49d1c6b4d..8bb715fe9 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h @@ -43,27 +43,21 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { -namespace implementation_array -{ +namespace implementation_array { //============================================================================== -template -HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) -{ +template +HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { root_node = NULL_NODE; n_nodes = 0; n_nodes_alloc = 16; nodes = new Node[n_nodes_alloc]; - for(size_t i = 0; i < n_nodes_alloc - 1; ++i) - nodes[i].next = i + 1; + for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; n_leaves = 0; freelist = 0; @@ -74,39 +68,35 @@ HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) } //============================================================================== -template -HierarchyTree::~HierarchyTree() -{ - delete [] nodes; +template +HierarchyTree::~HierarchyTree() { + delete[] nodes; } //============================================================================== -template -void HierarchyTree::init(Node* leaves, int n_leaves_, int level) -{ - switch(level) - { - case 0: - init_0(leaves, n_leaves_); - break; - case 1: - init_1(leaves, n_leaves_); - break; - case 2: - init_2(leaves, n_leaves_); - break; - case 3: - init_3(leaves, n_leaves_); - break; - default: - init_0(leaves, n_leaves_); +template +void HierarchyTree::init(Node* leaves, int n_leaves_, int level) { + switch (level) { + case 0: + init_0(leaves, n_leaves_); + break; + case 1: + init_1(leaves, n_leaves_); + break; + case 2: + init_2(leaves, n_leaves_); + break; + case 3: + init_3(leaves, n_leaves_); + break; + default: + init_0(leaves, n_leaves_); } } //============================================================================== -template -void HierarchyTree::init_0(Node* leaves, int n_leaves_) -{ +template +void HierarchyTree::init_0(Node* leaves, int n_leaves_) { clear(); n_leaves = (size_t)n_leaves_; @@ -116,25 +106,22 @@ void HierarchyTree::init_0(Node* leaves, int n_leaves_) freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; root_node = topdown(ids, ids + n_leaves); - delete [] ids; + delete[] ids; opath = 0; max_lookahead_level = -1; } //============================================================================== -template -void HierarchyTree::init_1(Node* leaves, int n_leaves_) -{ +template +void HierarchyTree::init_1(Node* leaves, int n_leaves_) { clear(); n_leaves = (size_t)n_leaves_; @@ -144,30 +131,25 @@ void HierarchyTree::init_1(Node* leaves, int n_leaves_) freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; - BV bound_bv; - if(n_leaves > 0) - bound_bv = nodes[0].bv; - for(size_t i = 1; i < n_leaves; ++i) - bound_bv += nodes[i].bv; + if (n_leaves > 0) bound_bv = nodes[0].bv; + for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); - for(size_t i = 0; i < n_leaves; ++i) + for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; const SortByMorton comp{nodes}; std::sort(ids, ids + n_leaves, comp); - root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1); - delete [] ids; + root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)), + coder.bits() - 1); + delete[] ids; refit(); @@ -176,9 +158,8 @@ void HierarchyTree::init_1(Node* leaves, int n_leaves_) } //============================================================================== -template -void HierarchyTree::init_2(Node* leaves, int n_leaves_) -{ +template +void HierarchyTree::init_2(Node* leaves, int n_leaves_) { clear(); n_leaves = (size_t)n_leaves_; @@ -188,30 +169,25 @@ void HierarchyTree::init_2(Node* leaves, int n_leaves_) freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; - BV bound_bv; - if(n_leaves > 0) - bound_bv = nodes[0].bv; - for(size_t i = 1; i < n_leaves; ++i) - bound_bv += nodes[i].bv; + if (n_leaves > 0) bound_bv = nodes[0].bv; + for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); - for(size_t i = 0; i < n_leaves; ++i) + for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; const SortByMorton comp{nodes}; std::sort(ids, ids + n_leaves, comp); - root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1); - delete [] ids; + root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)), + coder.bits() - 1); + delete[] ids; refit(); @@ -220,9 +196,8 @@ void HierarchyTree::init_2(Node* leaves, int n_leaves_) } //============================================================================== -template -void HierarchyTree::init_3(Node* leaves, int n_leaves_) -{ +template +void HierarchyTree::init_3(Node* leaves, int n_leaves_) { clear(); n_leaves = (size_t)n_leaves_; @@ -232,30 +207,24 @@ void HierarchyTree::init_3(Node* leaves, int n_leaves_) freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; - BV bound_bv; - if(n_leaves > 0) - bound_bv = nodes[0].bv; - for(size_t i = 1; i < n_leaves; ++i) - bound_bv += nodes[i].bv; + if (n_leaves > 0) bound_bv = nodes[0].bv; + for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); - for(size_t i = 0; i < n_leaves; ++i) + for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; const SortByMorton comp{nodes}; std::sort(ids, ids + n_leaves, comp); root_node = mortonRecurse_2(ids, ids + n_leaves); - delete [] ids; + delete[] ids; refit(); @@ -264,9 +233,8 @@ void HierarchyTree::init_3(Node* leaves, int n_leaves_) } //============================================================================== -template -size_t HierarchyTree::insert(const BV& bv, void* data) -{ +template +size_t HierarchyTree::insert(const BV& bv, void* data) { size_t node = createNode(NULL_NODE, bv, data); insertLeaf(root_node, node); ++n_leaves; @@ -274,25 +242,22 @@ size_t HierarchyTree::insert(const BV& bv, void* data) } //============================================================================== -template -void HierarchyTree::remove(size_t leaf) -{ +template +void HierarchyTree::remove(size_t leaf) { removeLeaf(leaf); deleteNode(leaf); --n_leaves; } //============================================================================== -template -void HierarchyTree::clear() -{ - delete [] nodes; +template +void HierarchyTree::clear() { + delete[] nodes; root_node = NULL_NODE; n_nodes = 0; n_nodes_alloc = 16; nodes = new Node[n_nodes_alloc]; - for(size_t i = 0; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; + for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; n_leaves = 0; freelist = 0; @@ -301,77 +266,69 @@ void HierarchyTree::clear() } //============================================================================== -template -bool HierarchyTree::empty() const -{ +template +bool HierarchyTree::empty() const { return (n_nodes == 0); } //============================================================================== -template -void HierarchyTree::update(size_t leaf, int lookahead_level) -{ +template +void HierarchyTree::update(size_t leaf, int lookahead_level) { size_t root = removeLeaf(leaf); - if(root != NULL_NODE) - { - if(lookahead_level > 0) - { - for(int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) + if (root != NULL_NODE) { + if (lookahead_level > 0) { + for (int i = 0; + (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) root = nodes[root].parent; - } - else + } else root = root_node; } insertLeaf(root, leaf); } //============================================================================== -template -bool HierarchyTree::update(size_t leaf, const BV& bv) -{ - if(nodes[leaf].bv.contain(bv)) return false; +template +bool HierarchyTree::update(size_t leaf, const BV& bv) { + if (nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } //============================================================================== -template -bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin) -{ +template +bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, + FCL_REAL margin) { HPP_FCL_UNUSED_VARIABLE(bv); HPP_FCL_UNUSED_VARIABLE(vel); HPP_FCL_UNUSED_VARIABLE(margin); - if(nodes[leaf].bv.contain(bv)) return false; + if (nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } //============================================================================== -template -bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel) -{ +template +bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel) { HPP_FCL_UNUSED_VARIABLE(vel); - if(nodes[leaf].bv.contain(bv)) return false; + if (nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } //============================================================================== -template -size_t HierarchyTree::getMaxHeight() const -{ - if(root_node == NULL_NODE) return 0; +template +size_t HierarchyTree::getMaxHeight() const { + if (root_node == NULL_NODE) return 0; return getMaxHeight(root_node); } //============================================================================== -template -size_t HierarchyTree::getMaxDepth() const -{ - if(root_node == NULL_NODE) return 0; +template +size_t HierarchyTree::getMaxDepth() const { + if (root_node == NULL_NODE) return 0; size_t max_depth; getMaxDepth(root_node, 0, max_depth); @@ -379,11 +336,9 @@ size_t HierarchyTree::getMaxDepth() const } //============================================================================== -template -void HierarchyTree::balanceBottomup() -{ - if(root_node != NULL_NODE) - { +template +void HierarchyTree::balanceBottomup() { + if (root_node != NULL_NODE) { Node* leaves = new Node[n_leaves]; Node* leaves_ = leaves; extractLeaves(root_node, leaves_); @@ -391,28 +346,23 @@ void HierarchyTree::balanceBottomup() std::copy(leaves, leaves + n_leaves, nodes); freelist = n_leaves; n_nodes = n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; bottomup(ids, ids + n_leaves); root_node = *ids; - delete [] ids; + delete[] ids; } } //============================================================================== -template -void HierarchyTree::balanceTopdown() -{ - if(root_node != NULL_NODE) - { +template +void HierarchyTree::balanceTopdown() { + if (root_node != NULL_NODE) { Node* leaves = new Node[n_leaves]; Node* leaves_ = leaves; extractLeaves(root_node, leaves_); @@ -420,34 +370,28 @@ void HierarchyTree::balanceTopdown() std::copy(leaves, leaves + n_leaves, nodes); freelist = n_leaves; n_nodes = n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; + for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; + for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; root_node = topdown(ids, ids + n_leaves); - delete [] ids; + delete[] ids; } } //============================================================================== -template -void HierarchyTree::balanceIncremental(int iterations) -{ - if(iterations < 0) iterations = (int)n_leaves; - if((root_node != NULL_NODE) && (iterations > 0)) - { - for(int i = 0; i < iterations; ++i) - { +template +void HierarchyTree::balanceIncremental(int iterations) { + if (iterations < 0) iterations = (int)n_leaves; + if ((root_node != NULL_NODE) && (iterations > 0)) { + for (int i = 0; i < iterations; ++i) { size_t node = root_node; unsigned int bit = 0; - while(!nodes[node].isLeaf()) - { - node = nodes[node].children[(opath>>bit)&1]; - bit = (bit+1)&(sizeof(unsigned int) * 8-1); + while (!nodes[node].isLeaf()) { + node = nodes[node].children[(opath >> bit) & 1]; + bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); } update(node); ++opath; @@ -456,111 +400,89 @@ void HierarchyTree::balanceIncremental(int iterations) } //============================================================================== -template -void HierarchyTree::refit() -{ - if(root_node != NULL_NODE) - recurseRefit(root_node); +template +void HierarchyTree::refit() { + if (root_node != NULL_NODE) recurseRefit(root_node); } //============================================================================== -template -void HierarchyTree::extractLeaves(size_t root, Node*& leaves) const -{ - if(!nodes[root].isLeaf()) - { +template +void HierarchyTree::extractLeaves(size_t root, Node*& leaves) const { + if (!nodes[root].isLeaf()) { extractLeaves(nodes[root].children[0], leaves); extractLeaves(nodes[root].children[1], leaves); - } - else - { + } else { *leaves = nodes[root]; leaves++; } } //============================================================================== -template -size_t HierarchyTree::size() const -{ +template +size_t HierarchyTree::size() const { return n_leaves; } //============================================================================== -template -size_t HierarchyTree::getRoot() const -{ +template +size_t HierarchyTree::getRoot() const { return root_node; } //============================================================================== -template -typename HierarchyTree::Node* HierarchyTree::getNodes() const -{ +template +typename HierarchyTree::Node* HierarchyTree::getNodes() const { return nodes; } //============================================================================== -template -void HierarchyTree::print(size_t root, int depth) -{ - for(int i = 0; i < depth; ++i) - std::cout << " "; +template +void HierarchyTree::print(size_t root, int depth) { + for (int i = 0; i < depth; ++i) std::cout << " "; Node* n = nodes + root; - std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl; - if(n->isLeaf()) - { - } - else - { - print(n->children[0], depth+1); - print(n->children[1], depth+1); + std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " + << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] + << ", " << n->bv.max_[2] << ")" << std::endl; + if (n->isLeaf()) { + } else { + print(n->children[0], depth + 1); + print(n->children[1], depth + 1); } } //============================================================================== -template -size_t HierarchyTree::getMaxHeight(size_t node) const -{ - if(!nodes[node].isLeaf()) - { +template +size_t HierarchyTree::getMaxHeight(size_t node) const { + if (!nodes[node].isLeaf()) { size_t height1 = getMaxHeight(nodes[node].children[0]); size_t height2 = getMaxHeight(nodes[node].children[1]); return std::max(height1, height2) + 1; - } - else + } else return 0; } //============================================================================== -template -void HierarchyTree::getMaxDepth(size_t node, size_t depth, size_t& max_depth) const -{ - if(!nodes[node].isLeaf()) - { - getMaxDepth(nodes[node].children[0], depth+1, max_depth); - getmaxDepth(nodes[node].children[1], depth+1, max_depth); - } - else +template +void HierarchyTree::getMaxDepth(size_t node, size_t depth, + size_t& max_depth) const { + if (!nodes[node].isLeaf()) { + getMaxDepth(nodes[node].children[0], depth + 1, max_depth); + getmaxDepth(nodes[node].children[1], depth + 1, max_depth); + } else max_depth = std::max(max_depth, depth); } //============================================================================== -template -void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) -{ +template +void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) { size_t* lcur_end = lend; - while(lbeg < lcur_end - 1) - { - size_t* min_it1 = nullptr, *min_it2 = nullptr; + while (lbeg < lcur_end - 1) { + size_t *min_it1 = nullptr, *min_it2 = nullptr; FCL_REAL min_size = (std::numeric_limits::max)(); - for(size_t* it1 = lbeg; it1 < lcur_end; ++it1) - { - for(size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) - { + for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) { + for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) { FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); - if(cur_size < min_size) - { + if (cur_size < min_size) { min_size = cur_size; min_it1 = it1; min_it2 = it2; @@ -568,7 +490,8 @@ void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) } } - size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr); + size_t p = + createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr); nodes[p].children[0] = *min_it1; nodes[p].children[1] = *min_it2; nodes[*min_it1].parent = p; @@ -582,39 +505,33 @@ void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) } //============================================================================== -template -size_t HierarchyTree::topdown(size_t* lbeg, size_t* lend) -{ - switch(topdown_level) - { - case 0: - return topdown_0(lbeg, lend); - break; - case 1: - return topdown_1(lbeg, lend); - break; - default: - return topdown_0(lbeg, lend); +template +size_t HierarchyTree::topdown(size_t* lbeg, size_t* lend) { + switch (topdown_level) { + case 0: + return topdown_0(lbeg, lend); + break; + case 1: + return topdown_1(lbeg, lend); + break; + default: + return topdown_0(lbeg, lend); } } //============================================================================== -template -size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) -{ +template +size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(num_leaves > bu_threshold) - { + if (num_leaves > 1) { + if (num_leaves > bu_threshold) { BV vol = nodes[*lbeg].bv; - for(size_t* i = lbeg + 1; i < lend; ++i) - vol += nodes[*i].bv; + for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv; size_t best_axis = 0; FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; - if(extent[1] > extent[0]) best_axis = 1; - if(extent[2] > extent[best_axis]) best_axis = 2; + if (extent[1] > extent[0]) best_axis = 1; + if (extent[2] > extent[best_axis]) best_axis = 2; nodeBaseLess comp(nodes, best_axis); size_t* lcenter = lbeg + num_leaves / 2; @@ -626,9 +543,7 @@ size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) nodes[nodes[node].children[0]].parent = node; nodes[nodes[node].children[1]].parent = node; return node; - } - else - { + } else { bottomup(lbeg, lend); return *lbeg; } @@ -637,53 +552,42 @@ size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) } //============================================================================== -template -size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) -{ +template +size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(num_leaves > bu_threshold) - { - Vec3f split_p = nodes[*lbeg].bv.center(); + if (num_leaves > 1) { + if (num_leaves > bu_threshold) { + Vec3f split_p = nodes[*lbeg].bv.center(); BV vol = nodes[*lbeg].bv; - for(size_t* i = lbeg + 1; i < lend; ++i) - { + for (size_t* i = lbeg + 1; i < lend; ++i) { split_p += nodes[*i].bv.center(); vol += nodes[*i].bv; } split_p /= static_cast(num_leaves); int best_axis = -1; int bestmidp = (int)num_leaves; - int splitcount[3][2] = {{0,0}, {0,0}, {0,0}}; - for(size_t* i = lbeg; i < lend; ++i) - { - Vec3f x = nodes[*i].bv.center() - split_p; - for(int j = 0; j < 3; ++j) - ++splitcount[j][x[j] > 0 ? 1 : 0]; + int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; + for (size_t* i = lbeg; i < lend; ++i) { + Vec3f x = nodes[*i].bv.center() - split_p; + for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; } - for(size_t i = 0; i < 3; ++i) - { - if((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) - { + for (size_t i = 0; i < 3; ++i) { + if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { int midp = std::abs(splitcount[i][0] - splitcount[i][1]); - if(midp < bestmidp) - { + if (midp < bestmidp) { best_axis = (int)i; bestmidp = midp; } } } - if(best_axis < 0) best_axis = 0; + if (best_axis < 0) best_axis = 0; FCL_REAL split_value = split_p[best_axis]; size_t* lcenter = lbeg; - for(size_t* i = lbeg; i < lend; ++i) - { - if(nodes[*i].bv.center()[best_axis] < split_value) - { + for (size_t* i = lbeg; i < lend; ++i) { + if (nodes[*i].bv.center()[best_axis] < split_value) { size_t temp = *i; *i = *lcenter; *lcenter = temp; @@ -697,9 +601,7 @@ size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) nodes[nodes[node].children[0]].parent = node; nodes[nodes[node].children[1]].parent = node; return node; - } - else - { + } else { bottomup(lbeg, lend); return *lbeg; } @@ -708,29 +610,22 @@ size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) } //============================================================================== -template -size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split, int bits) -{ +template +size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, + const uint32_t& split, int bits) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(bits > 0) - { + if (num_leaves > 1) { + if (bits > 0) { const SortByMorton comp{nodes, split}; size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); - if(lcenter == lbeg) - { + if (lcenter == lbeg) { uint32_t split2 = split | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split2, bits - 1); - } - else if(lcenter == lend) - { + } else if (lcenter == lend) { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split1, bits - 1); - } - else - { + } else { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); uint32_t split2 = split | (1 << (bits - 1)); @@ -743,41 +638,31 @@ size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, const uint nodes[child2].parent = node; return node; } - } - else - { + } else { size_t node = topdown(lbeg, lend); return node; } - } - else + } else return *lbeg; } //============================================================================== -template -size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split, int bits) -{ +template +size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, + const uint32_t& split, int bits) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(bits > 0) - { + if (num_leaves > 1) { + if (bits > 0) { const SortByMorton comp{nodes, split}; size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); - if(lcenter == lbeg) - { + if (lcenter == lbeg) { uint32_t split2 = split | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split2, bits - 1); - } - else if(lcenter == lend) - { + } else if (lcenter == lend) { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split1, bits - 1); - } - else - { + } else { uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); uint32_t split2 = split | (1 << (bits - 1)); @@ -790,9 +675,7 @@ size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, const uint nodes[child2].parent = node; return node; } - } - else - { + } else { size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); size_t node = createNode(NULL_NODE, nullptr); @@ -802,18 +685,15 @@ size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, const uint nodes[child2].parent = node; return node; } - } - else + } else return *lbeg; } //============================================================================== -template -size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) -{ +template +size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) { long num_leaves = lend - lbeg; - if(num_leaves > 1) - { + if (num_leaves > 1) { size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); size_t node = createNode(NULL_NODE, nullptr); @@ -822,91 +702,77 @@ size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) nodes[child1].parent = node; nodes[child2].parent = node; return node; - } - else + } else return *lbeg; } //============================================================================== -template -void HierarchyTree::insertLeaf(size_t root, size_t leaf) -{ - if(root_node == NULL_NODE) - { +template +void HierarchyTree::insertLeaf(size_t root, size_t leaf) { + if (root_node == NULL_NODE) { root_node = leaf; nodes[leaf].parent = NULL_NODE; - } - else - { - if(!nodes[root].isLeaf()) - { - do - { - root = nodes[root].children[select(leaf, nodes[root].children[0], nodes[root].children[1], nodes)]; - } - while(!nodes[root].isLeaf()); + } else { + if (!nodes[root].isLeaf()) { + do { + root = nodes[root].children[select(leaf, nodes[root].children[0], + nodes[root].children[1], nodes)]; + } while (!nodes[root].isLeaf()); } size_t prev = nodes[root].parent; size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr); - if(prev != NULL_NODE) - { + if (prev != NULL_NODE) { nodes[prev].children[indexOf(root)] = node; - nodes[node].children[0] = root; nodes[root].parent = node; - nodes[node].children[1] = leaf; nodes[leaf].parent = node; - do - { - if(!nodes[prev].bv.contain(nodes[node].bv)) - nodes[prev].bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; + nodes[node].children[0] = root; + nodes[root].parent = node; + nodes[node].children[1] = leaf; + nodes[leaf].parent = node; + do { + if (!nodes[prev].bv.contain(nodes[node].bv)) + nodes[prev].bv = nodes[nodes[prev].children[0]].bv + + nodes[nodes[prev].children[1]].bv; else break; node = prev; } while (NULL_NODE != (prev = nodes[node].parent)); - } - else - { - nodes[node].children[0] = root; nodes[root].parent = node; - nodes[node].children[1] = leaf; nodes[leaf].parent = node; + } else { + nodes[node].children[0] = root; + nodes[root].parent = node; + nodes[node].children[1] = leaf; + nodes[leaf].parent = node; root_node = node; } } } //============================================================================== -template -size_t HierarchyTree::removeLeaf(size_t leaf) -{ - if(leaf == root_node) - { +template +size_t HierarchyTree::removeLeaf(size_t leaf) { + if (leaf == root_node) { root_node = NULL_NODE; return NULL_NODE; - } - else - { + } else { size_t parent = nodes[leaf].parent; size_t prev = nodes[parent].parent; - size_t sibling = nodes[parent].children[1-indexOf(leaf)]; + size_t sibling = nodes[parent].children[1 - indexOf(leaf)]; - if(prev != NULL_NODE) - { + if (prev != NULL_NODE) { nodes[prev].children[indexOf(parent)] = sibling; nodes[sibling].parent = prev; deleteNode(parent); - while(prev != NULL_NODE) - { - BV new_bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; - if(!(new_bv == nodes[prev].bv)) - { + while (prev != NULL_NODE) { + BV new_bv = nodes[nodes[prev].children[0]].bv + + nodes[nodes[prev].children[1]].bv; + if (!(new_bv == nodes[prev].bv)) { nodes[prev].bv = new_bv; prev = nodes[prev].parent; - } - else break; + } else + break; } return (prev != NULL_NODE) ? prev : root_node; - } - else - { + } else { root_node = sibling; nodes[sibling].parent = NULL_NODE; deleteNode(parent); @@ -916,15 +782,13 @@ size_t HierarchyTree::removeLeaf(size_t leaf) } //============================================================================== -template -void HierarchyTree::update_(size_t leaf, const BV& bv) -{ +template +void HierarchyTree::update_(size_t leaf, const BV& bv) { size_t root = removeLeaf(leaf); - if(root != NULL_NODE) - { - if(max_lookahead_level >= 0) - { - for(int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) + if (root != NULL_NODE) { + if (max_lookahead_level >= 0) { + for (int i = 0; + (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) root = nodes[root].parent; } @@ -934,26 +798,22 @@ void HierarchyTree::update_(size_t leaf, const BV& bv) } //============================================================================== -template -size_t HierarchyTree::indexOf(size_t node) -{ +template +size_t HierarchyTree::indexOf(size_t node) { return (nodes[nodes[node].parent].children[1] == node); } //============================================================================== -template -size_t HierarchyTree::allocateNode() -{ - if(freelist == NULL_NODE) - { +template +size_t HierarchyTree::allocateNode() { + if (freelist == NULL_NODE) { Node* old_nodes = nodes; n_nodes_alloc *= 2; nodes = new Node[n_nodes_alloc]; std::copy(old_nodes, old_nodes + n_nodes, nodes); - delete [] old_nodes; + delete[] old_nodes; - for(size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) - nodes[i].next = i + 1; + for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; freelist = n_nodes; } @@ -968,12 +828,9 @@ size_t HierarchyTree::allocateNode() } //============================================================================== -template -size_t HierarchyTree::createNode(size_t parent, - const BV& bv1, - const BV& bv2, - void* data) -{ +template +size_t HierarchyTree::createNode(size_t parent, const BV& bv1, + const BV& bv2, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; @@ -982,11 +839,8 @@ size_t HierarchyTree::createNode(size_t parent, } //============================================================================== -template -size_t HierarchyTree::createNode(size_t parent, - const BV& bv, - void* data) -{ +template +size_t HierarchyTree::createNode(size_t parent, const BV& bv, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; @@ -995,10 +849,8 @@ size_t HierarchyTree::createNode(size_t parent, } //============================================================================== -template -size_t HierarchyTree::createNode(size_t parent, - void* data) -{ +template +size_t HierarchyTree::createNode(size_t parent, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; @@ -1006,69 +858,58 @@ size_t HierarchyTree::createNode(size_t parent, } //============================================================================== -template -void HierarchyTree::deleteNode(size_t node) -{ +template +void HierarchyTree::deleteNode(size_t node) { nodes[node].next = freelist; freelist = node; --n_nodes; } //============================================================================== -template -void HierarchyTree::recurseRefit(size_t node) -{ - if(!nodes[node].isLeaf()) - { +template +void HierarchyTree::recurseRefit(size_t node) { + if (!nodes[node].isLeaf()) { recurseRefit(nodes[node].children[0]); recurseRefit(nodes[node].children[1]); - nodes[node].bv = nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; - } - else + nodes[node].bv = + nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; + } else return; } //============================================================================== -template -void HierarchyTree::fetchLeaves(size_t root, Node*& leaves, int depth) -{ - if((!nodes[root].isLeaf()) && depth) - { - fetchLeaves(nodes[root].children[0], leaves, depth-1); - fetchLeaves(nodes[root].children[1], leaves, depth-1); +template +void HierarchyTree::fetchLeaves(size_t root, Node*& leaves, int depth) { + if ((!nodes[root].isLeaf()) && depth) { + fetchLeaves(nodes[root].children[0], leaves, depth - 1); + fetchLeaves(nodes[root].children[1], leaves, depth - 1); deleteNode(root); - } - else - { + } else { *leaves = nodes[root]; leaves++; } } //============================================================================== -template +template nodeBaseLess::nodeBaseLess(const NodeBase* nodes_, size_t d_) - : nodes(nodes_), d(d_) -{ + : nodes(nodes_), d(d_) { // Do nothing } //============================================================================== -template -bool nodeBaseLess::operator()(size_t i, size_t j) const -{ - if(nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) - return true; +template +bool nodeBaseLess::operator()(size_t i, size_t j) const { + if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true; return false; } //============================================================================== template -struct SelectImpl -{ - static bool run(size_t query, size_t node1, size_t node2, NodeBase* nodes) - { +struct SelectImpl { + static bool run(size_t query, size_t node1, size_t node2, + NodeBase* nodes) { HPP_FCL_UNUSED_VARIABLE(query); HPP_FCL_UNUSED_VARIABLE(node1); HPP_FCL_UNUSED_VARIABLE(node2); @@ -1077,8 +918,8 @@ struct SelectImpl return 0; } - static bool run(const BV& query, size_t node1, size_t node2, NodeBase* nodes) - { + static bool run(const BV& query, size_t node1, size_t node2, + NodeBase* nodes) { HPP_FCL_UNUSED_VARIABLE(query); HPP_FCL_UNUSED_VARIABLE(node1); HPP_FCL_UNUSED_VARIABLE(node2); @@ -1089,53 +930,51 @@ struct SelectImpl }; //============================================================================== -template -size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) -{ +template +size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) { return SelectImpl::run(query, node1, node2, nodes); } //============================================================================== -template -size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes) -{ +template +size_t select(const BV& query, size_t node1, size_t node2, + NodeBase* nodes) { return SelectImpl::run(query, node1, node2, nodes); } //============================================================================== template -struct SelectImpl -{ - static bool run(size_t query, size_t node1, size_t node2, NodeBase* nodes) - { +struct SelectImpl { + static bool run(size_t query, size_t node1, size_t node2, + NodeBase* nodes) { const AABB& bv = nodes[query].bv; const AABB& bv1 = nodes[node1].bv; const AABB& bv2 = nodes[node2].bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); + Vec3f v = bv.min_ + bv.max_; + Vec3f v1 = v - (bv1.min_ + bv1.max_); + Vec3f v2 = v - (bv2.min_ + bv2.max_); FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } - static bool run(const AABB& query, size_t node1, size_t node2, NodeBase* nodes) - { + static bool run(const AABB& query, size_t node1, size_t node2, + NodeBase* nodes) { const AABB& bv = query; const AABB& bv1 = nodes[node1].bv; const AABB& bv2 = nodes[node2].bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); + Vec3f v = bv.min_ + bv.max_; + Vec3f v1 = v - (bv1.min_ + bv1.max_); + Vec3f v2 = v - (bv2.min_ + bv2.max_); FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } }; -} // namespace implementation_array -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace implementation_array +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h index a8b3fb335..8709a848b 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h @@ -47,37 +47,30 @@ #include "hpp/fcl/broadphase/detail/morton.h" #include "hpp/fcl/broadphase/detail/node_base_array.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { -namespace implementation_array -{ +namespace implementation_array { /// @brief Class for hierarchy tree structure -template -class HierarchyTree -{ -public: +template +class HierarchyTree { + public: typedef NodeBase Node; - -private: - struct SortByMorton - { + + private: + struct SortByMorton { SortByMorton(Node* nodes_in) : nodes(nodes_in) {} SortByMorton(Node* nodes_in, uint32_t split_in) : nodes(nodes_in), split(split_in) {} - bool operator() (size_t a, size_t b) const - { - if((a != NULL_NODE) && (b != NULL_NODE)) + bool operator()(size_t a, size_t b) const { + if ((a != NULL_NODE) && (b != NULL_NODE)) return nodes[a].code < nodes[b].code; - else if(a == NULL_NODE) + else if (a == NULL_NODE) return split < nodes[b].code; - else if(b == NULL_NODE) + else if (b == NULL_NODE) return nodes[a].code < split; return false; @@ -86,42 +79,45 @@ class HierarchyTree Node* nodes{}; uint32_t split{}; }; - -public: + public: /// @brief Create hierarchy tree with suitable setting. - /// bu_threshold decides the height of tree node to start bottom-up construction / optimization; - /// topdown_level decides different methods to construct tree in topdown manner. - /// lower level method constructs tree with better quality but is slower. + /// bu_threshold decides the height of tree node to start bottom-up + /// construction / optimization; topdown_level decides different methods to + /// construct tree in topdown manner. lower level method constructs tree with + /// better quality but is slower. HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); - /// @brief Initialize the tree by a set of leaves using algorithm with a given level. + /// @brief Initialize the tree by a set of leaves using algorithm with a given + /// level. void init(Node* leaves, int n_leaves_, int level = 0); - /// @brief Initialize the tree by a set of leaves using algorithm with a given level. + /// @brief Initialize the tree by a set of leaves using algorithm with a given + /// level. size_t insert(const BV& bv, void* data); /// @brief Remove a leaf node void remove(size_t leaf); - /// @brief Clear the tree + /// @brief Clear the tree void clear(); - /// @brief Whether the tree is empty + /// @brief Whether the tree is empty bool empty() const; - - /// @brief update one leaf node + + /// @brief update one leaf node void update(size_t leaf, int lookahead_level = -1); - /// @brief update the tree when the bounding volume of a given leaf has changed + /// @brief update the tree when the bounding volume of a given leaf has + /// changed bool update(size_t leaf, const BV& bv); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vec3f& vel); /// @brief get the max height of the tree @@ -130,19 +126,20 @@ class HierarchyTree /// @brief get the max depth of the tree size_t getMaxDepth() const; - /// @brief balance the tree from bottom + /// @brief balance the tree from bottom void balanceBottomup(); - /// @brief balance the tree from top + /// @brief balance the tree from top void balanceTopdown(); - /// @brief balance the tree in an incremental way + /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); - /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner + /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, + /// update the entire tree in a bottom-up manner void refit(); - /// @brief extract all the leaves of the tree + /// @brief extract all the leaves of the tree void extractLeaves(size_t root, Node*& leaves) const; /// @brief number of leaves in the tree @@ -157,12 +154,11 @@ class HierarchyTree /// @brief print the tree in a recursive way void print(size_t root, int depth); -private: - - /// @brief construct a tree for a set of leaves from bottom -- very heavy way + private: + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(size_t* lbeg, size_t* lend); - - /// @brief construct a tree for a set of leaves from top + + /// @brief construct a tree for a set of leaves from top size_t topdown(size_t* lbeg, size_t* lend); /// @brief compute the maximum height of a subtree rooted from a given node @@ -171,48 +167,63 @@ class HierarchyTree /// @brief compute the maximum depth of a subtree rooted from a given node void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const; - /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. - /// During construction, first compute the best split axis as the axis along with the longest AABB edge. - /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold. + /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a + /// topdown manner. During construction, first compute the best split axis as + /// the axis along with the longest AABB edge. Then compute the median of all + /// nodes' center projection onto the axis and using it as the split + /// threshold. size_t topdown_0(size_t* lbeg, size_t* lend); - /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. - /// During construction, first compute the best split thresholds for different axes as the average of all nodes' center. - /// Then choose the split axis as the axis whose threshold can divide the nodes into two parts with almost similar size. - /// This construction is more expensive then topdown_0, but also can provide tree with better quality. + /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a + /// topdown manner. During construction, first compute the best split + /// thresholds for different axes as the average of all nodes' center. Then + /// choose the split axis as the axis whose threshold can divide the nodes + /// into two parts with almost similar size. This construction is more + /// expensive then topdown_0, but also can provide tree with better quality. size_t topdown_1(size_t* lbeg, size_t* lend); - /// @brief init tree from leaves in the topdown manner (topdown_0 or topdown_1) + /// @brief init tree from leaves in the topdown manner (topdown_0 or + /// topdown_1) void init_0(Node* leaves, int n_leaves_); - /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we use bottomup method to construct the subtree, which is slow but can construct tree with high quality. + /// @brief init tree from leaves using morton code. It uses morton_0, i.e., + /// for nodes which is of depth more than the maximum bits of the morton code, + /// we use bottomup method to construct the subtree, which is slow but can + /// construct tree with high quality. void init_1(Node* leaves, int n_leaves_); - /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the node index. + /// @brief init tree from leaves using morton code. It uses morton_0, i.e., + /// for nodes which is of depth more than the maximum bits of the morton code, + /// we split the leaves into two parts with the same size simply using the + /// node index. void init_2(Node* leaves, int n_leaves_); - /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. + /// @brief init tree from leaves using morton code. It uses morton_2, i.e., + /// for all nodes, we simply divide the leaves into parts with the same size + /// simply using the node index. void init_3(Node* leaves, int n_leaves_); - size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split, int bits); + size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split, + int bits); - size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split, int bits); + size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split, + int bits); size_t mortonRecurse_2(size_t* lbeg, size_t* lend); - /// @brief update one leaf node's bounding volume + /// @brief update one leaf node's bounding volume void update_(size_t leaf, const BV& bv); - /// @brief Insert a leaf node and also update its ancestors + /// @brief Insert a leaf node and also update its ancestors void insertLeaf(size_t root, size_t leaf); - /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted. - /// return the node with the smallest depth and is influenced by the remove operation + /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the + /// unnecessary internal nodes are deleted. return the node with the smallest + /// depth and is influenced by the remove operation size_t removeLeaf(size_t leaf); - /// @brief Delete all internal nodes and return all leaves nodes with given depth from root + /// @brief Delete all internal nodes and return all leaves nodes with given + /// depth from root void fetchLeaves(size_t root, Node*& leaves, int depth = -1); size_t indexOf(size_t node); @@ -220,59 +231,50 @@ class HierarchyTree size_t allocateNode(); /// @brief create one node (leaf or internal) - size_t createNode(size_t parent, - const BV& bv1, - const BV& bv2, - void* data); + size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data); - size_t createNode(size_t parent, - const BV& bv, - void* data); + size_t createNode(size_t parent, const BV& bv, void* data); - size_t createNode(size_t parent, - void* data); + size_t createNode(size_t parent, void* data); void deleteNode(size_t node); void recurseRefit(size_t node); -protected: + protected: size_t root_node; Node* nodes; size_t n_nodes; size_t n_nodes_alloc; - + size_t n_leaves; size_t freelist; unsigned int opath; int max_lookahead_level; -public: + public: /// @brief decide which topdown algorithm to use int topdown_level; /// @brief decide the depth to use expensive bottom-up algorithm int bu_threshold; -public: + public: static const size_t NULL_NODE = std::numeric_limits::max(); }; -template +template const size_t HierarchyTree::NULL_NODE; - /// @brief Functor comparing two nodes -template -struct nodeBaseLess -{ +template +struct nodeBaseLess { nodeBaseLess(const NodeBase* nodes_, size_t d_); - bool operator() (size_t i, size_t j) const; - -private: + bool operator()(size_t i, size_t j) const; + private: /// @brief the nodes array const NodeBase* nodes; @@ -282,18 +284,18 @@ struct nodeBaseLess /// @brief select the node from node1 and node2 which is close to the query-th /// node in the nodes. 0 for node1 and 1 for node2. -template +template size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes); /// @brief select the node from node1 and node2 which is close to the query /// AABB. 0 for node1 and 1 for node2. -template +template size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes); -} // namespace implementation_array -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace implementation_array +} // namespace detail +} // namespace fcl +} // namespace hpp #include "hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/interval_tree.h b/include/hpp/fcl/broadphase/detail/interval_tree.h index e3642ec9b..02ebfbbc2 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree.h +++ b/include/hpp/fcl/broadphase/detail/interval_tree.h @@ -51,9 +51,8 @@ namespace detail { /// @brief Class describes the information needed when we take the /// right branch in searching for intervals but possibly come back /// and check the left branch as well. -struct HPP_FCL_DLLAPI it_recursion_node -{ -public: +struct HPP_FCL_DLLAPI it_recursion_node { + public: IntervalTreeNode* start_node; unsigned int parent_index; @@ -61,12 +60,9 @@ struct HPP_FCL_DLLAPI it_recursion_node bool try_right_branch; }; - /// @brief Interval tree -class HPP_FCL_DLLAPI IntervalTree -{ -public: - +class HPP_FCL_DLLAPI IntervalTree { + public: IntervalTree(); ~IntervalTree(); @@ -92,8 +88,7 @@ class HPP_FCL_DLLAPI IntervalTree /// @brief Return result for a given query std::deque query(FCL_REAL low, FCL_REAL high); -protected: - + protected: IntervalTreeNode* root; IntervalTreeNode* nil; @@ -107,27 +102,28 @@ class HPP_FCL_DLLAPI IntervalTree /// @brief Inserts node into the tree as if it were a regular binary tree void recursiveInsert(IntervalTreeNode* node); - /// @brief recursively print a subtree + /// @brief recursively print a subtree void recursivePrint(IntervalTreeNode* node) const; /// @brief recursively find the node corresponding to the interval - IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const; + IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, + SimpleInterval* ivl) const; - /// @brief Travels up to the root fixing the max_high fields after an insertion or deletion + /// @brief Travels up to the root fixing the max_high fields after an + /// insertion or deletion void fixupMaxHigh(IntervalTreeNode* node); void deleteFixup(IntervalTreeNode* node); -private: + private: unsigned int recursion_node_stack_size; it_recursion_node* recursion_node_stack; unsigned int current_parent; unsigned int recursion_node_stack_top; }; - -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h b/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h index d6fe96ccc..b9771ae18 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h +++ b/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h @@ -48,44 +48,49 @@ namespace fcl { namespace detail { //============================================================================== -IntervalTreeNode::IntervalTreeNode() -{ +IntervalTreeNode::IntervalTreeNode() { // Do nothing } //============================================================================== IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) - : stored_interval (new_interval), - key(new_interval->low), - high(new_interval->high), - max_high(high) -{ + : stored_interval(new_interval), + key(new_interval->low), + high(new_interval->high), + max_high(high) { // Do nothing } //============================================================================== -IntervalTreeNode::~IntervalTreeNode() -{ +IntervalTreeNode::~IntervalTreeNode() { // Do nothing } //============================================================================== -void IntervalTreeNode::print( - IntervalTreeNode* nil, IntervalTreeNode* root) const -{ +void IntervalTreeNode::print(IntervalTreeNode* nil, + IntervalTreeNode* root) const { stored_interval->print(); std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high; std::cout << " l->key = "; - if(left == nil) std::cout << "nullptr"; else std::cout << left->key; + if (left == nil) + std::cout << "nullptr"; + else + std::cout << left->key; std::cout << " r->key = "; - if(right == nil) std::cout << "nullptr"; else std::cout << right->key; + if (right == nil) + std::cout << "nullptr"; + else + std::cout << right->key; std::cout << " p->key = "; - if(parent == root) std::cout << "nullptr"; else std::cout << parent->key; + if (parent == root) + std::cout << "nullptr"; + else + std::cout << parent->key; std::cout << " red = " << (int)red << std::endl; } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node.h b/include/hpp/fcl/broadphase/detail/interval_tree_node.h index eb632b7ba..e11702c99 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree_node.h +++ b/include/hpp/fcl/broadphase/detail/interval_tree_node.h @@ -41,23 +41,18 @@ #include "hpp/fcl/broadphase/detail/simple_interval.h" #include "hpp/fcl/fwd.hh" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { class HPP_FCL_DLLAPI IntervalTree; /// @brief The node for interval tree -class HPP_FCL_DLLAPI IntervalTreeNode -{ -public: - +class HPP_FCL_DLLAPI IntervalTreeNode { + public: friend class IntervalTree; - + /// @brief Create an empty node IntervalTreeNode(); @@ -66,10 +61,11 @@ class HPP_FCL_DLLAPI IntervalTreeNode ~IntervalTreeNode(); - /// @brief Print the interval node information: set left = nil and right = root + /// @brief Print the interval node information: set left = nil and right = + /// root void print(IntervalTreeNode* left, IntervalTreeNode* right) const; -protected: + protected: /// @brief interval stored in the node SimpleInterval* stored_interval; @@ -80,7 +76,7 @@ class HPP_FCL_DLLAPI IntervalTreeNode FCL_REAL max_high; /// @brief red or black node: if red = false then the node is black - bool red; + bool red; IntervalTreeNode* left; @@ -89,8 +85,8 @@ class HPP_FCL_DLLAPI IntervalTreeNode IntervalTreeNode* parent; }; -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/morton-inl.h b/include/hpp/fcl/broadphase/detail/morton-inl.h index 25b261998..53d7d90f7 100644 --- a/include/hpp/fcl/broadphase/detail/morton-inl.h +++ b/include/hpp/fcl/broadphase/detail/morton-inl.h @@ -41,33 +41,29 @@ #include "hpp/fcl/broadphase/detail/morton.h" - namespace hpp { -namespace fcl {/// @cond IGNORE +namespace fcl { /// @cond IGNORE namespace detail { //============================================================================== template -uint32_t quantize(S x, uint32_t n) -{ - return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n-1)), uint32_t(0)); +uint32_t quantize(S x, uint32_t n) { + return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0)); } //============================================================================== -template +template morton_functor::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) -{ + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== -template -uint32_t morton_functor::operator()(const Vec3f& point) const -{ +template +uint32_t morton_functor::operator()(const Vec3f& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); @@ -76,20 +72,18 @@ uint32_t morton_functor::operator()(const Vec3f& point) const } //============================================================================== -template +template morton_functor::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) -{ + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== -template -uint64_t morton_functor::operator()(const Vec3f& point) const -{ +template +uint64_t morton_functor::operator()(const Vec3f& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); @@ -98,35 +92,31 @@ uint64_t morton_functor::operator()(const Vec3f& point) const } //============================================================================== -template -constexpr size_t morton_functor::bits() -{ +template +constexpr size_t morton_functor::bits() { return 60; } //============================================================================== -template -constexpr size_t morton_functor::bits() -{ +template +constexpr size_t morton_functor::bits() { return 30; } //============================================================================== -template +template morton_functor>::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) -{ + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== -template +template std::bitset morton_functor>::operator()( - const Vec3f& point) const -{ + const Vec3f& point) const { S x = (point[0] - base[0]) * inv[0]; S y = (point[1] - base[1]) * inv[1]; S z = (point[2] - base[2]) * inv[2]; @@ -137,29 +127,27 @@ std::bitset morton_functor>::operator()( y *= 2; z *= 2; - for(size_t i = 0; i < bits()/3; ++i) - { + for (size_t i = 0; i < bits() / 3; ++i) { bset[start_bit--] = ((z < 1) ? 0 : 1); bset[start_bit--] = ((y < 1) ? 0 : 1); bset[start_bit--] = ((x < 1) ? 0 : 1); - x = ((x >= 1) ? 2*(x-1) : 2*x); - y = ((y >= 1) ? 2*(y-1) : 2*y); - z = ((z >= 1) ? 2*(z-1) : 2*z); + x = ((x >= 1) ? 2 * (x - 1) : 2 * x); + y = ((y >= 1) ? 2 * (y - 1) : 2 * y); + z = ((z >= 1) ? 2 * (z - 1) : 2 * z); } return bset; } //============================================================================== -template -constexpr size_t morton_functor>::bits() -{ +template +constexpr size_t morton_functor>::bits() { return N; } -} // namespace detail +} // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/morton.h b/include/hpp/fcl/broadphase/detail/morton.h index a00c99ff9..fb480d387 100644 --- a/include/hpp/fcl/broadphase/detail/morton.h +++ b/include/hpp/fcl/broadphase/detail/morton.h @@ -42,14 +42,11 @@ #include "hpp/fcl/BV/AABB.h" #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @cond IGNORE -namespace detail -{ +namespace detail { template uint32_t quantize(S x, uint32_t n); @@ -66,16 +63,15 @@ uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z); /// This is specialized for 32- and 64-bit unsigned integers giving /// a 30- or 60-bit code, respectively, and for `std::bitset` where /// N is the length of the code and must be a multiple of 3. -template +template struct morton_functor {}; /// @brief Functor to compute 30 bit morton code for a given AABB -template -struct morton_functor -{ +template +struct morton_functor { morton_functor(const AABB& bbox); - uint32_t operator() (const Vec3f& point) const; + uint32_t operator()(const Vec3f& point) const; const Vec3f base; const Vec3f inv; @@ -87,12 +83,11 @@ using morton_functoru32f = morton_functor; using morton_functoru32d = morton_functor; /// @brief Functor to compute 60 bit morton code for a given AABB -template -struct morton_functor -{ +template +struct morton_functor { morton_functor(const AABB& bbox); - uint64_t operator() (const Vec3f& point) const; + uint64_t operator()(const Vec3f& point) const; const Vec3f base; const Vec3f inv; @@ -100,17 +95,15 @@ struct morton_functor static constexpr size_t bits(); }; - /// @brief Functor to compute N bit morton code for a given AABB /// N must be a multiple of 3. -template -struct morton_functor> -{ - static_assert(N%3==0, "Number of bits must be a multiple of 3"); +template +struct morton_functor> { + static_assert(N % 3 == 0, "Number of bits must be a multiple of 3"); morton_functor(const AABB& bbox); - std::bitset operator() (const Vec3f& point) const; + std::bitset operator()(const Vec3f& point) const; const Vec3f base; const Vec3f inv; @@ -118,10 +111,10 @@ struct morton_functor> static constexpr size_t bits(); }; -} // namespace detail +} // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp #include "hpp/fcl/broadphase/detail/morton-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/node_base-inl.h b/include/hpp/fcl/broadphase/detail/node_base-inl.h index d93c899b7..d7a69680c 100644 --- a/include/hpp/fcl/broadphase/detail/node_base-inl.h +++ b/include/hpp/fcl/broadphase/detail/node_base-inl.h @@ -40,12 +40,9 @@ #include "hpp/fcl/broadphase/detail/node_base.h" -namespace hpp -{ -namespace fcl -{ -namespace detail -{ +namespace hpp { +namespace fcl { +namespace detail { //============================================================================// // // @@ -55,29 +52,26 @@ namespace detail //============================================================================== template -bool NodeBase::isLeaf() const -{ +bool NodeBase::isLeaf() const { return (children[1] == nullptr); } //============================================================================== template -bool NodeBase::isInternal() const -{ +bool NodeBase::isInternal() const { return !isLeaf(); } //============================================================================== template -NodeBase::NodeBase() -{ +NodeBase::NodeBase() { parent = nullptr; children[0] = nullptr; children[1] = nullptr; } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/node_base.h b/include/hpp/fcl/broadphase/detail/node_base.h index 998ef0b8a..5937b9cef 100644 --- a/include/hpp/fcl/broadphase/detail/node_base.h +++ b/include/hpp/fcl/broadphase/detail/node_base.h @@ -40,18 +40,14 @@ #include "hpp/fcl/data_types.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { /// @brief dynamic AABB tree node -template -struct NodeBase -{ +template +struct NodeBase { /// @brief the bounding volume for the node BV bv; @@ -64,8 +60,7 @@ struct NodeBase /// @brief whether is internal node bool isInternal() const; - union - { + union { /// @brief for leaf node, children nodes NodeBase* children[2]; void* data; @@ -77,9 +72,9 @@ struct NodeBase NodeBase(); }; -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #include "hpp/fcl/broadphase/detail/node_base-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h index ad0791c92..0ee3e5f05 100644 --- a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h +++ b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h @@ -40,34 +40,28 @@ #include "hpp/fcl/broadphase/detail/node_base_array.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { -namespace implementation_array -{ +namespace implementation_array { //============================================================================== -template -bool NodeBase::isLeaf() const -{ +template +bool NodeBase::isLeaf() const { return (children[1] == (size_t)(-1)); } //============================================================================== -template -bool NodeBase::isInternal() const -{ +template +bool NodeBase::isInternal() const { return !isLeaf(); } -} // namespace implementation_array -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace implementation_array +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/node_base_array.h b/include/hpp/fcl/broadphase/detail/node_base_array.h index 158d8ca31..ecf8dc01d 100644 --- a/include/hpp/fcl/broadphase/detail/node_base_array.h +++ b/include/hpp/fcl/broadphase/detail/node_base_array.h @@ -40,44 +40,37 @@ #include "hpp/fcl/data_types.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { -namespace implementation_array -{ +namespace implementation_array { -template -struct NodeBase -{ +template +struct NodeBase { BV bv; - union - { + union { size_t parent; size_t next; }; - - union - { + + union { size_t children[2]; void* data; }; uint32_t code; - + bool isLeaf() const; bool isInternal() const; }; -} // namespace implementation_array -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace implementation_array +} // namespace detail +} // namespace fcl +} // namespace hpp #include "hpp/fcl/broadphase/detail/node_base_array-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h index 987666195..e68e4d846 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h @@ -41,28 +41,21 @@ #include "hpp/fcl/broadphase/detail/simple_hash_table.h" #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { //============================================================================== -template -SimpleHashTable::SimpleHashTable(const HashFnc& h) - : h_(h) -{ +template +SimpleHashTable::SimpleHashTable(const HashFnc& h) : h_(h) { // Do nothing } //============================================================================== -template -void SimpleHashTable::init(size_t size) -{ - if(size == 0) - { +template +void SimpleHashTable::init(size_t size) { + if (size == 0) { throw std::logic_error("SimpleHashTable must have non-zero size."); } @@ -71,24 +64,21 @@ void SimpleHashTable::init(size_t size) } //============================================================================== -template -void SimpleHashTable::insert(Key key, Data value) -{ +template +void SimpleHashTable::insert(Key key, Data value) { std::vector indices = h_(key); size_t range = table_.size(); - for(size_t i = 0; i < indices.size(); ++i) + for (size_t i = 0; i < indices.size(); ++i) table_[indices[i] % range].push_back(value); } //============================================================================== -template -std::vector SimpleHashTable::query(Key key) const -{ +template +std::vector SimpleHashTable::query(Key key) const { size_t range = table_.size(); std::vector indices = h_(key); std::set result; - for(size_t i = 0; i < indices.size(); ++i) - { + for (size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i] % range; std::copy(table_[index].begin(), table_[index].end(), std::inserter(result, result.end())); @@ -98,28 +88,25 @@ std::vector SimpleHashTable::query(Key key) const } //============================================================================== -template -void SimpleHashTable::remove(Key key, Data value) -{ +template +void SimpleHashTable::remove(Key key, Data value) { size_t range = table_.size(); std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) - { + for (size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i] % range; table_[index].remove(value); } } //============================================================================== -template -void SimpleHashTable::clear() -{ +template +void SimpleHashTable::clear() { table_.clear(); table_.resize(table_size_); } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table.h b/include/hpp/fcl/broadphase/detail/simple_hash_table.h index c64578da2..e136ddeaa 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table.h @@ -42,20 +42,16 @@ #include #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { /// @brief A simple hash table implemented as multiple buckets. HashFnc is any /// extended hash function: HashFnc(key) = {index1, index2, ..., } -template -class SimpleHashTable -{ -protected: +template +class SimpleHashTable { + protected: typedef std::list Bin; std::vector table_; @@ -64,7 +60,7 @@ class SimpleHashTable size_t table_size_; -public: + public: SimpleHashTable(const HashFnc& h); /// @brief Init the number of bins in the hash table @@ -84,9 +80,9 @@ class SimpleHashTable void clear(); }; -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #include "hpp/fcl/broadphase/detail/simple_hash_table-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h index a9661c5cf..78719156c 100644 --- a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h +++ b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h @@ -46,19 +46,17 @@ namespace fcl { namespace detail { //============================================================================== -SimpleInterval::~SimpleInterval() -{ +SimpleInterval::~SimpleInterval() { // Do nothing } //============================================================================== -void SimpleInterval::print() -{ +void SimpleInterval::print() { // Do nothing } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/simple_interval.h b/include/hpp/fcl/broadphase/detail/simple_interval.h index 6f404316c..0f1976ebf 100644 --- a/include/hpp/fcl/broadphase/detail/simple_interval.h +++ b/include/hpp/fcl/broadphase/detail/simple_interval.h @@ -41,28 +41,24 @@ #include "hpp/fcl/fwd.hh" #include "hpp/fcl/data_types.h" -namespace hpp -{ -namespace fcl -{ -namespace detail -{ +namespace hpp { +namespace fcl { +namespace detail { /// @brief Interval trees implemented using red-black-trees as described in /// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest. -struct HPP_FCL_DLLAPI SimpleInterval -{ -public: +struct HPP_FCL_DLLAPI SimpleInterval { + public: virtual ~SimpleInterval(); - + virtual void print(); /// @brief interval is defined as [low, high] FCL_REAL low, high; }; -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h index 4719cc3ad..b0668d989 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h @@ -40,56 +40,48 @@ #include "hpp/fcl/broadphase/detail/sparse_hash_table.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { //============================================================================== template class TableT> + template class TableT> SparseHashTable::SparseHashTable(const HashFnc& h) - : h_(h) -{ + : h_(h) { // Do nothing } //============================================================================== template class TableT> -void SparseHashTable::init(size_t) -{ + template class TableT> +void SparseHashTable::init(size_t) { table_.clear(); } //============================================================================== template class TableT> -void SparseHashTable::insert(Key key, Data value) -{ + template class TableT> +void SparseHashTable::insert(Key key, Data value) { std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) + for (size_t i = 0; i < indices.size(); ++i) table_[indices[i]].push_back(value); } //============================================================================== template class TableT> -std::vector SparseHashTable::query(Key key) const -{ + template class TableT> +std::vector SparseHashTable::query( + Key key) const { std::vector indices = h_(key); std::set result; - for(size_t i = 0; i < indices.size(); ++i) - { + for (size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i]; typename Table::const_iterator p = table_.find(index); - if(p != table_.end()) - { - std::copy((*p).second.begin(), (*p).second.end(), std - ::inserter(result, result.end())); + if (p != table_.end()) { + std::copy((*p).second.begin(), (*p).second.end(), + std ::inserter(result, result.end())); } } @@ -98,12 +90,10 @@ std::vector SparseHashTable::query(Key key) co //============================================================================== template class TableT> -void SparseHashTable::remove(Key key, Data value) -{ + template class TableT> +void SparseHashTable::remove(Key key, Data value) { std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) - { + for (size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i]; table_[index].remove(value); } @@ -111,14 +101,13 @@ void SparseHashTable::remove(Key key, Data value) //============================================================================== template class TableT> -void SparseHashTable::clear() -{ + template class TableT> +void SparseHashTable::clear() { table_.clear(); } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h index 57a889b49..f0ca992b3 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h @@ -44,42 +44,36 @@ #include #include -namespace hpp -{ -namespace fcl -{ - -namespace detail -{ - -template -class unordered_map_hash_table -: public std::unordered_map -{ +namespace hpp { +namespace fcl { + +namespace detail { + +template +class unordered_map_hash_table : public std::unordered_map { typedef std::unordered_map Base; -public: - unordered_map_hash_table() : Base() {}; - + public: + unordered_map_hash_table() : Base(){}; }; /// @brief A hash table implemented using unordered_map template class TableT = unordered_map_hash_table> -class SparseHashTable -{ -protected: + template class TableT = unordered_map_hash_table> +class SparseHashTable { + protected: HashFnc h_; typedef std::list Bin; typedef TableT Table; - + Table table_; -public: + + public: SparseHashTable(const HashFnc& h); /// @brief Init the hash table. The bucket size is dynamically decided. void init(size_t); - + /// @brief insert one key-value pair into the hash table void insert(Key key, Data value); @@ -93,9 +87,9 @@ class SparseHashTable void clear(); }; -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #include "hpp/fcl/broadphase/detail/sparse_hash_table-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h index bca0603dd..1248b9eae 100644 --- a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h +++ b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h @@ -31,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 Jia Pan */ @@ -47,16 +47,14 @@ namespace detail { //============================================================================== SpatialHash::SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) - : cell_size(cell_size_), scene_limit(scene_limit_) -{ + : cell_size(cell_size_), scene_limit(scene_limit_) { width[0] = std::ceil(scene_limit.width() / cell_size); width[1] = std::ceil(scene_limit.height() / cell_size); width[2] = std::ceil(scene_limit.depth() / cell_size); } //============================================================================== -std::vector SpatialHash::operator()(const AABB& aabb) const -{ +std::vector SpatialHash::operator()(const AABB& aabb) const { int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size); int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size); int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size); @@ -64,14 +62,12 @@ std::vector SpatialHash::operator()(const AABB& aabb) const int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size); int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size); - std::vector keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z)); + std::vector keys((max_x - min_x) * (max_y - min_y) * + (max_z - min_z)); int id = 0; - for(int x = min_x; x < max_x; ++x) - { - for(int y = min_y; y < max_y; ++y) - { - for(int z = min_z; z < max_z; ++z) - { + for (int x = min_x; x < max_x; ++x) { + for (int y = min_y; y < max_y; ++y) { + for (int z = min_z; z < max_z; ++z) { keys[id++] = x + y * width[0] + z * width[0] * width[1]; } } @@ -79,8 +75,8 @@ std::vector SpatialHash::operator()(const AABB& aabb) const return keys; } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash.h b/include/hpp/fcl/broadphase/detail/spatial_hash.h index 34e78b932..a7b711ea2 100644 --- a/include/hpp/fcl/broadphase/detail/spatial_hash.h +++ b/include/hpp/fcl/broadphase/detail/spatial_hash.h @@ -31,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 Jia Pan */ @@ -41,30 +41,25 @@ #include "hpp/fcl/BV/AABB.h" #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace detail -{ +namespace detail { /// @brief Spatial hash function: hash an AABB to a set of integer values -struct HPP_FCL_DLLAPI SpatialHash -{ +struct HPP_FCL_DLLAPI SpatialHash { SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_); - - std::vector operator() (const AABB& aabb) const; -private: + std::vector operator()(const AABB& aabb) const; + private: FCL_REAL cell_size; AABB scene_limit; unsigned int width[3]; }; -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h index 3fde0367b..092c88ae3 100644 --- a/include/hpp/fcl/collision.h +++ b/include/hpp/fcl/collision.h @@ -36,7 +36,6 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_COLLISION_H #define HPP_FCL_COLLISION_H @@ -46,44 +45,51 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -/// @brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning -/// returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function -/// performs the collision between them. +/// @brief Main collision interface: given two collision objects, and the +/// requirements for contacts, including num of max contacts, whether perform +/// exhaustive collision (i.e., returning returning all the contact points), +/// whether return detailed contact information (i.e., normal, contact point, +/// depth; otherwise only contact primitive id is returned), this function +/// performs the collision between them. /// Return value is the number of contacts generated between the two objects. -HPP_FCL_DLLAPI std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, - const CollisionRequest& request, CollisionResult& result); - -/// @copydoc collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&) -HPP_FCL_DLLAPI std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result); - -/// @copydoc collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&) -/// \note this function update the initial guess of \c request if requested. +HPP_FCL_DLLAPI std::size_t collide(const CollisionObject* o1, + const CollisionObject* o2, + const CollisionRequest& request, + CollisionResult& result); + +/// @copydoc collide(const CollisionObject*, const CollisionObject*, const +/// CollisionRequest&, CollisionResult&) +HPP_FCL_DLLAPI std::size_t collide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const CollisionRequest& request, + CollisionResult& result); + +/// @copydoc collide(const CollisionObject*, const CollisionObject*, const +/// CollisionRequest&, CollisionResult&) \note this function update the initial +/// guess of \c request if requested. /// See QueryRequest::updateGuess inline std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, - CollisionRequest& request, CollisionResult& result) -{ - std::size_t res = collide(o1, o2, (const CollisionRequest&) request, result); - request.updateGuess (result); + CollisionRequest& request, CollisionResult& result) { + std::size_t res = collide(o1, o2, (const CollisionRequest&)request, result); + request.updateGuess(result); return res; } -/// @copydoc collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&) -/// \note this function update the initial guess of \c request if requested. +/// @copydoc collide(const CollisionObject*, const CollisionObject*, const +/// CollisionRequest&, CollisionResult&) \note this function update the initial +/// guess of \c request if requested. /// See QueryRequest::updateGuess inline std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - CollisionRequest& request, CollisionResult& result) -{ - std::size_t res = collide(o1, tf1, o2, tf2, - (const CollisionRequest&) request, result); - request.updateGuess (result); + CollisionRequest& request, CollisionResult& result) { + std::size_t res = + collide(o1, tf1, o2, tf2, (const CollisionRequest&)request, result); + request.updateGuess(result); return res; } @@ -94,61 +100,56 @@ inline std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, /// ComputeCollision calc_collision (o1, o2); /// std::size_t ncontacts = calc_collision(tf1, tf2, request, result); /// \endcode -class HPP_FCL_DLLAPI ComputeCollision -{ -public: - +class HPP_FCL_DLLAPI ComputeCollision { + public: /// @brief Default constructor from two Collision Geometries. ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2); std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) const; - + const CollisionRequest& request, + CollisionResult& result) const; + inline std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2, - CollisionRequest& request, CollisionResult& result) const - { - std::size_t res = operator()(tf1, tf2, (const CollisionRequest&) request, result); + CollisionRequest& request, + CollisionResult& result) const { + std::size_t res = operator()(tf1, tf2, (const CollisionRequest&)request, + result); request.updateGuess(result); return res; } - - bool operator==(const ComputeCollision & other) const - { - return o1 == other.o1 - && o2 == other.o2 - && solver == other.solver; + + bool operator==(const ComputeCollision& other) const { + return o1 == other.o1 && o2 == other.o2 && solver == other.solver; } - - bool operator!=(const ComputeCollision & other) const - { + + bool operator!=(const ComputeCollision& other) const { return !(*this == other); } - - virtual ~ComputeCollision() {}; - -protected: - + + virtual ~ComputeCollision(){}; + + protected: // These pointers are made mutable to let the derived classes to update - // their values when updating the collision geometry (e.g. creating a new one). - // This feature should be used carefully to avoid any mis usage - // (e.g, changing the type of the collision geometry should be avoided). - mutable const CollisionGeometry *o1; - mutable const CollisionGeometry *o2; - + // their values when updating the collision geometry (e.g. creating a new + // one). This feature should be used carefully to avoid any mis usage (e.g, + // changing the type of the collision geometry should be avoided). + mutable const CollisionGeometry* o1; + mutable const CollisionGeometry* o2; + GJKSolver solver; CollisionFunctionMatrix::CollisionFunc func; bool swap_geoms; virtual std::size_t run(const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) const; -public: - + const CollisionRequest& request, + CollisionResult& result) const; + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index 22223d7af..c50cab5b2 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_COLLISION_DATA_H #define HPP_FCL_COLLISION_DATA_H @@ -48,14 +47,11 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Contact information returned by collision -struct HPP_FCL_DLLAPI Contact -{ +struct HPP_FCL_DLLAPI Contact { /// @brief collision object 1 const CollisionGeometry* o1; @@ -73,7 +69,7 @@ struct HPP_FCL_DLLAPI Contact /// if object 2 is geometry shape, it is NONE (-1), /// if object 2 is octree, it is the id of the cell int b2; - + /// @brief contact normal, pointing from o1 to o2 Vec3f normal; @@ -87,108 +83,86 @@ struct HPP_FCL_DLLAPI Contact static const int NONE = -1; /// @brief Default constructor - Contact() : o1(NULL), - o2(NULL), - b1(NONE), - b2(NONE) - {} - - Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_), - o2(o2_), - b1(b1_), - b2(b2_) - {} - - Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, - const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_), - o2(o2_), - b1(b1_), - b2(b2_), - normal(normal_), - pos(pos_), - penetration_depth(depth_) - {} - - bool operator < (const Contact& other) const - { - if(b1 == other.b1) - return b2 < other.b2; + Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {} + + Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, + int b2_) + : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {} + + Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, + int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) + : o1(o1_), + o2(o2_), + b1(b1_), + b2(b2_), + normal(normal_), + pos(pos_), + penetration_depth(depth_) {} + + bool operator<(const Contact& other) const { + if (b1 == other.b1) return b2 < other.b2; return b1 < other.b1; } - bool operator == (const Contact& other) const - { - return o1 == other.o1 - && o2 == other.o2 - && b1 == other.b1 - && b2 == other.b2 - && normal == other.normal - && pos == other.pos - && penetration_depth == other.penetration_depth; - } - - bool operator != (const Contact& other) const - { - return !(*this == other); + bool operator==(const Contact& other) const { + return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 && + b2 == other.b2 && normal == other.normal && pos == other.pos && + penetration_depth == other.penetration_depth; } + + bool operator!=(const Contact& other) const { return !(*this == other); } }; struct QueryResult; /// @brief base class for all query requests -struct HPP_FCL_DLLAPI QueryRequest -{ +struct HPP_FCL_DLLAPI QueryRequest { /// @brief whether enable gjk initial guess bool enable_cached_gjk_guess; - + /// @brief the gjk initial guess set by user Vec3f cached_gjk_guess; /// @brief the support function initial guess set by user support_func_guess_t cached_support_func_guess; - + /// @brief enable timings when performing collision/distance request bool enable_timings; - QueryRequest () : - enable_cached_gjk_guess (false), - cached_gjk_guess (1,0,0), - cached_support_func_guess(support_func_guess_t::Zero()), - enable_timings(false) - {} + QueryRequest() + : enable_cached_gjk_guess(false), + cached_gjk_guess(1, 0, 0), + cached_support_func_guess(support_func_guess_t::Zero()), + enable_timings(false) {} void updateGuess(const QueryResult& result); /// @brief whether two QueryRequest are the same or not - inline bool operator ==(const QueryRequest& other) const - { - return enable_cached_gjk_guess == other.enable_cached_gjk_guess - && cached_gjk_guess == other.cached_gjk_guess - && cached_support_func_guess == other.cached_support_func_guess - && enable_timings == other.enable_timings; + inline bool operator==(const QueryRequest& other) const { + return enable_cached_gjk_guess == other.enable_cached_gjk_guess && + cached_gjk_guess == other.cached_gjk_guess && + cached_support_func_guess == other.cached_support_func_guess && + enable_timings == other.enable_timings; } }; /// @brief base class for all query results -struct HPP_FCL_DLLAPI QueryResult -{ +struct HPP_FCL_DLLAPI QueryResult { /// @brief stores the last GJK ray when relevant. Vec3f cached_gjk_guess; /// @brief stores the last support function vertex index, when relevant. support_func_guess_t cached_support_func_guess; - + /// @brief timings for the given request CPUTimes timings; - + QueryResult() - : cached_gjk_guess(Vec3f::Zero()) - , cached_support_func_guess(support_func_guess_t::Constant(-1)) - {} + : cached_gjk_guess(Vec3f::Zero()), + cached_support_func_guess(support_func_guess_t::Constant(-1)) {} }; -inline void QueryRequest::updateGuess(const QueryResult& result) -{ +inline void QueryRequest::updateGuess(const QueryResult& result) { if (enable_cached_gjk_guess) { cached_gjk_guess = result.cached_gjk_guess; cached_support_func_guess = result.cached_support_func_guess; @@ -198,25 +172,24 @@ inline void QueryRequest::updateGuess(const QueryResult& result) struct CollisionResult; /// @brief flag declaration for specifying required params in CollisionResult -enum CollisionRequestFlag -{ - CONTACT = 0x00001, - DISTANCE_LOWER_BOUND = 0x00002, - NO_REQUEST = 0x01000 +enum CollisionRequestFlag { + CONTACT = 0x00001, + DISTANCE_LOWER_BOUND = 0x00002, + NO_REQUEST = 0x01000 }; /// @brief request to the collision algorithm -struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest -{ +struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest { /// @brief The maximum number of contacts will return size_t num_max_contacts; - /// @brief whether the contact information (normal, penetration depth and contact position) will return + /// @brief whether the contact information (normal, penetration depth and + /// contact position) will return bool enable_contact; /// Whether a lower bound on distance is returned when objects are disjoint bool enable_distance_lower_bound; - + /// @brief Distance below which objects are considered in collision. /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation /// @note If set to -inf, the objects tested for collision are considered @@ -227,11 +200,13 @@ struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest /// @brief Distance below which bounding volumes are broken down. /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation FCL_REAL break_distance; - + /// @brief Distance above which GJK solver makes an early stopping. - /// GJK stops searching for the closest points when it proves that the distance between two geometries is above this threshold. + /// GJK stops searching for the closest points when it proves that the + /// distance between two geometries is above this threshold. /// - /// @remarks Consequently, the closest points might be incorrect, but allows to save computational resources. + /// @remarks Consequently, the closest points might be incorrect, but allows + /// to save computational resources. FCL_REAL distance_upper_bound; /// @brief Constructor from a flag and a maximal number of contacts. @@ -239,121 +214,97 @@ struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest /// @param[in] flag Collision request flag /// @param[in] num_max_contacts Maximal number of allowed contacts /// - CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) : - num_max_contacts(num_max_contacts_), - enable_contact(flag & CONTACT), - enable_distance_lower_bound (flag & DISTANCE_LOWER_BOUND), - security_margin (0), - break_distance (1e-3), - distance_upper_bound ((std::numeric_limits::max)()) - { - } + CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) + : num_max_contacts(num_max_contacts_), + enable_contact(flag & CONTACT), + enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND), + security_margin(0), + break_distance(1e-3), + distance_upper_bound((std::numeric_limits::max)()) {} /// @brief Default constructor. - CollisionRequest() : - num_max_contacts(1), - enable_contact(false), - enable_distance_lower_bound (false), - security_margin (0), - break_distance (1e-3), - distance_upper_bound ((std::numeric_limits::max)()) - { - } + CollisionRequest() + : num_max_contacts(1), + enable_contact(false), + enable_distance_lower_bound(false), + security_margin(0), + break_distance(1e-3), + distance_upper_bound((std::numeric_limits::max)()) {} bool isSatisfied(const CollisionResult& result) const; /// @brief whether two CollisionRequest are the same or not - inline bool operator ==(const CollisionRequest& other) const - { - return QueryRequest::operator==(other) - && num_max_contacts == other.num_max_contacts - && enable_contact == other.enable_contact - && enable_distance_lower_bound == other.enable_distance_lower_bound - && security_margin == other.security_margin - && break_distance == other.break_distance - && distance_upper_bound == other.distance_upper_bound; + inline bool operator==(const CollisionRequest& other) const { + return QueryRequest::operator==(other) && + num_max_contacts == other.num_max_contacts && + enable_contact == other.enable_contact && + enable_distance_lower_bound == other.enable_distance_lower_bound && + security_margin == other.security_margin && + break_distance == other.break_distance && + distance_upper_bound == other.distance_upper_bound; } }; /// @brief collision result -struct HPP_FCL_DLLAPI CollisionResult : QueryResult -{ -private: +struct HPP_FCL_DLLAPI CollisionResult : QueryResult { + private: /// @brief contact information std::vector contacts; -public: + public: /// Lower bound on distance between objects if they are disjoint. /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation /// @note computed only on request (or if it does not add any computational /// overhead). FCL_REAL distance_lower_bound; -public: + public: CollisionResult() - : distance_lower_bound ((std::numeric_limits::max)()) - { - } + : distance_lower_bound((std::numeric_limits::max)()) {} /// @brief Update the lower bound only if the distance is inferior. - inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_) - { + inline void updateDistanceLowerBound(const FCL_REAL& distance_lower_bound_) { if (distance_lower_bound_ < distance_lower_bound) distance_lower_bound = distance_lower_bound_; } /// @brief add one contact into result structure - inline void addContact(const Contact& c) - { - contacts.push_back(c); - } + inline void addContact(const Contact& c) { contacts.push_back(c); } /// @brief whether two CollisionResult are the same or not - inline bool operator ==(const CollisionResult& other) const - { - return contacts == other.contacts - && distance_lower_bound == other.distance_lower_bound; + inline bool operator==(const CollisionResult& other) const { + return contacts == other.contacts && + distance_lower_bound == other.distance_lower_bound; } /// @brief return binary collision result - bool isCollision() const - { - return contacts.size() > 0; - } + bool isCollision() const { return contacts.size() > 0; } /// @brief number of contacts found - size_t numContacts() const - { - return contacts.size(); - } + size_t numContacts() const { return contacts.size(); } /// @brief get the i-th contact calculated - const Contact& getContact(size_t i) const - { - if(contacts.size() == 0) - throw std::invalid_argument("The number of contacts is zero. No Contact can be returned."); - - if(i < contacts.size()) + const Contact& getContact(size_t i) const { + if (contacts.size() == 0) + throw std::invalid_argument( + "The number of contacts is zero. No Contact can be returned."); + + if (i < contacts.size()) return contacts[i]; else return contacts.back(); } /// @brief get all the contacts - void getContacts(std::vector& contacts_) const - { + void getContacts(std::vector& contacts_) const { contacts_.resize(contacts.size()); std::copy(contacts.begin(), contacts.end(), contacts_.begin()); } - - const std::vector & getContacts() const - { - return contacts; - } + + const std::vector& getContacts() const { return contacts; } /// @brief clear the results obtained - void clear() - { + void clear() { distance_lower_bound = (std::numeric_limits::max)(); contacts.clear(); distance_lower_bound = (std::numeric_limits::max)(); @@ -368,46 +319,38 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult struct DistanceResult; /// @brief request to the distance computation -struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest -{ +struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest { /// @brief whether to return the nearest points bool enable_nearest_points; /// @brief error threshold for approximate distance - FCL_REAL rel_err; // relative error, between 0 and 1 - FCL_REAL abs_err; // absolute error + FCL_REAL rel_err; // relative error, between 0 and 1 + FCL_REAL abs_err; // absolute error /// \param enable_nearest_points_ enables the nearest points computation. /// \param rel_err_ /// \param abs_err_ - DistanceRequest(bool enable_nearest_points_ = false, - FCL_REAL rel_err_ = 0.0, - FCL_REAL abs_err_ = 0.0) : - enable_nearest_points(enable_nearest_points_), - rel_err(rel_err_), - abs_err(abs_err_) - { - } + DistanceRequest(bool enable_nearest_points_ = false, FCL_REAL rel_err_ = 0.0, + FCL_REAL abs_err_ = 0.0) + : enable_nearest_points(enable_nearest_points_), + rel_err(rel_err_), + abs_err(abs_err_) {} bool isSatisfied(const DistanceResult& result) const; /// @brief whether two DistanceRequest are the same or not - inline bool operator ==(const DistanceRequest& other) const - { - return QueryRequest::operator==(other) - && enable_nearest_points == other.enable_nearest_points - && rel_err == other.rel_err - && abs_err == other.abs_err; + inline bool operator==(const DistanceRequest& other) const { + return QueryRequest::operator==(other) && + enable_nearest_points == other.enable_nearest_points && + rel_err == other.rel_err && abs_err == other.abs_err; } }; /// @brief distance result -struct HPP_FCL_DLLAPI DistanceResult : QueryResult -{ - -public: - - /// @brief minimum distance between two objects. if two objects are in collision, min_distance <= 0. +struct HPP_FCL_DLLAPI DistanceResult : QueryResult { + public: + /// @brief minimum distance between two objects. if two objects are in + /// collision, min_distance <= 0. FCL_REAL min_distance; /// @brief nearest points @@ -436,21 +379,19 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult /// @brief invalid contact primitive information static const int NONE = -1; - - DistanceResult(FCL_REAL min_distance_ = - (std::numeric_limits::max)()): - min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) - { - const Vec3f nan (Vec3f::Constant(std::numeric_limits::quiet_NaN())); - nearest_points [0] = nearest_points [1] = normal = nan; - } + DistanceResult( + FCL_REAL min_distance_ = (std::numeric_limits::max)()) + : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) { + const Vec3f nan( + Vec3f::Constant(std::numeric_limits::quiet_NaN())); + nearest_points[0] = nearest_points[1] = normal = nan; + } /// @brief add distance information into the result - void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) - { - if(min_distance > distance) - { + void update(FCL_REAL distance, const CollisionGeometry* o1_, + const CollisionGeometry* o2_, int b1_, int b2_) { + if (min_distance > distance) { min_distance = distance; o1 = o1_; o2 = o2_; @@ -461,11 +402,9 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult /// @brief add distance information into the result void update(FCL_REAL distance, const CollisionGeometry* o1_, - const CollisionGeometry* o2_, int b1_, int b2_, - const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_) - { - if(min_distance > distance) - { + const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, + const Vec3f& p2, const Vec3f& normal_) { + if (min_distance > distance) { min_distance = distance; o1 = o1_; o2 = o2_; @@ -478,10 +417,8 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult } /// @brief add distance information into the result - void update(const DistanceResult& other_result) - { - if(min_distance > other_result.min_distance) - { + void update(const DistanceResult& other_result) { + if (min_distance > other_result.min_distance) { min_distance = other_result.min_distance; o1 = other_result.o1; o2 = other_result.o2; @@ -494,67 +431,78 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult } /// @brief clear the result - void clear() - { - const Vec3f nan (Vec3f::Constant(std::numeric_limits::quiet_NaN())); + void clear() { + const Vec3f nan( + Vec3f::Constant(std::numeric_limits::quiet_NaN())); min_distance = (std::numeric_limits::max)(); o1 = NULL; o2 = NULL; b1 = NONE; b2 = NONE; - nearest_points [0] = nearest_points [1] = normal = nan; + nearest_points[0] = nearest_points[1] = normal = nan; timings.clear(); } /// @brief whether two DistanceResult are the same or not - inline bool operator ==(const DistanceResult& other) const - { - bool is_same = min_distance == other.min_distance - && nearest_points[0] == other.nearest_points[0] - && nearest_points[1] == other.nearest_points[1] - && normal == other.normal - && o1 == other.o1 - && o2 == other.o2 - && b1 == other.b1 - && b2 == other.b2; - -// TODO: check also that two GeometryObject are indeed equal. + inline bool operator==(const DistanceResult& other) const { + bool is_same = min_distance == other.min_distance && + nearest_points[0] == other.nearest_points[0] && + nearest_points[1] == other.nearest_points[1] && + normal == other.normal && o1 == other.o1 && o2 == other.o2 && + b1 == other.b1 && b2 == other.b2; + + // TODO: check also that two GeometryObject are indeed equal. if ((o1 != NULL) ^ (other.o1 != NULL)) return false; is_same &= (o1 == other.o1); -// else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1; + // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1; if ((o2 != NULL) ^ (other.o2 != NULL)) return false; is_same &= (o2 == other.o2); -// else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2; - + // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2; + return is_same; } - }; -inline CollisionRequestFlag operator~(CollisionRequestFlag a) -{return static_cast(~static_cast(a));} - -inline CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b) -{return static_cast(static_cast(a) | static_cast(b));} +inline CollisionRequestFlag operator~(CollisionRequestFlag a) { + return static_cast(~static_cast(a)); +} -inline CollisionRequestFlag operator&(CollisionRequestFlag a, CollisionRequestFlag b) -{return static_cast(static_cast(a) & static_cast(b));} +inline CollisionRequestFlag operator|(CollisionRequestFlag a, + CollisionRequestFlag b) { + return static_cast(static_cast(a) | + static_cast(b)); +} -inline CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b) -{return static_cast(static_cast(a) ^ static_cast(b));} +inline CollisionRequestFlag operator&(CollisionRequestFlag a, + CollisionRequestFlag b) { + return static_cast(static_cast(a) & + static_cast(b)); +} -inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a, CollisionRequestFlag b) -{return (CollisionRequestFlag&)((int&)(a) |= static_cast(b));} +inline CollisionRequestFlag operator^(CollisionRequestFlag a, + CollisionRequestFlag b) { + return static_cast(static_cast(a) ^ + static_cast(b)); +} -inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a, CollisionRequestFlag b) -{return (CollisionRequestFlag&)((int&)(a) &= static_cast(b));} +inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a, + CollisionRequestFlag b) { + return (CollisionRequestFlag&)((int&)(a) |= static_cast(b)); +} -inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, CollisionRequestFlag b) -{return (CollisionRequestFlag&)((int&)(a) ^= static_cast(b));} +inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a, + CollisionRequestFlag b) { + return (CollisionRequestFlag&)((int&)(a) &= static_cast(b)); +} +inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, + CollisionRequestFlag b) { + return (CollisionRequestFlag&)((int&)(a) ^= static_cast(b)); } -} // namespace hpp +} // namespace fcl + +} // namespace hpp #endif diff --git a/include/hpp/fcl/collision_func_matrix.h b/include/hpp/fcl/collision_func_matrix.h index 3c1461444..57b78bdfa 100644 --- a/include/hpp/fcl/collision_func_matrix.h +++ b/include/hpp/fcl/collision_func_matrix.h @@ -35,41 +35,46 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_COLLISION_FUNC_MATRIX_H #define HPP_FCL_COLLISION_FUNC_MATRIX_H - #include #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -/// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface +/// @brief collision matrix stores the functions for collision between different +/// types of objects and provides a uniform call interface -struct HPP_FCL_DLLAPI CollisionFunctionMatrix -{ - /// @brief the uniform call interface for collision: for collision, we need know - /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2; - /// 2. the solver for narrow phase collision, this is for the collision between geometric shapes; - /// 3. the request setting for collision (e.g., whether need to return normal information, whether need to compute cost); +struct HPP_FCL_DLLAPI CollisionFunctionMatrix { + /// @brief the uniform call interface for collision: for collision, we need + /// know + /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 + /// and tf2; + /// 2. the solver for narrow phase collision, this is for the collision + /// between geometric shapes; + /// 3. the request setting for collision (e.g., whether need to return normal + /// information, whether need to compute cost); /// 4. the structure to return collision result - typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result); + typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); - /// @brief each item in the collision matrix is a function to handle collision between objects of type1 and type2 + /// @brief each item in the collision matrix is a function to handle collision + /// between objects of type1 and type2 CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT]; CollisionFunctionMatrix(); }; +} // namespace fcl - -} - -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h index ff1d3cb7f..6d9159c54 100644 --- a/include/hpp/fcl/collision_object.h +++ b/include/hpp/fcl/collision_object.h @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_COLLISION_OBJECT_BASE_H #define HPP_FCL_COLLISION_OBJECT_BASE_H @@ -47,37 +46,63 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief object type: BVH (mesh, points), basic geometry, octree -enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_HFIELD, OT_COUNT}; +enum OBJECT_TYPE { + OT_UNKNOWN, + OT_BVH, + OT_GEOM, + OT_OCTREE, + OT_HFIELD, + OT_COUNT +}; -/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree -enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24, - GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, GEOM_ELLIPSOID, HF_AABB, HF_OBBRSS, NODE_COUNT}; +/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, +/// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, +/// cylinder, convex, plane, triangle), and octree +enum NODE_TYPE { + BV_UNKNOWN, + BV_AABB, + BV_OBB, + BV_RSS, + BV_kIOS, + BV_OBBRSS, + BV_KDOP16, + BV_KDOP18, + BV_KDOP24, + GEOM_BOX, + GEOM_SPHERE, + GEOM_CAPSULE, + GEOM_CONE, + GEOM_CYLINDER, + GEOM_CONVEX, + GEOM_PLANE, + GEOM_HALFSPACE, + GEOM_TRIANGLE, + GEOM_OCTREE, + GEOM_ELLIPSOID, + HF_AABB, + HF_OBBRSS, + NODE_COUNT +}; /// @addtogroup Construction_Of_BVH /// @{ /// @brief The geometry for the object for collision or distance computation -class HPP_FCL_DLLAPI CollisionGeometry -{ -public: - +class HPP_FCL_DLLAPI CollisionGeometry { + public: CollisionGeometry() - : aabb_center(Vec3f::Constant((std::numeric_limits::max)())) - , aabb_radius(-1) - , cost_density(1) - , threshold_occupied(1) - , threshold_free(0) - { - } + : aabb_center(Vec3f::Constant((std::numeric_limits::max)())), + aabb_radius(-1), + cost_density(1), + threshold_occupied(1), + threshold_free(0) {} /// \brief Copy constructor - CollisionGeometry(const CollisionGeometry & other) = default; + CollisionGeometry(const CollisionGeometry& other) = default; virtual ~CollisionGeometry() {} @@ -85,22 +110,17 @@ class HPP_FCL_DLLAPI CollisionGeometry virtual CollisionGeometry* clone() const = 0; /// \brief Equality operator - bool operator==(const CollisionGeometry & other) const - { - return - cost_density == other.cost_density - && threshold_occupied == other.threshold_occupied - && threshold_free == other.threshold_free - && aabb_center == other.aabb_center - && aabb_radius == other.aabb_radius - && aabb_local == other.aabb_local - && isEqual(other) - ; + bool operator==(const CollisionGeometry& other) const { + return cost_density == other.cost_density && + threshold_occupied == other.threshold_occupied && + threshold_free == other.threshold_free && + aabb_center == other.aabb_center && + aabb_radius == other.aabb_radius && aabb_local == other.aabb_local && + isEqual(other); } /// \brief Difference operator - bool operator!=(const CollisionGeometry & other) const - { + bool operator!=(const CollisionGeometry& other) const { return isNotEqual(other); } @@ -114,24 +134,16 @@ class HPP_FCL_DLLAPI CollisionGeometry virtual void computeLocalAABB() = 0; /// @brief get user data in geometry - void* getUserData() const - { - return user_data; - } + void* getUserData() const { return user_data; } /// @brief set user data in geometry - void setUserData(void *data) - { - user_data = data; - } + void setUserData(void* data) { user_data = data; } /// @brief whether the object is completely occupied - inline bool isOccupied() const - { return cost_density >= threshold_occupied; } + inline bool isOccupied() const { return cost_density >= threshold_occupied; } /// @brief whether the object is completely free - inline bool isFree() const - { return cost_density <= threshold_free; } + inline bool isFree() const { return cost_density <= threshold_free; } /// @brief whether the object has some uncertainty bool isUncertain() const; @@ -142,11 +154,12 @@ class HPP_FCL_DLLAPI CollisionGeometry /// @brief AABB radius FCL_REAL aabb_radius; - /// @brief AABB in local coordinate, used for tight AABB when only translation transform + /// @brief AABB in local coordinate, used for tight AABB when only translation + /// transform AABB aabb_local; /// @brief pointer to user defined data specific to this object - void *user_data; + void* user_data; /// @brief collision cost for unit volume FCL_REAL cost_density; @@ -161,123 +174,93 @@ class HPP_FCL_DLLAPI CollisionGeometry virtual Vec3f computeCOM() const { return Vec3f::Zero(); } /// @brief compute the inertia matrix, related to the origin - virtual Matrix3f computeMomentofInertia() const { return Matrix3f::Constant(NAN); } + virtual Matrix3f computeMomentofInertia() const { + return Matrix3f::Constant(NAN); + } /// @brief compute the volume virtual FCL_REAL computeVolume() const { return 0; } /// @brief compute the inertia matrix, related to the com - virtual Matrix3f computeMomentofInertiaRelatedToCOM() const - { + virtual Matrix3f computeMomentofInertiaRelatedToCOM() const { Matrix3f C = computeMomentofInertia(); Vec3f com = computeCOM(); FCL_REAL V = computeVolume(); return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), - C(0, 1) + V * com[0] * com[1], - C(0, 2) + V * com[0] * com[2], - C(1, 0) + V * com[1] * com[0], - C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), - C(1, 2) + V * com[1] * com[2], - C(2, 0) + V * com[2] * com[0], - C(2, 1) + V * com[2] * com[1], - C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])).finished(); + C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], + C(1, 0) + V * com[1] * com[0], + C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), + C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0], + C(2, 1) + V * com[2] * com[1], + C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])) + .finished(); } -private: - + private: /// @brief equal operator with another object of derived type. - virtual bool isEqual(const CollisionGeometry & other) const = 0; - + virtual bool isEqual(const CollisionGeometry& other) const = 0; + /// @brief not equal operator with another object of derived type. - virtual bool isNotEqual(const CollisionGeometry & other) const - { + virtual bool isNotEqual(const CollisionGeometry& other) const { return !(*this == other); } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -/// @brief the object for collision or distance computation, contains the geometry and the transform information -class HPP_FCL_DLLAPI CollisionObject -{ -public: - CollisionObject(const shared_ptr &cgeom_, - bool compute_local_aabb = true) - : cgeom(cgeom_) - , user_data(nullptr) - { +/// @brief the object for collision or distance computation, contains the +/// geometry and the transform information +class HPP_FCL_DLLAPI CollisionObject { + public: + CollisionObject(const shared_ptr& cgeom_, + bool compute_local_aabb = true) + : cgeom(cgeom_), user_data(nullptr) { init(compute_local_aabb); } - CollisionObject(const shared_ptr &cgeom_, const Transform3f& tf, - bool compute_local_aabb = true) - : cgeom(cgeom_) - , t(tf) - , user_data(nullptr) - { + CollisionObject(const shared_ptr& cgeom_, + const Transform3f& tf, bool compute_local_aabb = true) + : cgeom(cgeom_), t(tf), user_data(nullptr) { init(compute_local_aabb); } - CollisionObject(const shared_ptr &cgeom_, const Matrix3f& R, const Vec3f& T, + CollisionObject(const shared_ptr& cgeom_, + const Matrix3f& R, const Vec3f& T, bool compute_local_aabb = true) - : cgeom(cgeom_) - , t(R, T) - , user_data(nullptr) - { + : cgeom(cgeom_), t(R, T), user_data(nullptr) { init(compute_local_aabb); } - - bool operator==(const CollisionObject & other) const - { + + bool operator==(const CollisionObject& other) const { return cgeom == other.cgeom && t == other.t && user_data == other.user_data; } - - bool operator!=(const CollisionObject & other) const - { + + bool operator!=(const CollisionObject& other) const { return !(*this == other); } - ~CollisionObject() - { - } + ~CollisionObject() {} /// @brief get the type of the object - OBJECT_TYPE getObjectType() const - { - return cgeom->getObjectType(); - } + OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); } /// @brief get the node type - NODE_TYPE getNodeType() const - { - return cgeom->getNodeType(); - } + NODE_TYPE getNodeType() const { return cgeom->getNodeType(); } /// @brief get the AABB in world space - const AABB& getAABB() const - { - return aabb; - } + const AABB& getAABB() const { return aabb; } /// @brief get the AABB in world space - AABB& getAABB() - { - return aabb; - } + AABB& getAABB() { return aabb; } /// @brief compute the AABB in world space - void computeAABB() - { - if(t.getRotation().isIdentity()) - { + void computeAABB() { + if (t.getRotation().isIdentity()) { aabb = translate(cgeom->aabb_local, t.getTranslation()); - } - else - { - Vec3f center (t.transform(cgeom->aabb_center)); + } else { + Vec3f center(t.transform(cgeom->aabb_center)); Vec3f delta(Vec3f::Constant(cgeom->aabb_radius)); aabb.min_ = center - delta; aabb.max_ = center + delta; @@ -285,105 +268,65 @@ class HPP_FCL_DLLAPI CollisionObject } /// @brief get user data in object - void* getUserData() const - { - return user_data; - } + void* getUserData() const { return user_data; } /// @brief set user data in object - void setUserData(void *data) - { - user_data = data; - } + void setUserData(void* data) { user_data = data; } /// @brief get translation of the object - inline const Vec3f& getTranslation() const - { - return t.getTranslation(); - } + inline const Vec3f& getTranslation() const { return t.getTranslation(); } /// @brief get matrix rotation of the object - inline const Matrix3f& getRotation() const - { - return t.getRotation(); - } + inline const Matrix3f& getRotation() const { return t.getRotation(); } /// @brief get object's transform - inline const Transform3f& getTransform() const - { - return t; - } + inline const Transform3f& getTransform() const { return t; } /// @brief set object's rotation matrix - void setRotation(const Matrix3f& R) - { - t.setRotation(R); - } + void setRotation(const Matrix3f& R) { t.setRotation(R); } /// @brief set object's translation - void setTranslation(const Vec3f& T) - { - t.setTranslation(T); - } + void setTranslation(const Vec3f& T) { t.setTranslation(T); } /// @brief set object's transform - void setTransform(const Matrix3f& R, const Vec3f& T) - { - t.setTransform(R, T); - } + void setTransform(const Matrix3f& R, const Vec3f& T) { t.setTransform(R, T); } /// @brief set object's transform - void setTransform(const Transform3f& tf) - { - t = tf; - } + void setTransform(const Transform3f& tf) { t = tf; } /// @brief whether the object is in local coordinate - bool isIdentityTransform() const - { - return t.isIdentity(); - } + bool isIdentityTransform() const { return t.isIdentity(); } /// @brief set the object in local coordinate - void setIdentityTransform() - { - t.setIdentity(); - } + void setIdentityTransform() { t.setIdentity(); } /// @brief get geometry from the object instance - const shared_ptr collisionGeometry() const - { + const shared_ptr collisionGeometry() const { return cgeom; } /// @brief get geometry from the object instance - const shared_ptr& collisionGeometry() - { - return cgeom; - } + const shared_ptr& collisionGeometry() { return cgeom; } /// @brief Associate a new CollisionGeometry /// /// @param[in] collision_geometry The new CollisionGeometry - /// @param[in] compute_local_aabb Whether the local aabb of the input new has to be computed. + /// @param[in] compute_local_aabb Whether the local aabb of the input new has + /// to be computed. /// - void setCollisionGeometry(const shared_ptr & collision_geometry, - bool compute_local_aabb = true) - { - if(collision_geometry.get() != cgeom.get()) - { + void setCollisionGeometry( + const shared_ptr& collision_geometry, + bool compute_local_aabb = true) { + if (collision_geometry.get() != cgeom.get()) { cgeom = collision_geometry; init(compute_local_aabb); } } -protected: - - void init(bool compute_local_aabb = true) - { - if (cgeom) - { - if(compute_local_aabb) cgeom->computeLocalAABB(); + protected: + void init(bool compute_local_aabb = true) { + if (cgeom) { + if (compute_local_aabb) cgeom->computeLocalAABB(); computeAABB(); } } @@ -396,11 +339,11 @@ class HPP_FCL_DLLAPI CollisionObject mutable AABB aabb; /// @brief pointer to user defined data specific to this object - void *user_data; + void* user_data; }; -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/collision_utility.h b/include/hpp/fcl/collision_utility.h index c9a7345a0..4dc889fe4 100644 --- a/include/hpp/fcl/collision_utility.h +++ b/include/hpp/fcl/collision_utility.h @@ -19,15 +19,13 @@ #include -namespace hpp -{ -namespace fcl -{ - HPP_FCL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, - const Transform3f& pose, - const AABB& aabb); +namespace hpp { +namespace fcl { +HPP_FCL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, + const Transform3f& pose, + const AABB& aabb); } -} // namespace hpp +} // namespace hpp -#endif // FCL_COLLISION_UTILITY_H +#endif // FCL_COLLISION_UTILITY_H diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h index ea8e2549f..31c27fbd8 100644 --- a/include/hpp/fcl/data_types.h +++ b/include/hpp/fcl/data_types.h @@ -43,26 +43,25 @@ #include -namespace hpp -{ +namespace hpp { #ifdef HPP_FCL_HAS_OCTOMAP - #define OCTOMAP_VERSION_AT_LEAST(x,y,z) \ - (OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \ - (OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \ - OCTOMAP_PATCH_VERSION >= z)))) - - #define OCTOMAP_VERSION_AT_MOST(x,y,z) \ - (OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \ - (OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \ - OCTOMAP_PATCH_VERSION <= z)))) -#endif // HPP_FCL_HAS_OCTOMAP -} - -namespace hpp -{ -namespace fcl -{ +#define OCTOMAP_VERSION_AT_LEAST(x, y, z) \ + (OCTOMAP_MAJOR_VERSION > x || \ + (OCTOMAP_MAJOR_VERSION >= x && \ + (OCTOMAP_MINOR_VERSION > y || \ + (OCTOMAP_MINOR_VERSION >= y && OCTOMAP_PATCH_VERSION >= z)))) + +#define OCTOMAP_VERSION_AT_MOST(x, y, z) \ + (OCTOMAP_MAJOR_VERSION < x || \ + (OCTOMAP_MAJOR_VERSION <= x && \ + (OCTOMAP_MINOR_VERSION < y || \ + (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z)))) +#endif // HPP_FCL_HAS_OCTOMAP +} // namespace hpp + +namespace hpp { +namespace fcl { typedef double FCL_REAL; typedef Eigen::Matrix Vec3f; typedef Eigen::Matrix VecXf; @@ -73,9 +72,8 @@ typedef Eigen::Matrix MatrixXf; typedef Eigen::Vector2i support_func_guess_t; /// @brief Triangle with 3 indices for points -class HPP_FCL_DLLAPI Triangle -{ -public: +class HPP_FCL_DLLAPI Triangle { + public: typedef std::size_t index_type; typedef int size_type; @@ -83,15 +81,13 @@ class HPP_FCL_DLLAPI Triangle Triangle() {} /// @brief Create a triangle with given vertex indices - Triangle(index_type p1, index_type p2, index_type p3) - { - set(p1, p2, p3); - } + Triangle(index_type p1, index_type p2, index_type p3) { set(p1, p2, p3); } /// @brief Set the vertex indices of the triangle - inline void set(index_type p1, index_type p2, index_type p3) - { - vids[0] = p1; vids[1] = p2; vids[2] = p3; + inline void set(index_type p1, index_type p2, index_type p3) { + vids[0] = p1; + vids[1] = p2; + vids[2] = p3; } /// @brief Access the triangle index @@ -101,40 +97,35 @@ class HPP_FCL_DLLAPI Triangle static inline size_type size() { return 3; } - bool operator==(const Triangle& other) const - { - return vids[0] == other.vids[0] - && vids[1] == other.vids[1] - && vids[2] == other.vids[2]; - } - - bool operator!=(const Triangle& other) const - { - return !(*this == other); + bool operator==(const Triangle& other) const { + return vids[0] == other.vids[0] && vids[1] == other.vids[1] && + vids[2] == other.vids[2]; } -private: + bool operator!=(const Triangle& other) const { return !(*this == other); } + + private: /// @brief indices for each vertex of triangle index_type vids[3]; }; /// @brief Quadrilateral with 4 indices for points -struct HPP_FCL_DLLAPI Quadrilateral -{ +struct HPP_FCL_DLLAPI Quadrilateral { typedef std::size_t index_type; typedef int size_type; Quadrilateral() {} - Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3) - { + Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3) { set(p0, p1, p2, p3); } /// @brief Set the vertex indices of the quadrilateral - inline void set(index_type p0, index_type p1, index_type p2, index_type p3) - { - vids[0] = p0; vids[1] = p1; vids[2] = p2; vids[3] = p3; + inline void set(index_type p0, index_type p1, index_type p2, index_type p3) { + vids[0] = p0; + vids[1] = p1; + vids[2] = p2; + vids[3] = p3; } /// @access the quadrilateral index @@ -143,27 +134,22 @@ struct HPP_FCL_DLLAPI Quadrilateral inline index_type& operator[](index_type i) { return vids[i]; } static inline size_type size() { return 4; } - - bool operator==(const Quadrilateral& other) const - { - return vids[0] == other.vids[0] - && vids[1] == other.vids[1] - && vids[2] == other.vids[2] - && vids[3] == other.vids[3]; + + bool operator==(const Quadrilateral& other) const { + return vids[0] == other.vids[0] && vids[1] == other.vids[1] && + vids[2] == other.vids[2] && vids[3] == other.vids[3]; } - - bool operator!=(const Quadrilateral& other) const - { + + bool operator!=(const Quadrilateral& other) const { return !(*this == other); } -private: + private: index_type vids[4]; }; -} - -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h index dffaca99f..c3e7b2fda 100644 --- a/include/hpp/fcl/distance.h +++ b/include/hpp/fcl/distance.h @@ -43,42 +43,48 @@ #include #include -namespace hpp -{ -namespace fcl -{ - -/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. -/// Return value is the minimum distance generated between the two objects. -HPP_FCL_DLLAPI FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, - const DistanceRequest& request, DistanceResult& result); - -/// @copydoc distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&) -HPP_FCL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result); - -/// @copydoc distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&) -/// \note this function update the initial guess of \c request if requested. +namespace hpp { +namespace fcl { + +/// @brief Main distance interface: given two collision objects, and the +/// requirements for contacts, including whether return the nearest points, this +/// function performs the distance between them. Return value is the minimum +/// distance generated between the two objects. +HPP_FCL_DLLAPI FCL_REAL distance(const CollisionObject* o1, + const CollisionObject* o2, + const DistanceRequest& request, + DistanceResult& result); + +/// @copydoc distance(const CollisionObject*, const CollisionObject*, const +/// DistanceRequest&, DistanceResult&) +HPP_FCL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result); + +/// @copydoc distance(const CollisionObject*, const CollisionObject*, const +/// DistanceRequest&, DistanceResult&) \note this function update the initial +/// guess of \c request if requested. /// See QueryRequest::updateGuess inline FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, - DistanceRequest& request, DistanceResult& result) -{ - FCL_REAL res = distance(o1, o2, (const DistanceRequest&) request, result); - request.updateGuess (result); + DistanceRequest& request, DistanceResult& result) { + FCL_REAL res = distance(o1, o2, (const DistanceRequest&)request, result); + request.updateGuess(result); return res; } -/// @copydoc distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&) -/// \note this function update the initial guess of \c request if requested. +/// @copydoc distance(const CollisionObject*, const CollisionObject*, const +/// DistanceRequest&, DistanceResult&) \note this function update the initial +/// guess of \c request if requested. /// See QueryRequest::updateGuess inline FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - DistanceRequest& request, DistanceResult& result) -{ - FCL_REAL res = distance(o1, tf1, o2, tf2, - (const DistanceRequest&) request, result); - request.updateGuess (result); + DistanceRequest& request, DistanceResult& result) { + FCL_REAL res = + distance(o1, tf1, o2, tf2, (const DistanceRequest&)request, result); + request.updateGuess(result); return res; } @@ -90,60 +96,55 @@ inline FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, /// FCL_REAL distance = calc_distance(tf1, tf2, request, result); /// \endcode class HPP_FCL_DLLAPI ComputeDistance { -public: + public: ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2); FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) const; + const DistanceRequest& request, + DistanceResult& result) const; inline FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2, - DistanceRequest& request, DistanceResult& result) const - { - FCL_REAL res = operator()(tf1, tf2, (const DistanceRequest&) request, result); - request.updateGuess (result); + DistanceRequest& request, + DistanceResult& result) const { + FCL_REAL res = operator()(tf1, tf2, (const DistanceRequest&)request, + result); + request.updateGuess(result); return res; } - - bool operator==(const ComputeDistance & other) const - { - return o1 == other.o1 - && o2 == other.o2 - && swap_geoms == other.swap_geoms - && solver == other.solver - && func == other.func; + + bool operator==(const ComputeDistance& other) const { + return o1 == other.o1 && o2 == other.o2 && swap_geoms == other.swap_geoms && + solver == other.solver && func == other.func; } - - bool operator!=(const ComputeDistance & other) const - { + + bool operator!=(const ComputeDistance& other) const { return !(*this == other); } - - virtual ~ComputeDistance() {}; -protected: - + virtual ~ComputeDistance(){}; + + protected: // These pointers are made mutable to let the derived classes to update - // their values when updating the collision geometry (e.g. creating a new one). - // This feature should be used carefully to avoid any mis usage - // (e.g, changing the type of the collision geometry should be avoided). - mutable const CollisionGeometry *o1; - mutable const CollisionGeometry *o2; - + // their values when updating the collision geometry (e.g. creating a new + // one). This feature should be used carefully to avoid any mis usage (e.g, + // changing the type of the collision geometry should be avoided). + mutable const CollisionGeometry* o1; + mutable const CollisionGeometry* o2; + GJKSolver solver; DistanceFunctionMatrix::DistanceFunc func; bool swap_geoms; - + virtual FCL_REAL run(const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) const; -public: + const DistanceRequest& request, + DistanceResult& result) const; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - - -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/distance_func_matrix.h b/include/hpp/fcl/distance_func_matrix.h index bfc3f894f..cf51cb906 100644 --- a/include/hpp/fcl/distance_func_matrix.h +++ b/include/hpp/fcl/distance_func_matrix.h @@ -42,31 +42,36 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -/// @brief distance matrix stores the functions for distance between different types of objects and provides a uniform call interface -struct HPP_FCL_DLLAPI DistanceFunctionMatrix -{ +/// @brief distance matrix stores the functions for distance between different +/// types of objects and provides a uniform call interface +struct HPP_FCL_DLLAPI DistanceFunctionMatrix { /// @brief the uniform call interface for distance: for distance, we need know - /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2; - /// 2. the solver for narrow phase collision, this is for distance computation between geometric shapes; - /// 3. the request setting for distance (e.g., whether need to return nearest points); - typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 + /// and tf2; + /// 2. the solver for narrow phase collision, this is for distance computation + /// between geometric shapes; + /// 3. the request setting for distance (e.g., whether need to return nearest + /// points); + typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result); - - /// @brief each item in the distance matrix is a function to handle distance between objects of type1 and type2 + const DistanceRequest& request, + DistanceResult& result); + + /// @brief each item in the distance matrix is a function to handle distance + /// between objects of type1 and type2 DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT]; DistanceFunctionMatrix(); }; -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/doc.hh b/include/hpp/fcl/doc.hh index a5aad2bf4..4d645bf23 100644 --- a/include/hpp/fcl/doc.hh +++ b/include/hpp/fcl/doc.hh @@ -32,10 +32,8 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// \mainpage /// \anchor hpp_fcl_documentation @@ -52,10 +50,13 @@ namespace fcl /// \par Using hpp-fcl /// /// The main entry points to the library are functions -/// \li hpp::fcl::collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&) -/// \li hpp::fcl::distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&) +/// \li hpp::fcl::collide(const CollisionObject*, const CollisionObject*, const +/// CollisionRequest&, CollisionResult&) \li hpp::fcl::distance(const +/// CollisionObject*, const CollisionObject*, const DistanceRequest&, +/// DistanceResult&) /// -/// \section hpp_fcl_collision_and_distance_lower_bound_computation Collision detection and distance lower bound +/// \section hpp_fcl_collision_and_distance_lower_bound_computation Collision +/// detection and distance lower bound /// /// Collision queries can return a distance lower bound between the two objects, /// which is computationally cheaper than computing the real distance. @@ -75,5 +76,5 @@ namespace fcl /// hpp::fcl::ComputeCollision and the objects are considered as not /// colliding. -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh index be4ccc0ee..524510795 100644 --- a/include/hpp/fcl/fwd.hh +++ b/include/hpp/fcl/fwd.hh @@ -43,51 +43,50 @@ #include #if _WIN32 - #define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__ +#define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__ #else - #define HPP_FCL_PRETTY_FUNCTION __PRETTY_FUNCTION__ +#define HPP_FCL_PRETTY_FUNCTION __PRETTY_FUNCTION__ #endif #define HPP_FCL_UNUSED_VARIABLE(var) (void)(var) -#define HPP_FCL_THROW_PRETTY(message,exception) \ -{ \ -std::stringstream ss; \ -ss << "From file: " << __FILE__ << "\n"; \ -ss << "in function: " << HPP_FCL_PRETTY_FUNCTION << "\n"; \ -ss << "at line: " << __LINE__ << "\n"; \ -ss << "message: " << message << "\n"; \ -throw exception(ss.str()); \ -} +#define HPP_FCL_THROW_PRETTY(message, exception) \ + { \ + std::stringstream ss; \ + ss << "From file: " << __FILE__ << "\n"; \ + ss << "in function: " << HPP_FCL_PRETTY_FUNCTION << "\n"; \ + ss << "at line: " << __LINE__ << "\n"; \ + ss << "message: " << message << "\n"; \ + throw exception(ss.str()); \ + } #if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) - #define HPP_FCL_WITH_CXX11_SUPPORT +#define HPP_FCL_WITH_CXX11_SUPPORT #endif namespace hpp { namespace fcl { - using std::shared_ptr; - using std::dynamic_pointer_cast; - using std::make_shared; +using std::dynamic_pointer_cast; +using std::make_shared; +using std::shared_ptr; - class CollisionObject; - typedef shared_ptr CollisionObjectPtr_t; - typedef shared_ptr < const CollisionObject> CollisionObjectConstPtr_t; - class CollisionGeometry; - typedef shared_ptr CollisionGeometryPtr_t; - typedef shared_ptr - CollisionGeometryConstPtr_t; - class Transform3f; +class CollisionObject; +typedef shared_ptr CollisionObjectPtr_t; +typedef shared_ptr CollisionObjectConstPtr_t; +class CollisionGeometry; +typedef shared_ptr CollisionGeometryPtr_t; +typedef shared_ptr CollisionGeometryConstPtr_t; +class Transform3f; - class AABB; +class AABB; - class BVHModelBase; - typedef shared_ptr BVHModelPtr_t; +class BVHModelBase; +typedef shared_ptr BVHModelPtr_t; - class OcTree; - typedef shared_ptr OcTreePtr_t; - typedef shared_ptr OcTreeConstPtr_t; -} -} // namespace hpp +class OcTree; +typedef shared_ptr OcTreePtr_t; +typedef shared_ptr OcTreeConstPtr_t; +} // namespace fcl +} // namespace hpp -#endif // HPP_FCL_FWD_HH +#endif // HPP_FCL_FWD_HH diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h index a7ac1ab82..eacd611ea 100644 --- a/include/hpp/fcl/hfield.h +++ b/include/hpp/fcl/hfield.h @@ -45,98 +45,82 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @addtogroup Construction_Of_HeightField /// @{ -struct HPP_FCL_DLLAPI HFNodeBase -{ +struct HPP_FCL_DLLAPI HFNodeBase { /// @brief An index for first child node or primitive /// If the value is positive, it is the index of the first child bv node /// If the value is negative, it is -(primitive index + 1) /// Zero is not used. size_t first_child; - + Eigen::DenseIndex x_id, x_size; Eigen::DenseIndex y_id, y_size; - + /// @brief Default constructor - HFNodeBase() - : first_child(0) - , x_id(-1), x_size(0) - , y_id(-1), y_size(0) - {} - + HFNodeBase() : first_child(0), x_id(-1), x_size(0), y_id(-1), y_size(0) {} + /// @brief Comparison operator - bool operator==(const HFNodeBase & other) const - { - return - first_child == other.first_child - && x_id == other.x_id - && x_size == other.x_size - && y_id == other.y_id - && y_size == other.y_size; + bool operator==(const HFNodeBase& other) const { + return first_child == other.first_child && x_id == other.x_id && + x_size == other.x_size && y_id == other.y_id && + y_size == other.y_size; } - + /// @brief Difference operator - bool operator!=(const HFNodeBase & other) const - { - return !(*this == other); - } + bool operator!=(const HFNodeBase& other) const { return !(*this == other); } - /// @brief Whether current node is a leaf node (i.e. contains a primitive index) + /// @brief Whether current node is a leaf node (i.e. contains a primitive + /// index) inline bool isLeaf() const { return x_size == 1 && y_size == 1; } - /// @brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel + /// @brief Return the index of the first child. The index is referred to the + /// bounding volume array (i.e. bvs) in BVHModel inline size_t leftChild() const { return first_child; } - /// @brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel + /// @brief Return the index of the second child. The index is referred to the + /// bounding volume array (i.e. bvs) in BVHModel inline size_t rightChild() const { return first_child + 1; } - - inline Eigen::Vector2i leftChildIndexes() const { return Eigen::Vector2i(x_id,y_id); } - inline Eigen::Vector2i rightChildIndexes() const { return Eigen::Vector2i(x_id + x_size/2, y_id + y_size/2); } + + inline Eigen::Vector2i leftChildIndexes() const { + return Eigen::Vector2i(x_id, y_id); + } + inline Eigen::Vector2i rightChildIndexes() const { + return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2); + } }; -template -struct HPP_FCL_DLLAPI HFNode -: public HFNodeBase -{ +template +struct HPP_FCL_DLLAPI HFNode : public HFNodeBase { typedef HFNodeBase Base; - + /// @brief bounding volume storing the geometry BV bv; - + /// @brief Equality operator - bool operator==(const HFNode & other) const - { + bool operator==(const HFNode& other) const { return Base::operator==(other) && bv == other.bv; } - + /// @brief Difference operator - bool operator!=(const HFNode & other) const - { - return !(*this == other); - } + bool operator!=(const HFNode& other) const { return !(*this == other); } /// @brief Check whether two BVNode collide - bool overlap(const HFNode& other) const - { - return bv.overlap(other.bv); - } + bool overlap(const HFNode& other) const { return bv.overlap(other.bv); } /// @brief Check whether two BVNode collide bool overlap(const HFNode& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const - { + FCL_REAL& sqrDistLowerBound) const { return bv.overlap(other.bv, request, sqrDistLowerBound); } - /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distance, return the nearest points. - FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const - { + /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and + /// the underlying BV supports distance, return the nearest points. + FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL, + Vec3f* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } @@ -144,12 +128,11 @@ struct HPP_FCL_DLLAPI HFNode Vec3f getCenter() const { return bv.center(); } /// @brief Access to the orientation of the BV - const Matrix3f& getOrientation() const - { + const Matrix3f& getOrientation() const { static const Matrix3f id3 = Matrix3f::Identity(); return id3; } - + virtual ~HFNode() {} /// \cond @@ -157,172 +140,159 @@ struct HPP_FCL_DLLAPI HFNode /// \endcond }; -namespace details -{ +namespace details { - template - struct UpdateBoundingVolume - { - static void run(const Vec3f & pointA, const Vec3f & pointB, BV & bv) - { - AABB bv_aabb(pointA,pointB); -// AABB bv_aabb; -// bv_aabb.update(pointA,pointB); - convertBV(bv_aabb,bv); - } - }; +template +struct UpdateBoundingVolume { + static void run(const Vec3f& pointA, const Vec3f& pointB, BV& bv) { + AABB bv_aabb(pointA, pointB); + // AABB bv_aabb; + // bv_aabb.update(pointA,pointB); + convertBV(bv_aabb, bv); + } +}; - template<> - struct UpdateBoundingVolume - { - static void run(const Vec3f & pointA, const Vec3f & pointB, AABB & bv) - { - AABB bv_aabb(pointA,pointB); - convertBV(bv_aabb,bv); -// bv.update(pointA,pointB); - } - }; +template <> +struct UpdateBoundingVolume { + static void run(const Vec3f& pointA, const Vec3f& pointB, AABB& bv) { + AABB bv_aabb(pointA, pointB); + convertBV(bv_aabb, bv); + // bv.update(pointA,pointB); + } +}; -} +} // namespace details -/// @brief Data structure depicting a height field given by the base grid dimensions and the elevation along the grid. -/// \tparam BV one of the bounding volume class in \ref Bounding_Volume. +/// @brief Data structure depicting a height field given by the base grid +/// dimensions and the elevation along the grid. \tparam BV one of the bounding +/// volume class in \ref Bounding_Volume. /// -/// An height field is defined by its base dimensions along the X and Y axes and a set ofpoints defined by their altitude, regularly dispatched on the grid. -/// The height field is centered at the origin and the corners of the geometry correspond to the following coordinates [± x_dim/2; ± y_dim/2]. -template -class HPP_FCL_DLLAPI HeightField -: public CollisionGeometry -{ -public: - +/// An height field is defined by its base dimensions along the X and Y axes and +/// a set ofpoints defined by their altitude, regularly dispatched on the grid. +/// The height field is centered at the origin and the corners of the geometry +/// correspond to the following coordinates [± x_dim/2; ± y_dim/2]. +template +class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { + public: typedef CollisionGeometry Base; - + typedef HFNode Node; typedef std::vector BVS; - + /// @brief Constructing an empty HeightField HeightField() - : CollisionGeometry() - , min_height((std::numeric_limits::min)()) - , max_height((std::numeric_limits::max)()) - {} - - /// @brief Constructing an HeightField from its base dimensions and the set of heights points. - /// The granularity of the height field along X and Y direction is extraded from the Z grid. + : CollisionGeometry(), + min_height((std::numeric_limits::min)()), + max_height((std::numeric_limits::max)()) {} + + /// @brief Constructing an HeightField from its base dimensions and the set of + /// heights points. + /// The granularity of the height field along X and Y direction is + /// extraded from the Z grid. /// /// \param[in] x_dim Dimension along the X axis /// \param[in] y_dim Dimension along the Y axis - /// \param[in] heights Matrix containing the altitude of each point compositng the height field - /// \param[in] min_height Minimal height of the height field + /// \param[in] heights Matrix containing the altitude of each point compositng + /// the height field \param[in] min_height Minimal height of the height field /// - HeightField(const FCL_REAL x_dim, - const FCL_REAL y_dim, - const MatrixXf & heights, - const FCL_REAL min_height = (FCL_REAL)0) - : CollisionGeometry() - { - init(x_dim,y_dim,heights,min_height); + HeightField(const FCL_REAL x_dim, const FCL_REAL y_dim, + const MatrixXf& heights, const FCL_REAL min_height = (FCL_REAL)0) + : CollisionGeometry() { + init(x_dim, y_dim, heights, min_height); } /// @brief Copy contructor from another HeightField /// /// \param[in] other to copy. /// - HeightField(const HeightField & other) - : CollisionGeometry(other) - , x_dim(other.x_dim) - , y_dim(other.y_dim) - , heights(other.heights) - , min_height(other.min_height) - , max_height(other.max_height) - , x_grid(other.x_grid) - , y_grid(other.y_grid) - , bvs(other.bvs) - , num_bvs(other.num_bvs) - { - - } - + HeightField(const HeightField& other) + : CollisionGeometry(other), + x_dim(other.x_dim), + y_dim(other.y_dim), + heights(other.heights), + min_height(other.min_height), + max_height(other.max_height), + x_grid(other.x_grid), + y_grid(other.y_grid), + bvs(other.bvs), + num_bvs(other.num_bvs) {} + /// @brief Returns a const reference of the grid along the X direction. - const VecXf & getXGrid() const { return x_grid; } + const VecXf& getXGrid() const { return x_grid; } /// @brief Returns a const reference of the grid along the Y direction. - const VecXf & getYGrid() const { return y_grid; } - + const VecXf& getYGrid() const { return y_grid; } + /// @brief Returns a const reference of the heights - const MatrixXf & getHeights() const { return heights; } - + const MatrixXf& getHeights() const { return heights; } + /// @brief Returns the dimension of the Height Field along the X direction. FCL_REAL getXDim() const { return x_dim; } /// @brief Returns the dimension of the Height Field along the Y direction. FCL_REAL getYDim() const { return y_dim; } - + /// @brief Returns the minimal height value of the Height Field. FCL_REAL getMinHeight() const { return min_height; } /// @brief Returns the maximal height value of the Height Field. FCL_REAL getMaxHeight() const { return max_height; } - - virtual HeightField * clone() const - { - return new HeightField(*this); - } - + + virtual HeightField* clone() const { return new HeightField(*this); } + /// @brief deconstruction, delete mesh data related. virtual ~HeightField() {} - - /// @brief Compute the AABB for the HeightField, used for broad-phase collision - void computeLocalAABB() - { - const Vec3f A(x_grid[0],y_grid[0],min_height); - const Vec3f B(x_grid[x_grid.size()-1],y_grid[y_grid.size()-1],max_height); - const AABB aabb_(A,B); - - aabb_radius = (A-B).norm()/2.; + + /// @brief Compute the AABB for the HeightField, used for broad-phase + /// collision + void computeLocalAABB() { + const Vec3f A(x_grid[0], y_grid[0], min_height); + const Vec3f B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1], + max_height); + const AABB aabb_(A, B); + + aabb_radius = (A - B).norm() / 2.; aabb_local = aabb_; aabb_center = aabb_.center(); } - + /// @brief Update Height Field height - void updateHeights(const MatrixXf & new_heights) - { - if(new_heights.rows() != heights.rows() || new_heights.cols() != heights.cols()) - HPP_FCL_THROW_PRETTY("The matrix containing the new heights values does not have the same matrix size as the original one.\n" - "\tinput values - rows: " << new_heights.rows() << " - cols: " << new_heights.cols() << "\n" << - "\texpected values - rows: " << heights.rows() << " - cols: " << heights.cols() << "\n" - ,std::invalid_argument); - + void updateHeights(const MatrixXf& new_heights) { + if (new_heights.rows() != heights.rows() || + new_heights.cols() != heights.cols()) + HPP_FCL_THROW_PRETTY( + "The matrix containing the new heights values does not have the same " + "matrix size as the original one.\n" + "\tinput values - rows: " + << new_heights.rows() << " - cols: " << new_heights.cols() << "\n" + << "\texpected values - rows: " << heights.rows() + << " - cols: " << heights.cols() << "\n", + std::invalid_argument); + heights = new_heights.cwiseMax(min_height); this->max_height = recursiveUpdateHeight(0); assert(this->max_height == heights.maxCoeff()); } - -protected: - - void init(const FCL_REAL x_dim, - const FCL_REAL y_dim, - const MatrixXf & heights, - const FCL_REAL min_height) - { + + protected: + void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf& heights, + const FCL_REAL min_height) { this->x_dim = x_dim; this->y_dim = y_dim; this->heights = heights.cwiseMax(min_height); this->min_height = min_height; this->max_height = heights.maxCoeff(); - - const Eigen::DenseIndex - NX = heights.cols(), - NY = heights.rows(); + + const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows(); assert(NX >= 2 && "The number of columns is too small."); assert(NY >= 2 && "The number of rows is too small."); - - x_grid = VecXf::LinSpaced(NX,-0.5*x_dim,0.5*x_dim); - y_grid = VecXf::LinSpaced(NY,0.5*y_dim,-0.5*y_dim); - + + x_grid = VecXf::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim); + y_grid = VecXf::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim); + // Allocate BVS - const size_t num_tot_bvs = (size_t)(NX * NY) - 1 + (size_t)((NX-1) * (NY-1)); + const size_t num_tot_bvs = + (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1)); bvs.resize(num_tot_bvs); num_bvs = 0; - + // Build tree buildTree(); } @@ -330,215 +300,195 @@ class HPP_FCL_DLLAPI HeightField /// @brief Get the object type: it is a HFIELD OBJECT_TYPE getObjectType() const { return OT_HFIELD; } - Vec3f computeCOM() const - { - return Vec3f::Zero(); - } - - FCL_REAL computeVolume() const - { - return 0; - } + Vec3f computeCOM() const { return Vec3f::Zero(); } - Matrix3f computeMomentofInertia() const - { - return Matrix3f::Zero(); - } + FCL_REAL computeVolume() const { return 0; } -protected: + Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); } + protected: /// @brief Dimensions in meters along X and Y directions FCL_REAL x_dim, y_dim; - + /// @brief Elevation values in meters of the Height Field MatrixXf heights; - - /// @brief Minimal height of the Height Field: all values bellow min_height will be discarded. + + /// @brief Minimal height of the Height Field: all values bellow min_height + /// will be discarded. FCL_REAL min_height, max_height; - - /// @brief Grids along the X and Y directions. Useful for plotting or other related things. + + /// @brief Grids along the X and Y directions. Useful for plotting or other + /// related things. VecXf x_grid, y_grid; - + /// @brief Bounding volume hierarchy BVS bvs; unsigned int num_bvs; /// @brief Build the bounding volume hierarchy - int buildTree() - { + int buildTree() { num_bvs = 1; - const FCL_REAL max_recursive_height = recursiveBuildTree(0, 0, heights.cols()-1, 0, heights.rows()-1); - assert(max_recursive_height == max_height && "the maximal height is not correct"); + const FCL_REAL max_recursive_height = + recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1); + assert(max_recursive_height == max_height && + "the maximal height is not correct"); HPP_FCL_UNUSED_VARIABLE(max_recursive_height); - + bvs.resize(num_bvs); return BVH_OK; } - - FCL_REAL recursiveUpdateHeight(const size_t bv_id) - { - HFNode & bv_node = bvs[bv_id]; - + + FCL_REAL recursiveUpdateHeight(const size_t bv_id) { + HFNode& bv_node = bvs[bv_id]; + FCL_REAL max_height; - if(bv_node.isLeaf()) - { - max_height = heights.block<2,2>(bv_node.y_id,bv_node.x_id).maxCoeff(); - } - else - { + if (bv_node.isLeaf()) { + max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff(); + } else { FCL_REAL max_left_height = recursiveUpdateHeight(bv_node.leftChild()), max_right_height = recursiveUpdateHeight(bv_node.rightChild()); - - max_height = (std::max)(max_left_height,max_right_height); + + max_height = (std::max)(max_left_height, max_right_height); } - - const Vec3f pointA(x_grid[bv_node.x_id],y_grid[bv_node.y_id],min_height); - const Vec3f pointB(x_grid[bv_node.x_id+bv_node.x_size],y_grid[bv_node.y_id+bv_node.y_size],max_height); - - details::UpdateBoundingVolume::run(pointA,pointB,bv_node.bv); - + + const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); + const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size], + y_grid[bv_node.y_id + bv_node.y_size], max_height); + + details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); + return max_height; } - - FCL_REAL recursiveBuildTree(const size_t bv_id, - const Eigen::DenseIndex x_id, const Eigen::DenseIndex x_size, - const Eigen::DenseIndex y_id, const Eigen::DenseIndex y_size) - { + + FCL_REAL recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id, + const Eigen::DenseIndex x_size, + const Eigen::DenseIndex y_id, + const Eigen::DenseIndex y_size) { assert(x_id < heights.cols() && "x_id is out of bounds"); assert(y_id < heights.rows() && "y_id is out of bounds"); - assert(x_size >= 0 && y_size >= 0 && "x_size or y_size are not of correct value"); + assert(x_size >= 0 && y_size >= 0 && + "x_size or y_size are not of correct value"); assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension"); - - HFNode & bvnode = bvs[bv_id]; + + HFNode& bvnode = bvs[bv_id]; FCL_REAL max_height; - if(x_size == 1 && y_size == 1) // don't build any BV for the current child node - { - max_height = heights.block<2,2>(y_id,x_id).maxCoeff(); - } - else + if (x_size == 1 && + y_size == 1) // don't build any BV for the current child node { + max_height = heights.block<2, 2>(y_id, x_id).maxCoeff(); + } else { bvnode.first_child = num_bvs; num_bvs += 2; - + FCL_REAL max_left_height = min_height, max_right_height = min_height; - if(x_size >= y_size) // splitting along the X axis + if (x_size >= y_size) // splitting along the X axis { - Eigen::DenseIndex x_size_half = x_size/2; - if(x_size == 1) x_size_half = 1; - max_left_height = recursiveBuildTree(bvnode.leftChild(), - x_id, x_size_half, - y_id, y_size); - - max_right_height = recursiveBuildTree(bvnode.rightChild(), - x_id + x_size_half, x_size - x_size_half, - y_id, y_size); - } - else // splitting along the Y axis + Eigen::DenseIndex x_size_half = x_size / 2; + if (x_size == 1) x_size_half = 1; + max_left_height = recursiveBuildTree(bvnode.leftChild(), x_id, + x_size_half, y_id, y_size); + + max_right_height = + recursiveBuildTree(bvnode.rightChild(), x_id + x_size_half, + x_size - x_size_half, y_id, y_size); + } else // splitting along the Y axis { - Eigen::DenseIndex y_size_half = y_size/2; - if(y_size == 1) y_size_half = 1; - max_left_height = recursiveBuildTree(bvnode.leftChild(), - x_id, x_size, + Eigen::DenseIndex y_size_half = y_size / 2; + if (y_size == 1) y_size_half = 1; + max_left_height = recursiveBuildTree(bvnode.leftChild(), x_id, x_size, y_id, y_size_half); - - max_right_height = recursiveBuildTree(bvnode.rightChild(), - x_id, x_size, - y_id + y_size_half, y_size - y_size_half); + + max_right_height = + recursiveBuildTree(bvnode.rightChild(), x_id, x_size, + y_id + y_size_half, y_size - y_size_half); } - - max_height = (std::max)(max_left_height,max_right_height); + + max_height = (std::max)(max_left_height, max_right_height); } -// max_height = std::max(max_height,min_height); - - const Vec3f pointA(x_grid[x_id],y_grid[y_id],min_height); - assert(x_id+x_size < x_grid.size()); - assert(y_id+y_size < y_grid.size()); - const Vec3f pointB(x_grid[x_id+x_size],y_grid[y_id+y_size],max_height); - - details::UpdateBoundingVolume::run(pointA,pointB,bvnode.bv); - bvnode.x_id = x_id; bvnode.y_id = y_id; - bvnode.x_size = x_size; bvnode.y_size = y_size; + // max_height = std::max(max_height,min_height); + + const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height); + assert(x_id + x_size < x_grid.size()); + assert(y_id + y_size < y_grid.size()); + const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], + max_height); + + details::UpdateBoundingVolume::run(pointA, pointB, bvnode.bv); + bvnode.x_id = x_id; + bvnode.y_id = y_id; + bvnode.x_size = x_size; + bvnode.y_size = y_size; return max_height; } -public: + public: /// @brief Access the bv giving the its index - const HFNode & getBV(unsigned int i) const - { - if(i >= num_bvs) + const HFNode& getBV(unsigned int i) const { + if (i >= num_bvs) HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument); return bvs[i]; } /// @brief Access the bv giving the its index - HFNode & getBV(unsigned int i) - { - if(i >= num_bvs) + HFNode& getBV(unsigned int i) { + if (i >= num_bvs) HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument); return bvs[i]; } - + /// @brief Get the BV type: default is unknown NODE_TYPE getNodeType() const { return BV_UNKNOWN; } -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const HeightField * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const HeightField & other = *other_ptr; - - return - x_dim == other.x_dim - && y_dim == other.y_dim - && heights == other.heights - && min_height == other.min_height - && max_height == other.max_height - && x_grid == other.x_grid - && y_grid == other.y_grid - && bvs == other.bvs - && num_bvs == other.num_bvs; + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const HeightField* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const HeightField& other = *other_ptr; + + return x_dim == other.x_dim && y_dim == other.y_dim && + heights == other.heights && min_height == other.min_height && + max_height == other.max_height && x_grid == other.x_grid && + y_grid == other.y_grid && bvs == other.bvs && + num_bvs == other.num_bvs; } - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -/// @brief Specialization of getNodeType() for HeightField with different BV types -template<> +/// @brief Specialization of getNodeType() for HeightField with different BV +/// types +template <> NODE_TYPE HeightField::getNodeType() const; -template<> +template <> NODE_TYPE HeightField::getNodeType() const; -template<> +template <> NODE_TYPE HeightField::getNodeType() const; -template<> +template <> NODE_TYPE HeightField::getNodeType() const; -template<> +template <> NODE_TYPE HeightField::getNodeType() const; -template<> +template <> NODE_TYPE HeightField >::getNodeType() const; -template<> +template <> NODE_TYPE HeightField >::getNodeType() const; -template<> +template <> NODE_TYPE HeightField >::getNodeType() const; /// @} -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/internal/BV_fitter.h b/include/hpp/fcl/internal/BV_fitter.h index a0e886dfd..514741f0b 100644 --- a/include/hpp/fcl/internal/BV_fitter.h +++ b/include/hpp/fcl/internal/BV_fitter.h @@ -44,56 +44,53 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Compute a bounding volume that fits a set of n points. -template -void fit(Vec3f* ps, unsigned int n, BV& bv) -{ - for(unsigned int i = 0; i < n; ++i) // TODO(jcarpent): vectorize +template +void fit(Vec3f* ps, unsigned int n, BV& bv) { + for (unsigned int i = 0; i < n; ++i) // TODO(jcarpent): vectorize { bv += ps[i]; } } -template<> +template <> void fit(Vec3f* ps, unsigned int n, OBB& bv); -template<> +template <> void fit(Vec3f* ps, unsigned int n, RSS& bv); -template<> +template <> void fit(Vec3f* ps, unsigned int n, kIOS& bv); -template<> +template <> void fit(Vec3f* ps, unsigned int n, OBBRSS& bv); -template<> +template <> void fit(Vec3f* ps, unsigned int n, AABB& bv); -/// @brief The class for the default algorithm fitting a bounding volume to a set of points -template -class HPP_FCL_DLLAPI BVFitterTpl -{ -public: +/// @brief The class for the default algorithm fitting a bounding volume to a +/// set of points +template +class HPP_FCL_DLLAPI BVFitterTpl { + public: /// @brief default deconstructor virtual ~BVFitterTpl() {} /// @brief Prepare the geometry primitive data for fitting - void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) - { + void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = NULL; tri_indices = tri_indices_; type = type_; } - /// @brief Prepare the geometry primitive data for fitting, for deformable mesh - void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) - { + /// @brief Prepare the geometry primitive data for fitting, for deformable + /// mesh + void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, + BVHModelType type_) { vertices = vertices_; prev_vertices = prev_vertices_; tri_indices = tri_indices_; @@ -101,61 +98,58 @@ class HPP_FCL_DLLAPI BVFitterTpl } /// @brief Compute the fitting BV - virtual BV fit(unsigned int* primitive_indices, unsigned int num_primitives) = 0; + virtual BV fit(unsigned int* primitive_indices, + unsigned int num_primitives) = 0; /// @brief Clear the geometry primitive data - void clear() - { + void clear() { vertices = NULL; prev_vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } -protected: - + protected: Vec3f* vertices; Vec3f* prev_vertices; Triangle* tri_indices; BVHModelType type; }; -/// @brief The class for the default algorithm fitting a bounding volume to a set of points -template -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl -{ +/// @brief The class for the default algorithm fitting a bounding volume to a +/// set of points +template +class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { typedef BVFitterTpl Base; -public: - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data - BV fit(unsigned int* primitive_indices, unsigned int num_primitives) - { + + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data + BV fit(unsigned int* primitive_indices, unsigned int num_primitives) { BV bv; - if(type == BVH_MODEL_TRIANGLES) /// The primitive is triangle + if (type == BVH_MODEL_TRIANGLES) /// The primitive is triangle { - for(unsigned int i = 0; i < num_primitives; ++i) - { + for (unsigned int i = 0; i < num_primitives; ++i) { Triangle t = tri_indices[primitive_indices[i]]; bv += vertices[t[0]]; bv += vertices[t[1]]; bv += vertices[t[2]]; - if(prev_vertices) /// can fitting both current and previous frame + if (prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[t[0]]; bv += prev_vertices[t[1]]; bv += prev_vertices[t[2]]; } } - } - else if(type == BVH_MODEL_POINTCLOUD) /// The primitive is point + } else if (type == BVH_MODEL_POINTCLOUD) /// The primitive is point { - for(unsigned int i = 0; i < num_primitives; ++i) - { + for (unsigned int i = 0; i < num_primitives; ++i) { bv += vertices[primitive_indices[i]]; - if(prev_vertices) /// can fitting both current and previous frame + if (prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[primitive_indices[i]]; } @@ -165,65 +159,65 @@ class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl return bv; } -protected: - using Base::vertices; + protected: using Base::prev_vertices; using Base::tri_indices; using Base::type; + using Base::vertices; }; /// @brief Specification of BVFitter for OBB bounding volume -template<> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl -{ -public: - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. +template <> +class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. OBB fit(unsigned int* primitive_indices, unsigned int num_primitives); }; /// @brief Specification of BVFitter for RSS bounding volume -template<> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl -{ -public: - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. +template <> +class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. RSS fit(unsigned int* primitive_indices, unsigned int num_primitives); }; /// @brief Specification of BVFitter for kIOS bounding volume -template<> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl -{ -public: - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. +template <> +class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. kIOS fit(unsigned int* primitive_indices, unsigned int num_primitives); }; /// @brief Specification of BVFitter for OBBRSS bounding volume -template<> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl -{ -public: - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. +template <> +class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. OBBRSS fit(unsigned int* primitive_indices, unsigned int num_primitives); }; /// @brief Specification of BVFitter for AABB bounding volume -template<> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl -{ -public: - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. +template <> +class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { + public: + /// @brief Compute a bounding volume that fits a set of primitives (points or + /// triangles). The primitive data was set by set function and + /// primitive_indices is the primitive index relative to the data. AABB fit(unsigned int* primitive_indices, unsigned int num_primitives); }; -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/internal/BV_splitter.h b/include/hpp/fcl/internal/BV_splitter.h index cdfcf5ae4..24c5c6077 100644 --- a/include/hpp/fcl/internal/BV_splitter.h +++ b/include/hpp/fcl/internal/BV_splitter.h @@ -44,77 +44,73 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Three types of split algorithms are provided in FCL as default -enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER}; - +enum SplitMethodType { + SPLIT_METHOD_MEAN, + SPLIT_METHOD_MEDIAN, + SPLIT_METHOD_BV_CENTER +}; /// @brief A class describing the split rule that splits each BV node -template -class HPP_FCL_DLLAPI BVSplitter -{ -public: - - BVSplitter(SplitMethodType method) : split_vector(0,0,0), split_method(method) - { - } +template +class BVSplitter { + public: + BVSplitter(SplitMethodType method) + : split_vector(0, 0, 0), split_method(method) {} /// @brief Default deconstructor virtual ~BVSplitter() {} /// @brief Set the geometry data needed by the split rule - void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) - { + void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; tri_indices = tri_indices_; type = type_; } - /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node - void computeRule(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives) - { - switch(split_method) - { - case SPLIT_METHOD_MEAN: - computeRule_mean(bv, primitive_indices, num_primitives); - break; - case SPLIT_METHOD_MEDIAN: - computeRule_median(bv, primitive_indices, num_primitives); - break; - case SPLIT_METHOD_BV_CENTER: - computeRule_bvcenter(bv, primitive_indices, num_primitives); - break; - default: - std::cerr << "Split method not supported" << std::endl; + /// @brief Compute the split rule according to a subset of geometry and the + /// corresponding BV node + void computeRule(const BV& bv, unsigned int* primitive_indices, + unsigned int num_primitives) { + switch (split_method) { + case SPLIT_METHOD_MEAN: + computeRule_mean(bv, primitive_indices, num_primitives); + break; + case SPLIT_METHOD_MEDIAN: + computeRule_median(bv, primitive_indices, num_primitives); + break; + case SPLIT_METHOD_BV_CENTER: + computeRule_bvcenter(bv, primitive_indices, num_primitives); + break; + default: + std::cerr << "Split method not supported" << std::endl; } } /// @brief Apply the split rule on a given point - bool apply(const Vec3f& q) const - { - return q[split_axis] > split_value; - } + bool apply(const Vec3f& q) const { return q[split_axis] > split_value; } /// @brief Clear the geometry data set before - void clear() - { + void clear() { vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } -protected: - - /// @brief The axis based on which the split decision is made. For most BV, the axis is aligned with one of the world coordinate, so only split_axis is needed. - /// For oriented node, we can use a vector to make a better split decision. + protected: + /// @brief The axis based on which the split decision is made. For most BV, + /// the axis is aligned with one of the world coordinate, so only split_axis + /// is needed. For oriented node, we can use a vector to make a better split + /// decision. int split_axis; Vec3f split_vector; - /// @brief The split threshold, different primitives are splitted according whether their projection on the split_axis is larger or smaller than the threshold + /// @brief The split threshold, different primitives are splitted according + /// whether their projection on the split_axis is larger or smaller than the + /// threshold FCL_REAL split_value; /// @brief The mesh vertices or points handled by the splitter @@ -130,47 +126,43 @@ class HPP_FCL_DLLAPI BVSplitter SplitMethodType split_method; /// @brief Split algorithm 1: Split the node from center - void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) - { + void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) { Vec3f center = bv.center(); int axis = 2; - if(bv.width() >= bv.height() && bv.width() >= bv.depth()) + if (bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; - else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) + else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; split_value = center[axis]; } - /// @brief Split algorithm 2: Split the node according to the mean of the data contained - void computeRule_mean(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives) - { + /// @brief Split algorithm 2: Split the node according to the mean of the data + /// contained + void computeRule_mean(const BV& bv, unsigned int* primitive_indices, + unsigned int num_primitives) { int axis = 2; - if(bv.width() >= bv.height() && bv.width() >= bv.depth()) + if (bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; - else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) + else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; FCL_REAL sum = 0; - if(type == BVH_MODEL_TRIANGLES) - { - for(unsigned int i = 0; i < num_primitives; ++i) - { + if (type == BVH_MODEL_TRIANGLES) { + for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle& t = tri_indices[primitive_indices[i]]; - sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]); + sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + + vertices[t[2]][split_axis]); } sum /= 3; - } - else if(type == BVH_MODEL_POINTCLOUD) - { - for(unsigned int i = 0; i < num_primitives; ++i) - { + } else if (type == BVH_MODEL_POINTCLOUD) { + for (unsigned int i = 0; i < num_primitives; ++i) { sum += vertices[primitive_indices[i]][split_axis]; } } @@ -178,97 +170,117 @@ class HPP_FCL_DLLAPI BVSplitter split_value = sum / num_primitives; } - /// @brief Split algorithm 3: Split the node according to the median of the data contained - void computeRule_median(const BV& bv, unsigned int* primitive_indices, unsigned int num_primitives) - { + /// @brief Split algorithm 3: Split the node according to the median of the + /// data contained + void computeRule_median(const BV& bv, unsigned int* primitive_indices, + unsigned int num_primitives) { int axis = 2; - if(bv.width() >= bv.height() && bv.width() >= bv.depth()) + if (bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; - else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) + else if (bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; std::vector proj((size_t)num_primitives); - if(type == BVH_MODEL_TRIANGLES) - { - for(unsigned int i = 0; i < num_primitives; ++i) - { + if (type == BVH_MODEL_TRIANGLES) { + for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle& t = tri_indices[primitive_indices[i]]; - proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3; + proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + + vertices[t[2]][split_axis]) / + 3; } - } - else if(type == BVH_MODEL_POINTCLOUD) - { - for(unsigned int i = 0; i < num_primitives; ++i) + } else if (type == BVH_MODEL_POINTCLOUD) { + for (unsigned int i = 0; i < num_primitives; ++i) proj[i] = vertices[primitive_indices[i]][split_axis]; } std::sort(proj.begin(), proj.end()); - if(num_primitives % 2 == 1) - { + if (num_primitives % 2 == 1) { split_value = proj[(num_primitives - 1) / 2]; - } - else - { - split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; + } else { + split_value = + (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; } } }; - -template<> -bool BVSplitter::apply(const Vec3f& q) const; - -template<> -bool BVSplitter::apply(const Vec3f& q) const; - -template<> -bool BVSplitter::apply(const Vec3f& q) const; - -template<> -bool BVSplitter::apply(const Vec3f& q) const; - -template<> -void BVSplitter::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_median(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_median(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -template<> -void BVSplitter::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); - -} - -} // namespace hpp +template <> +bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; + +template <> +bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; + +template <> +bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; + +template <> +bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( + const OBB& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( + const OBB& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_median( + const OBB& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( + const RSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( + const RSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_median( + const RSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( + const kIOS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( + const kIOS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_median( + const kIOS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( + const OBBRSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( + const OBBRSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +template <> +void HPP_FCL_DLLAPI BVSplitter::computeRule_median( + const OBBRSS& bv, unsigned int* primitive_indices, + unsigned int num_primitives); + +} // namespace fcl + +} // namespace hpp #endif diff --git a/include/hpp/fcl/internal/intersect.h b/include/hpp/fcl/internal/intersect.h index 7c7b0f194..1166bc74f 100644 --- a/include/hpp/fcl/internal/intersect.h +++ b/include/hpp/fcl/internal/intersect.h @@ -42,26 +42,22 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief CCD intersect kernel among primitives -class HPP_FCL_DLLAPI Intersect -{ -public: - static bool buildTrianglePlane - (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t); -}; // class Intersect +class HPP_FCL_DLLAPI Intersect { + public: + static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, + const Vec3f& v3, Vec3f* n, FCL_REAL* t); +}; // class Intersect /// @brief Project functions -class HPP_FCL_DLLAPI Project -{ -public: - struct HPP_FCL_DLLAPI ProjectResult - { - /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) +class HPP_FCL_DLLAPI Project { + public: + struct HPP_FCL_DLLAPI ProjectResult { + /// @brief Parameterization of the projected point (based on the simplex to + /// be projected, use 2 or 3 or 4 of the array) FCL_REAL parameterization[4]; /// @brief square distance from the query point to the projected simplex @@ -70,42 +66,44 @@ class HPP_FCL_DLLAPI Project /// @brief the code of the projection type unsigned int encode; - ProjectResult() : sqr_distance(-1), encode(0) - { - } + ProjectResult() : sqr_distance(-1), encode(0) {} }; /// @brief Project point p onto line a-b - static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, const Vec3f& p); + static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, + const Vec3f& p); /// @brief Project point p onto triangle a-b-c - static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& p); + static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, + const Vec3f& c, const Vec3f& p); /// @brief Project point p onto tetrahedra a-b-c-d - static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, const Vec3f& p); + static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, + const Vec3f& c, const Vec3f& d, + const Vec3f& p); /// @brief Project origin (0) onto line a-b static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b); /// @brief Project origin (0) onto triangle a-b-c - static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c); + static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, + const Vec3f& c); /// @brief Project origin (0) onto tetrahedran a-b-c-d - static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d); + static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, + const Vec3f& c, const Vec3f& d); }; /// @brief Triangle distance functions -class HPP_FCL_DLLAPI TriangleDistance -{ -public: - +class HPP_FCL_DLLAPI TriangleDistance { + public: /// @brief Returns closest points between an segment pair. /// The first segment is P + t * A /// The second segment is Q + t * B /// X, Y are the closest points on the two segments /// VEC is the vector between X and Y - static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B, - Vec3f& VEC, Vec3f& X, Vec3f& Y); + static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, + const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y); /// Compute squared distance between triangles /// @param S and T are two triangles @@ -116,13 +114,13 @@ class HPP_FCL_DLLAPI TriangleDistance /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3], - Vec3f& P, Vec3f& Q); + static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, + Vec3f& Q); - static FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - Vec3f& P, Vec3f& Q); + static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, Vec3f& P, + Vec3f& Q); /// Compute squared distance between triangles /// @param S and T are two triangles @@ -134,9 +132,9 @@ class HPP_FCL_DLLAPI TriangleDistance /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q); + static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Matrix3f& R, const Vec3f& Tl, Vec3f& P, + Vec3f& Q); /// Compute squared distance between triangles /// @param S and T are two triangles @@ -148,10 +146,8 @@ class HPP_FCL_DLLAPI TriangleDistance /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, - Vec3f& P, Vec3f& Q); - + static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Transform3f& tf, Vec3f& P, Vec3f& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices @@ -163,11 +159,11 @@ class HPP_FCL_DLLAPI TriangleDistance /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q); + static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Matrix3f& R, const Vec3f& Tl, Vec3f& P, + Vec3f& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices @@ -179,17 +175,15 @@ class HPP_FCL_DLLAPI TriangleDistance /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, - Vec3f& P, Vec3f& Q); - + static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Transform3f& tf, Vec3f& P, Vec3f& Q); }; -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index b97432b21..0e2d8051f 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -42,72 +42,74 @@ #include #include -namespace hpp -{ -namespace fcl -{ - template - HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result); - - template - HPP_FCL_DLLAPI std::size_t ShapeShapeCollide - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result); - -#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1,T2) \ -template<> \ -HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance \ - (const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const DistanceRequest& request, \ - DistanceResult& result); \ -template<> \ -HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance \ -(const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const DistanceRequest& request, \ - DistanceResult& result) - - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box,Halfspace); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box,Plane); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box,Sphere); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule,Capsule); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule,Halfspace); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule,Plane); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone,Halfspace); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone,Plane); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder,Halfspace); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder,Plane); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere,Halfspace); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere,Plane); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere,Sphere); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere,Cylinder); - - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace); - SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace); +namespace hpp { +namespace fcl { +template +HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + +template +HPP_FCL_DLLAPI std::size_t ShapeShapeCollide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ + template <> \ + HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance( \ + const CollisionGeometry* o1, const Transform3f& tf1, \ + const CollisionGeometry* o2, const Transform3f& tf2, \ + const GJKSolver* nsolver, const DistanceRequest& request, \ + DistanceResult& result); \ + template <> \ + HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance( \ + const CollisionGeometry* o1, const Transform3f& tf1, \ + const CollisionGeometry* o2, const Transform3f& tf2, \ + const GJKSolver* nsolver, const DistanceRequest& request, \ + DistanceResult& result) + +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Capsule); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Sphere); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder); + +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace); +SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace); #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION -#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1,T2) \ -template<> \ -HPP_FCL_DLLAPI std::size_t ShapeShapeCollide \ - (const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const CollisionRequest& request, \ - CollisionResult& result) +#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2) \ + template <> \ + HPP_FCL_DLLAPI std::size_t ShapeShapeCollide( \ + const CollisionGeometry* o1, const Transform3f& tf1, \ + const CollisionGeometry* o2, const Transform3f& tf2, \ + const GJKSolver* nsolver, const CollisionRequest& request, \ + CollisionResult& result) - SHAPE_SHAPE_COLLIDE_SPECIALIZATION(Sphere,Sphere); +SHAPE_SHAPE_COLLIDE_SPECIALIZATION(Sphere, Sphere); #undef SHAPE_SHAPE_COLLIDE_SPECIALIZATION -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/tools.h b/include/hpp/fcl/internal/tools.h index 0b4db2311..928db9dfe 100644 --- a/include/hpp/fcl/internal/tools.h +++ b/include/hpp/fcl/internal/tools.h @@ -49,35 +49,28 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -template +template static inline typename Derived::Scalar triple( - const Eigen::MatrixBase& x, - const Eigen::MatrixBase& y, - const Eigen::MatrixBase& z) -{ + const Eigen::MatrixBase& x, const Eigen::MatrixBase& y, + const Eigen::MatrixBase& z) { return x.derived().dot(y.derived().cross(z.derived())); } -template -void generateCoordinateSystem( - const Eigen::MatrixBase& _w, - const Eigen::MatrixBase& _u, - const Eigen::MatrixBase& _v) -{ +template +void generateCoordinateSystem(const Eigen::MatrixBase& _w, + const Eigen::MatrixBase& _u, + const Eigen::MatrixBase& _v) { typedef typename Derived1::Scalar T; - Eigen::MatrixBase& w = const_cast < Eigen::MatrixBase& > (_w); - Eigen::MatrixBase& u = const_cast < Eigen::MatrixBase& > (_u); - Eigen::MatrixBase& v = const_cast < Eigen::MatrixBase& > (_v); + Eigen::MatrixBase& w = const_cast&>(_w); + Eigen::MatrixBase& u = const_cast&>(_u); + Eigen::MatrixBase& v = const_cast&>(_v); T inv_length; - if(std::abs(w[0]) >= std::abs(w[1])) - { + if (std::abs(w[0]) >= std::abs(w[1])) { inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]); u[0] = -w[2] * inv_length; u[1] = (T)0; @@ -85,9 +78,7 @@ void generateCoordinateSystem( v[0] = w[1] * u[2]; v[1] = w[2] * u[0] - w[0] * u[2]; v[2] = -w[1] * u[0]; - } - else - { + } else { inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]); u[0] = (T)0; u[1] = w[2] * inv_length; @@ -99,19 +90,22 @@ void generateCoordinateSystem( } /* ----- Start Matrices ------ */ -template -void relativeTransform(const Eigen::MatrixBase& R1, const Eigen::MatrixBase& t1, - const Eigen::MatrixBase& R2, const Eigen::MatrixBase& t2, - const Eigen::MatrixBase& R , const Eigen::MatrixBase& t) -{ - const_cast< Eigen::MatrixBase& >(R) = R1.transpose() * R2; - const_cast< Eigen::MatrixBase& >(t) = R1.transpose() * (t2 - t1); +template +void relativeTransform(const Eigen::MatrixBase& R1, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& R2, + const Eigen::MatrixBase& t2, + const Eigen::MatrixBase& R, + const Eigen::MatrixBase& t) { + const_cast&>(R) = R1.transpose() * R2; + const_cast&>(t) = R1.transpose() * (t2 - t1); } -/// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors -template -void eigen(const Eigen::MatrixBase& m, typename Derived::Scalar dout[3], Vector* vout) -{ +/// @brief compute the eigen vector and eigen vector of a matrix. dout is the +/// eigen values, vout is the eigen vectors +template +void eigen(const Eigen::MatrixBase& m, + typename Derived::Scalar dout[3], Vector* vout) { typedef typename Derived::Scalar Scalar; Derived R(m.derived()); int n = 3; @@ -123,50 +117,46 @@ void eigen(const Eigen::MatrixBase& m, typename Derived::Scalar dout[3] Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; Scalar d[3]; - for(ip = 0; ip < n; ++ip) - { + for (ip = 0; ip < n; ++ip) { b[ip] = d[ip] = R(ip, ip); z[ip] = 0; } nrot = 0; - for(i = 0; i < 50; ++i) - { + for (i = 0; i < 50; ++i) { sm = 0; - for(ip = 0; ip < n; ++ip) - for(iq = ip + 1; iq < n; ++iq) - sm += std::abs(R(ip, iq)); - if(sm == 0.0) - { + for (ip = 0; ip < n; ++ip) + for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq)); + if (sm == 0.0) { vout[0] << v[0][0], v[0][1], v[0][2]; vout[1] << v[1][0], v[1][1], v[1][2]; vout[2] << v[2][0], v[2][1], v[2][2]; - dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2]; + dout[0] = d[0]; + dout[1] = d[1]; + dout[2] = d[2]; return; } - if(i < 3) tresh = 0.2 * sm / (n * n); - else tresh = 0.0; + if (i < 3) + tresh = 0.2 * sm / (n * n); + else + tresh = 0.0; - for(ip = 0; ip < n; ++ip) - { - for(iq= ip + 1; iq < n; ++iq) - { + for (ip = 0; ip < n; ++ip) { + for (iq = ip + 1; iq < n; ++iq) { g = 100.0 * std::abs(R(ip, iq)); - if(i > 3 && - std::abs(d[ip]) + g == std::abs(d[ip]) && - std::abs(d[iq]) + g == std::abs(d[iq])) + if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) && + std::abs(d[iq]) + g == std::abs(d[iq])) R(ip, iq) = 0.0; - else if(std::abs(R(ip, iq)) > tresh) - { + else if (std::abs(R(ip, iq)) > tresh) { h = d[iq] - d[ip]; - if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h; - else - { + if (std::abs(h) + g == std::abs(h)) + t = (R(ip, iq)) / h; + else { theta = 0.5 * h / (R(ip, iq)); - t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta)); - if(theta < 0.0) t = -t; + t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta)); + if (theta < 0.0) t = -t; } c = 1.0 / std::sqrt(1 + t * t); s = t * c; @@ -177,16 +167,35 @@ void eigen(const Eigen::MatrixBase& m, typename Derived::Scalar dout[3] d[ip] -= h; d[iq] += h; R(ip, iq) = 0.0; - for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); } - for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); } - for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); } - for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); } + for (j = 0; j < ip; ++j) { + g = R(j, ip); + h = R(j, iq); + R(j, ip) = g - s * (h + g * tau); + R(j, iq) = h + s * (g - h * tau); + } + for (j = ip + 1; j < iq; ++j) { + g = R(ip, j); + h = R(j, iq); + R(ip, j) = g - s * (h + g * tau); + R(j, iq) = h + s * (g - h * tau); + } + for (j = iq + 1; j < n; ++j) { + g = R(ip, j); + h = R(iq, j); + R(ip, j) = g - s * (h + g * tau); + R(iq, j) = h + s * (g - h * tau); + } + for (j = 0; j < n; ++j) { + g = v[j][ip]; + h = v[j][iq]; + v[j][ip] = g - s * (h + g * tau); + v[j][iq] = h + s * (g - h * tau); + } nrot++; } } } - for(ip = 0; ip < n; ++ip) - { + for (ip = 0; ip < n; ++ip) { b[ip] += z[ip]; d[ip] = b[ip]; z[ip] = 0.0; @@ -198,14 +207,15 @@ void eigen(const Eigen::MatrixBase& m, typename Derived::Scalar dout[3] return; } -template -bool isEqual(const Eigen::MatrixBase& lhs, const Eigen::MatrixBase& rhs, const FCL_REAL tol = std::numeric_limits::epsilon()*100) -{ +template +bool isEqual(const Eigen::MatrixBase& lhs, + const Eigen::MatrixBase& rhs, + const FCL_REAL tol = std::numeric_limits::epsilon() * + 100) { return ((lhs - rhs).array().abs() < tol).all(); } -} -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif - diff --git a/include/hpp/fcl/internal/traversal.h b/include/hpp/fcl/internal/traversal.h index 39582e66a..dbf032bb6 100644 --- a/include/hpp/fcl/internal/traversal.h +++ b/include/hpp/fcl/internal/traversal.h @@ -39,40 +39,37 @@ /// @cond INTERNAL -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -enum { - RelativeTransformationIsIdentity = 1 -}; +enum { RelativeTransformationIsIdentity = 1 }; -namespace details -{ - template - struct HPP_FCL_DLLAPI RelativeTransformation - { - RelativeTransformation () : R (Matrix3f::Identity()) {} +namespace details { +template +struct HPP_FCL_DLLAPI RelativeTransformation { + RelativeTransformation() : R(Matrix3f::Identity()) {} - const Matrix3f& _R () const { return R; } - const Vec3f & _T () const { return T; } + const Matrix3f& _R() const { return R; } + const Vec3f& _T() const { return T; } - Matrix3f R; - Vec3f T; - }; + Matrix3f R; + Vec3f T; +}; - template <> - struct HPP_FCL_DLLAPI RelativeTransformation - { - static const Matrix3f& _R () { throw std::logic_error ("should never reach this point"); } - static const Vec3f & _T () { throw std::logic_error ("should never reach this point"); } - }; -} // namespace details +template <> +struct HPP_FCL_DLLAPI RelativeTransformation { + static const Matrix3f& _R() { + throw std::logic_error("should never reach this point"); + } + static const Vec3f& _T() { + throw std::logic_error("should never reach this point"); + } +}; +} // namespace details -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h index 2c554467e..173fb58a1 100644 --- a/include/hpp/fcl/internal/traversal_node_base.h +++ b/include/hpp/fcl/internal/traversal_node_base.h @@ -44,32 +44,31 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Node structure encoding the information required for traversal. -class TraversalNodeBase -{ -public: - TraversalNodeBase () : enable_statistics(false) {} +class TraversalNodeBase { + public: + TraversalNodeBase() : enable_statistics(false) {} virtual ~TraversalNodeBase() {} virtual void preprocess() {} - + virtual void postprocess() {} - /// @brief Whether b is a leaf node in the first BVH tree + /// @brief Whether b is a leaf node in the first BVH tree virtual bool isFirstNodeLeaf(unsigned int /*b*/) const { return true; } /// @brief Whether b is a leaf node in the second BVH tree virtual bool isSecondNodeLeaf(unsigned int /*b*/) const { return true; } /// @brief Traverse the subtree of the node in the first tree first - virtual bool firstOverSecond(unsigned int /*b1*/, unsigned int /*b2*/) const { return true; } + virtual bool firstOverSecond(unsigned int /*b1*/, unsigned int /*b2*/) const { + return true; + } /// @brief Get the left child of the node b in the first tree virtual int getFirstLeftChild(unsigned int b) const { return (int)b; } @@ -100,12 +99,12 @@ class TraversalNodeBase /// regroup class about traversal for distance. /// @{ -/// @brief Node structure encoding the information required for collision traversal. -class CollisionTraversalNodeBase : public TraversalNodeBase -{ -public: - CollisionTraversalNodeBase (const CollisionRequest& request_) : - request (request_), result(NULL) {} +/// @brief Node structure encoding the information required for collision +/// traversal. +class CollisionTraversalNodeBase : public TraversalNodeBase { + public: + CollisionTraversalNodeBase(const CollisionRequest& request_) + : request(request_), result(NULL) {} virtual ~CollisionTraversalNodeBase() {} @@ -116,10 +115,12 @@ class CollisionTraversalNodeBase : public TraversalNodeBase /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. - virtual bool BVDisjoints(unsigned int b1, unsigned int b2, FCL_REAL& sqrDistLowerBound) const = 0; + virtual bool BVDisjoints(unsigned int b1, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const = 0; /// @brief Leaf test between node b1 and b2, if they are both leafs - virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/, FCL_REAL& /*sqrDistLowerBound*/) const = 0; + virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/, + FCL_REAL& /*sqrDistLowerBound*/) const = 0; /// @brief Check whether the traversal can stop bool canStop() const { return this->request.isSatisfied(*(this->result)); } @@ -129,7 +130,6 @@ class CollisionTraversalNodeBase : public TraversalNodeBase /// @brief collision result kept during the traversal iteration CollisionResult* result; - }; /// @} @@ -138,10 +138,10 @@ class CollisionTraversalNodeBase : public TraversalNodeBase /// regroup class about traversal for distance. /// @{ -/// @brief Node structure encoding the information required for distance traversal. -class DistanceTraversalNodeBase : public TraversalNodeBase -{ -public: +/// @brief Node structure encoding the information required for distance +/// traversal. +class DistanceTraversalNodeBase : public TraversalNodeBase { + public: DistanceTraversalNodeBase() : result(NULL) {} virtual ~DistanceTraversalNodeBase() {} @@ -149,8 +149,8 @@ class DistanceTraversalNodeBase : public TraversalNodeBase /// @brief BV test between b1 and b2 /// @return a lower bound of the distance between the two BV. /// @note except for OBB, this method returns the distance. - virtual FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int /*b2*/) const - { + virtual FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, + unsigned int /*b2*/) const { return (std::numeric_limits::max)(); } @@ -158,8 +158,7 @@ class DistanceTraversalNodeBase : public TraversalNodeBase virtual void leafComputeDistance(unsigned int b1, unsigned int b2) const = 0; /// @brief Check whether the traversal can stop - virtual bool canStop(FCL_REAL /*c*/) const - { return false; } + virtual bool canStop(FCL_REAL /*c*/) const { return false; } /// @brief request setting for distance DistanceRequest request; @@ -170,11 +169,10 @@ class DistanceTraversalNodeBase : public TraversalNodeBase ///@} -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond #endif - diff --git a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h b/include/hpp/fcl/internal/traversal_node_bvh_hfield.h index 8bd110ae6..27a0caed5 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h +++ b/include/hpp/fcl/internal/traversal_node_bvh_hfield.h @@ -55,117 +55,106 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @addtogroup Traversal_For_Collision /// @{ -/// @brief Traversal node for collision between one BVH model and one HeightField -template +/// @brief Traversal node for collision between one BVH model and one +/// HeightField +template class MeshHeightFieldCollisionTraversalNode -: public CollisionTraversalNodeBase -{ -public: + : public CollisionTraversalNodeBase { + public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; MeshHeightFieldCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) - { + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; - + vertices1 = NULL; tri_indices1 = NULL; } - + /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const - { + bool isFirstNodeLeaf(unsigned int b) const { assert(model1 != NULL && "model1 is NULL"); return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const - { + bool isSecondNodeLeaf(unsigned int b) const { assert(model2 != NULL && "model2 is NULL"); return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better - bool firstOverSecond(unsigned int b1, unsigned int b2) const - { + bool firstOverSecond(unsigned int b1, unsigned int b2) const { FCL_REAL sz1 = model1->getBV(b1).bv.size(); FCL_REAL sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); - if(l2 || (!l1 && (sz1 > sz2))) - return true; + if (l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const - { + int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const - { + int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const - { + int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const - { + int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int b1, unsigned int b2) const { + if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).overlap(this->model2->getBV(b2)); else - return !overlap(RT._R(), RT._T(), - this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); + return !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, + this->model2->getBV(b2).bv); } - + /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. - bool BVDisjoints(unsigned int b1, unsigned int b2, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int b1, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).overlap(this->model2->getBV(b2), - this->request, sqrDistLowerBound); + this->request, sqrDistLowerBound); else { - bool res = !overlap(RT._R(), RT._T(), - this->model1->getBV(b1).bv, this->model2->getBV(b2).bv, - this->request, sqrDistLowerBound); - assert (!res || sqrDistLowerBound > 0); + bool res = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, + this->model2->getBV(b2).bv, this->request, + sqrDistLowerBound); + assert(!res || sqrDistLowerBound > 0); return res; } } @@ -184,52 +173,53 @@ class MeshHeightFieldCollisionTraversalNode /// @note If the distance between objects is less than the security margin, /// and the object are not colliding, the penetration depth is /// negative. - void leafCollides(unsigned int b1, unsigned int b2, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_leaf_tests++; + void leafCollides(unsigned int b1, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_leaf_tests++; - const BVNode & node1 = this->model1->getBV(b1); - const HeightFieldNode & node2 = this->model2->getBV(b2); + const BVNode& node1 = this->model1->getBV(b1); + const HeightFieldNode& node2 = this->model2->getBV(b2); int primitive_id1 = node1.primitiveId(); - const Triangle & tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Vec3f& P1 = vertices1[tri_id1[0]]; const Vec3f& P2 = vertices1[tri_id1[1]]; const Vec3f& P3 = vertices1[tri_id1[2]]; - TriangleP tri1 (P1, P2, P3); - + TriangleP tri1(P1, P2, P3); + typedef Convex ConvexTriangle; ConvexTriangle convex1, convex2; - details::buildConvexTriangles(node2,*this->model2,convex2,convex2); - + details::buildConvexTriangles(node2, *this->model2, convex2, convex2); + GJKSolver solver; - Vec3f p1, p2; // closest points if no collision contact points if collision. + Vec3f p1, + p2; // closest points if no collision contact points if collision. Vec3f normal; FCL_REAL distance; - solver.shapeDistance (tri1, this->tf1, tri2, this->tf2, - distance, p1, p2, normal); + solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2, + normal); FCL_REAL distToCollision = distance - this->request.security_margin; sqrDistLowerBound = distance * distance; - if (distToCollision <= 0) { // collision - Vec3f p (p1); // contact point - FCL_REAL penetrationDepth (0); - if(this->result->numContacts() < this->request.num_max_contacts) { + if (distToCollision <= 0) { // collision + Vec3f p(p1); // contact point + FCL_REAL penetrationDepth(0); + if (this->result->numContacts() < this->request.num_max_contacts) { // How much (Q1, Q2, Q3) should be moved so that all vertices are // above (P1, P2, P3). penetrationDepth = -distance; if (distance > 0) { - normal = (p2-p1).normalized (); - p = .5* (p1+p2); + normal = (p2 - p1).normalized(); + p = .5 * (p1 + p2); } this->result->addContact(Contact(this->model1, this->model2, - primitive_id1, primitive_id2, - p, normal, penetrationDepth)); + primitive_id1, primitive_id2, p, + normal, penetrationDepth)); } } } - + /// @brief The first BVH model const BVHModel* model1; /// @brief The second HeightField model @@ -240,97 +230,95 @@ class MeshHeightFieldCollisionTraversalNode mutable int num_leaf_tests; mutable FCL_REAL query_time_seconds; - Vec3f* vertices1 - Triangle* tri_indices1; + Vec3f* vertices1 Triangle* tri_indices1; details::RelativeTransformation RT; }; -/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) -typedef MeshHeightFieldCollisionTraversalNode MeshHeightFieldCollisionTraversalNodeOBB ; -typedef MeshHeightFieldCollisionTraversalNode MeshHeightFieldCollisionTraversalNodeRSS ; -typedef MeshHeightFieldCollisionTraversalNode MeshHeightFieldCollisionTraversalNodekIOS ; -typedef MeshHeightFieldCollisionTraversalNode MeshHeightFieldCollisionTraversalNodeOBBRSS; +/// @brief Traversal node for collision between two meshes if their underlying +/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) +typedef MeshHeightFieldCollisionTraversalNode + MeshHeightFieldCollisionTraversalNodeOBB; +typedef MeshHeightFieldCollisionTraversalNode + MeshHeightFieldCollisionTraversalNodeRSS; +typedef MeshHeightFieldCollisionTraversalNode + MeshHeightFieldCollisionTraversalNodekIOS; +typedef MeshHeightFieldCollisionTraversalNode + MeshHeightFieldCollisionTraversalNodeOBBRSS; /// @} -namespace details -{ - template struct DistanceTraversalBVDistanceLowerBound_impl - { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) - { - return b1.distance(b2); - } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, const BVNode& b2) - { - return distance(R, T, b1.bv, b2.bv); - } - }; +namespace details { +template +struct DistanceTraversalBVDistanceLowerBound_impl { + static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + return b1.distance(b2); + } + static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + const BVNode& b2) { + return distance(R, T, b1.bv, b2.bv); + } +}; - template<> struct DistanceTraversalBVDistanceLowerBound_impl - { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) - { - FCL_REAL sqrDistLowerBound; - CollisionRequest request (DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (b1.overlap(b2, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt (sqrDistLowerBound); +template <> +struct DistanceTraversalBVDistanceLowerBound_impl { + static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + FCL_REAL sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (b1.overlap(b2, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, const BVNode& b2) - { - FCL_REAL sqrDistLowerBound; - CollisionRequest request (DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt (sqrDistLowerBound); + return sqrt(sqrDistLowerBound); + } + static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + const BVNode& b2) { + FCL_REAL sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; } - }; + return sqrt(sqrDistLowerBound); + } +}; - template<> struct DistanceTraversalBVDistanceLowerBound_impl - { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) - { - FCL_REAL sqrDistLowerBound; - CollisionRequest request (DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (b1.overlap(b2, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt (sqrDistLowerBound); +template <> +struct DistanceTraversalBVDistanceLowerBound_impl { + static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + FCL_REAL sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (b1.overlap(b2, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, const BVNode& b2) - { - FCL_REAL sqrDistLowerBound; - CollisionRequest request (DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt (sqrDistLowerBound); + return sqrt(sqrDistLowerBound); + } + static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + const BVNode& b2) { + FCL_REAL sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; } - }; -} // namespace details + return sqrt(sqrDistLowerBound); + } +}; +} // namespace details /// @addtogroup Traversal_For_Distance /// @{ /// @brief Traversal node for distance computation between BVH models -template -class BVHDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - BVHDistanceTraversalNode() : DistanceTraversalNodeBase() - { +template +class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { + public: + BVHDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; @@ -340,52 +328,44 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase } /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const - { + bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const - { + bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better - bool firstOverSecond(unsigned int b1, unsigned int b2) const - { + bool firstOverSecond(unsigned int b1, unsigned int b2) const { FCL_REAL sz1 = model1->getBV(b1).bv.size(); FCL_REAL sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); - if(l2 || (!l1 && (sz1 > sz2))) - return true; + if (l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const - { + int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const - { + int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const - { + int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const - { + int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } @@ -400,12 +380,10 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase mutable FCL_REAL query_time_seconds; }; - /// @brief Traversal node for distance computation between two meshes -template -class MeshDistanceTraversalNode : public BVHDistanceTraversalNode -{ -public: +template +class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { + public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity @@ -420,8 +398,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode using BVHDistanceTraversalNode::num_bv_tests; using BVHDistanceTraversalNode::num_leaf_tests; - MeshDistanceTraversalNode() : BVHDistanceTraversalNode() - { + MeshDistanceTraversalNode() : BVHDistanceTraversalNode() { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; @@ -431,32 +408,28 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode abs_err = this->request.abs_err; } - void preprocess() - { - if(!RTIsIdentity) preprocessOrientedNode(); + void preprocess() { + if (!RTIsIdentity) preprocessOrientedNode(); } - void postprocess() - { - if(!RTIsIdentity) postprocessOrientedNode(); + void postprocess() { + if (!RTIsIdentity) postprocessOrientedNode(); } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const - { - if(enable_statistics) num_bv_tests++; + FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { + if (enable_statistics) num_bv_tests++; if (RTIsIdentity) - return details::DistanceTraversalBVDistanceLowerBound_impl - ::run (model1->getBV(b1), model2->getBV(b2)); + return details::DistanceTraversalBVDistanceLowerBound_impl::run( + model1->getBV(b1), model2->getBV(b2)); else - return details::DistanceTraversalBVDistanceLowerBound_impl - ::run (RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); + return details::DistanceTraversalBVDistanceLowerBound_impl::run( + RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); } /// @brief Distance testing between leaves (two triangles) - void leafComputeDistance(unsigned int b1, unsigned int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); @@ -480,12 +453,11 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode FCL_REAL d2; if (RTIsIdentity) - d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23, - P1, P2); + d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, + P2); else - d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23, - RT._R(), RT._T(), - P1, P2); + d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, + RT._R(), RT._T(), P1, P2); FCL_REAL d = sqrt(d2); this->result->update(d, this->model1, this->model2, primitive_id1, @@ -493,9 +465,9 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + bool canStop(FCL_REAL c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } @@ -512,9 +484,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode details::RelativeTransformation RT; -private: - void preprocessOrientedNode() - { + private: + void preprocessOrientedNode() { const int init_tri_id1 = 0, init_tri_id2 = 0; const Triangle& init_tri1 = tri_indices1[init_tri_id1]; const Triangle& init_tri2 = tri_indices2[init_tri_id2]; @@ -531,54 +502,53 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode init_tri2_points[2] = vertices2[init_tri2[2]]; Vec3f p1, p2, normal; - FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance - (init_tri1_points[0], init_tri1_points[1], - init_tri1_points[2], init_tri2_points[0], - init_tri2_points[1], init_tri2_points[2], - RT._R(), RT._T(), p1, p2)); + FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance( + init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], + init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), + RT._T(), p1, p2)); result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2, - normal); + normal); } - void postprocessOrientedNode() - { - /// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space. - if(request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2)) - { + void postprocessOrientedNode() { + /// the points obtained by triDistance are not in world space: both are in + /// object1's local coordinate system, so we need to convert them into the + /// world space. + if (request.enable_nearest_points && (result->o1 == model1) && + (result->o2 == model2)) { result->nearest_points[0] = tf1.transform(result->nearest_points[0]); result->nearest_points[1] = tf1.transform(result->nearest_points[1]); } } }; -/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS) -typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeRSS; -typedef MeshDistanceTraversalNode MeshDistanceTraversalNodekIOS; +/// @brief Traversal node for distance computation between two meshes if their +/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS) +typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeRSS; +typedef MeshDistanceTraversalNode MeshDistanceTraversalNodekIOS; typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; /// @} -/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to be transformed -namespace details -{ +/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to +/// be transformed +namespace details { -template -inline const Matrix3f& getBVAxes(const BV& bv) -{ +template +inline const Matrix3f& getBVAxes(const BV& bv) { return bv.axes; } -template<> -inline const Matrix3f& getBVAxes(const OBBRSS& bv) -{ +template <> +inline const Matrix3f& getBVAxes(const OBBRSS& bv) { return bv.obb.axes; } -} +} // namespace details -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h index bac8064ce..8b2521ca5 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h +++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H #define HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H @@ -49,23 +48,18 @@ #include #include - -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @addtogroup Traversal_For_Collision /// @{ /// @brief Traversal node for collision between BVH and shape -template -class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - BVHShapeCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase (request) - { +template +class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase { + public: + BVHShapeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; @@ -75,20 +69,17 @@ class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase } /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const - { + bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const - { + int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const - { + int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } @@ -102,13 +93,11 @@ class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase }; /// @brief Traversal node for collision between shape and BVH -template -class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - ShapeBVHCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase(request) - { +template +class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase { + public: + ShapeBVHCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; @@ -118,26 +107,20 @@ class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase } /// @brief Alway extend the second model, which is a BVH model - bool firstOverSecond(unsigned int, unsigned int) const - { - return false; - } + bool firstOverSecond(unsigned int, unsigned int) const { return false; } /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const - { + bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const - { + int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const - { + int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } @@ -150,21 +133,19 @@ class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase mutable FCL_REAL query_time_seconds; }; - /// @brief Traversal node for collision between mesh and shape -template -class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode -{ -public: +template +class MeshShapeCollisionTraversalNode + : public BVHShapeCollisionTraversalNode { + public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; - MeshShapeCollisionTraversalNode(const CollisionRequest& request) : - BVHShapeCollisionTraversalNode (request) - { + MeshShapeCollisionTraversalNode(const CollisionRequest& request) + : BVHShapeCollisionTraversalNode(request) { vertices = NULL; tri_indices = NULL; @@ -172,13 +153,13 @@ class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNodeenable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).bv.overlap(this->model2_bv); else - return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); + return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv); } /// test between BV b1 and shape @@ -186,24 +167,25 @@ class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNodeenable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; bool res; if (RTIsIdentity) - res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound); + res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, + sqrDistLowerBound); else res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv, - this->request, sqrDistLowerBound); - assert (!res || sqrDistLowerBound > 0); + this->model2_bv, this->model1->getBV(b1).bv, this->request, + sqrDistLowerBound); + assert(!res || sqrDistLowerBound > 0); return res; } /// @brief Intersection testing between leaves (one triangle and one shape) - void leafCollides(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_leaf_tests++; + void leafCollides(unsigned int b1, unsigned int /*b2*/, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model1->getBV(b1); int primitive_id = node.primitiveId(); @@ -216,41 +198,37 @@ class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNodeshapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3, - Id , distance, c2, c1, normal); + collision = nsolver->shapeTriangleInteraction( + *(this->model2), this->tf2, p1, p2, p3, Id, distance, c2, c1, normal); } else { - collision = - nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3, - this->tf1, distance, c2, c1, normal); + collision = nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, + p1, p2, p3, this->tf1, + distance, c2, c1, normal); } - if(collision) { - if(this->request.num_max_contacts > this->result->numContacts()) - { + if (collision) { + if (this->request.num_max_contacts > this->result->numContacts()) { this->result->addContact(Contact(this->model1, this->model2, - primitive_id, Contact::NONE, - c1, -normal, -distance)); - assert (this->result->isCollision ()); + primitive_id, Contact::NONE, c1, + -normal, -distance)); + assert(this->result->isCollision()); return; } } sqrDistLowerBound = distance * distance; - assert (distance > 0); - if ( this->request.security_margin > 0 - && distance <= this->request.security_margin) - { - this->result->addContact(Contact(this->model1, this->model2, - primitive_id, Contact::NONE, - .5 * (c1+c2), (c2-c1).normalized (), - -distance)); + assert(distance > 0); + if (this->request.security_margin > 0 && + distance <= this->request.security_margin) { + this->result->addContact(Contact(this->model1, this->model2, primitive_id, + Contact::NONE, .5 * (c1 + c2), + (c2 - c1).normalized(), -distance)); } - assert (!this->result->isCollision () || sqrDistLowerBound > 0); + assert(!this->result->isCollision() || sqrDistLowerBound > 0); } Vec3f* vertices; @@ -260,18 +238,17 @@ class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode -class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode -{ -public: +template +class ShapeMeshCollisionTraversalNode + : public ShapeBVHCollisionTraversalNode { + public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; - ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode() - { + ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode() { vertices = NULL; tri_indices = NULL; @@ -280,37 +257,38 @@ class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNodeenable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const { + if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model2->getBV(b2).bv.overlap(this->model1_bv); else - return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); + return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), + this->model1_bv, this->model2->getBV(b2).bv); } /// BV test between b1 and b2 /// @param b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; bool res; if (RTIsIdentity) - res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, sqrDistLowerBound); + res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, + sqrDistLowerBound); else res = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv, sqrDistLowerBound); - assert (!res || sqrDistLowerBound > 0); + assert(!res || sqrDistLowerBound > 0); return res; } /// @brief Intersection testing between leaves (one shape and one triangle) - void leafCollides(unsigned int /*b1*/, unsigned int b2, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_leaf_tests++; + void leafCollides(unsigned int /*b1*/, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model2->getBV(b2); int primitive_id = node.primitiveId(); @@ -323,41 +301,37 @@ class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNodeshapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3, - Id , c1, c2, distance, normal); + collision = nsolver->shapeTriangleInteraction( + *(this->model1), this->tf1, p1, p2, p3, Id, c1, c2, distance, normal); } else { - collision = - nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3, - this->tf2, c1, c2, distance, normal); + collision = nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, + p1, p2, p3, this->tf2, c1, + c2, distance, normal); } if (collision) { - if(this->request.num_max_contacts > this->result->numContacts()) - { - this->result->addContact (Contact(this->model1 , this->model2, - Contact::NONE, primitive_id, - c1, normal, -distance)); - assert (this->result->isCollision ()); + if (this->request.num_max_contacts > this->result->numContacts()) { + this->result->addContact(Contact(this->model1, this->model2, + Contact::NONE, primitive_id, c1, + normal, -distance)); + assert(this->result->isCollision()); return; } } sqrDistLowerBound = distance * distance; - assert (distance > 0); - if ( this->request.security_margin == 0 - && distance <= this->request.security_margin) - { - this->result.addContact (Contact(this->model1 , this->model2, - Contact::NONE, primitive_id, - .5 * (c1+c2), (c2-c1).normalized (), - -distance)); + assert(distance > 0); + if (this->request.security_margin == 0 && + distance <= this->request.security_margin) { + this->result.addContact(Contact(this->model1, this->model2, Contact::NONE, + primitive_id, .5 * (c1 + c2), + (c2 - c1).normalized(), -distance)); } - assert (!this->result->isCollision () || sqrDistLowerBound > 0); + assert(!this->result->isCollision() || sqrDistLowerBound > 0); } Vec3f* vertices; @@ -372,41 +346,35 @@ class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode -class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() - { +template +class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { + public: + BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; - + num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const - { + bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const - { + int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const - { + int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const - { + FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { return model1->getBV(b1).bv.distance(model2_bv); } @@ -420,61 +388,53 @@ class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase }; /// @brief Traversal node for distance computation between shape and BVH -template -class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() - { +template +class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { + public: + ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; - + num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const - { + bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const - { + int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const - { + int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const - { + FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { return model1_bv.distance(model2->getBV(b2).bv); } const S* model1; const BVHModel* model2; BV model1_bv; - + mutable int num_bv_tests; mutable int num_leaf_tests; mutable FCL_REAL query_time_seconds; }; - /// @brief Traversal node for distance between mesh and shape -template -class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode -{ -public: - MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode() - { +template +class MeshShapeDistanceTraversalNode + : public BVHShapeDistanceTraversalNode { + public: + MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode() { vertices = NULL; tri_indices = NULL; @@ -485,35 +445,33 @@ class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNodeenable_statistics) this->num_leaf_tests++; - + void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_leaf_tests++; + const BVNode& node = this->model1->getBV(b1); - + int primitive_id = node.primitiveId(); - + const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; - + FCL_REAL d; Vec3f closest_p1, closest_p2, normal; nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3, - Transform3f (), d, closest_p2, closest_p1, + Transform3f(), d, closest_p2, closest_p1, normal); this->result->update(d, this->model1, this->model2, primitive_id, - DistanceResult::NONE, closest_p1, closest_p2, - normal); + DistanceResult::NONE, closest_p1, closest_p2, normal); } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + bool canStop(FCL_REAL c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } @@ -523,28 +481,22 @@ class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode -void meshShapeDistanceOrientedNodeleafComputeDistance(unsigned int b1, unsigned int /* b2 */, - const BVHModel* model1, const S& model2, - Vec3f* vertices, Triangle* tri_indices, - const Transform3f& tf1, - const Transform3f& tf2, - const GJKSolver* nsolver, - bool enable_statistics, - int & num_leaf_tests, - const DistanceRequest& /* request */, - DistanceResult& result) -{ - if(enable_statistics) num_leaf_tests++; - +namespace details { + +template +void meshShapeDistanceOrientedNodeleafComputeDistance( + unsigned int b1, unsigned int /* b2 */, const BVHModel* model1, + const S& model2, Vec3f* vertices, Triangle* tri_indices, + const Transform3f& tf1, const Transform3f& tf2, const GJKSolver* nsolver, + bool enable_statistics, int& num_leaf_tests, + const DistanceRequest& /* request */, DistanceResult& result) { + if (enable_statistics) num_leaf_tests++; + const BVNode& node = model1->getBV(b1); int primitive_id = node.primitiveId(); @@ -552,7 +504,7 @@ void meshShapeDistanceOrientedNodeleafComputeDistance(unsigned int b1, unsigned const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; - + FCL_REAL distance; Vec3f closest_p1, closest_p2, normal; nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance, @@ -562,21 +514,18 @@ void meshShapeDistanceOrientedNodeleafComputeDistance(unsigned int b1, unsigned closest_p1, closest_p2, normal); } - -template -static inline void distancePreprocessOrientedNode(const BVHModel* model1, - Vec3f* vertices, Triangle* tri_indices, int init_tri_id, - const S& model2, const Transform3f& tf1, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& /* request */, - DistanceResult& result) -{ +template +static inline void distancePreprocessOrientedNode( + const BVHModel* model1, Vec3f* vertices, Triangle* tri_indices, + int init_tri_id, const S& model2, const Transform3f& tf1, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& /* request */, DistanceResult& result) { const Triangle& init_tri = tri_indices[init_tri_id]; - + const Vec3f& p1 = vertices[init_tri[0]]; const Vec3f& p2 = vertices[init_tri[1]]; const Vec3f& p3 = vertices[init_tri[2]]; - + FCL_REAL distance; Vec3f closest_p1, closest_p2, normal; nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance, @@ -586,118 +535,108 @@ static inline void distancePreprocessOrientedNode(const BVHModel* model1, closest_p1, closest_p2, normal); } - -} +} // namespace details /// @endcond +/// @brief Traversal node for distance between mesh and shape, when mesh BVH is +/// one of the oriented node (RSS, kIOS, OBBRSS) +template +class MeshShapeDistanceTraversalNodeRSS + : public MeshShapeDistanceTraversalNode { + public: + MeshShapeDistanceTraversalNodeRSS() + : MeshShapeDistanceTraversalNode() {} - -/// @brief Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, kIOS, OBBRSS) -template -class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode -{ -public: - MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode() - { - } - - void preprocess() - { - details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, - *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); + void preprocess() { + details::distancePreprocessOrientedNode( + this->model1, this->vertices, this->tri_indices, 0, *(this->model2), + this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } - void postprocess() - { - } + void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); + FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv); } - void leafComputeDistance(unsigned int b1, unsigned int b2) const - { - details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b1, b2, this->model1, *(this->model2), this->vertices, + this->tri_indices, this->tf1, this->tf2, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); } }; +template +class MeshShapeDistanceTraversalNodekIOS + : public MeshShapeDistanceTraversalNode { + public: + MeshShapeDistanceTraversalNodekIOS() + : MeshShapeDistanceTraversalNode() {} -template -class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode -{ -public: - MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode() - { - } - - void preprocess() - { - details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, - *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); + void preprocess() { + details::distancePreprocessOrientedNode( + this->model1, this->vertices, this->tri_indices, 0, *(this->model2), + this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } - void postprocess() - { - } + void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); + FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv); } - void leafComputeDistance(unsigned int b1, unsigned int b2) const - { - details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b1, b2, this->model1, *(this->model2), this->vertices, + this->tri_indices, this->tf1, this->tf2, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); } - }; -template -class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode -{ -public: - MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode() - { - } +template +class MeshShapeDistanceTraversalNodeOBBRSS + : public MeshShapeDistanceTraversalNode { + public: + MeshShapeDistanceTraversalNodeOBBRSS() + : MeshShapeDistanceTraversalNode() {} - void preprocess() - { - details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, - *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); + void preprocess() { + details::distancePreprocessOrientedNode( + this->model1, this->vertices, this->tri_indices, 0, *(this->model2), + this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } - void postprocess() - { - - } + void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); + FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv); } - void leafComputeDistance(unsigned int b1, unsigned int b2) const - { - details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b1, b2, this->model1, *(this->model2), this->vertices, + this->tri_indices, this->tf1, this->tf2, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); } - }; /// @brief Traversal node for distance between shape and mesh -template -class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode -{ -public: - ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode() - { +template +class ShapeMeshDistanceTraversalNode + : public ShapeBVHDistanceTraversalNode { + public: + ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode() { vertices = NULL; tri_indices = NULL; @@ -708,24 +647,23 @@ class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNodeenable_statistics) this->num_leaf_tests++; - + void leafComputeDistance(unsigned int /*b1*/, unsigned int b2) const { + if (this->enable_statistics) this->num_leaf_tests++; + const BVNode& node = this->model2->getBV(b2); - + int primitive_id = node.primitiveId(); - + const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; - + FCL_REAL distance; Vec3f closest_p1, closest_p2, normal; nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3, - Transform3f (), distance, closest_p1, + Transform3f(), distance, closest_p1, closest_p2, normal); this->result->update(distance, this->model1, this->model2, @@ -734,9 +672,9 @@ class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + bool canStop(FCL_REAL c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } @@ -746,112 +684,107 @@ class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode -class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode -{ -public: - ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode() - { - } +/// @brief Traversal node for distance between shape and mesh, when mesh BVH is +/// one of the oriented node (RSS, kIOS, OBBRSS) +template +class ShapeMeshDistanceTraversalNodeRSS + : public ShapeMeshDistanceTraversalNode { + public: + ShapeMeshDistanceTraversalNodeRSS() + : ShapeMeshDistanceTraversalNode() {} - void preprocess() - { - details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, - *(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result)); + void preprocess() { + details::distancePreprocessOrientedNode( + this->model2, this->vertices, this->tri_indices, 0, *(this->model1), + this->tf2, this->tf1, this->nsolver, this->request, *(this->result)); } - void postprocess() - { - } + void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); + FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf2.getRotation(), this->tf2.getTranslation(), + this->model1_bv, this->model2->getBV(b2).bv); } - void leafComputeDistance(unsigned int b1, unsigned int b2) const - { - details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b2, b1, this->model2, *(this->model1), this->vertices, + this->tri_indices, this->tf2, this->tf1, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); } - }; -template -class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode -{ -public: - ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode() - { - } +template +class ShapeMeshDistanceTraversalNodekIOS + : public ShapeMeshDistanceTraversalNode { + public: + ShapeMeshDistanceTraversalNodekIOS() + : ShapeMeshDistanceTraversalNode() {} - void preprocess() - { - details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, - *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result)); + void preprocess() { + details::distancePreprocessOrientedNode( + this->model2, this->vertices, this->tri_indices, 0, *(this->model1), + this->tf2, this->tf1, this->nsolver, *(this->result)); } - void postprocess() - { - } + void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); + FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf2.getRotation(), this->tf2.getTranslation(), + this->model1_bv, this->model2->getBV(b2).bv); } - void leafComputeDistance(unsigned int b1, unsigned int b2) const - { - details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b2, b1, this->model2, *(this->model1), this->vertices, + this->tri_indices, this->tf2, this->tf1, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); } - }; -template -class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode -{ -public: - ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode() - { - } +template +class ShapeMeshDistanceTraversalNodeOBBRSS + : public ShapeMeshDistanceTraversalNode { + public: + ShapeMeshDistanceTraversalNodeOBBRSS() + : ShapeMeshDistanceTraversalNode() {} - void preprocess() - { - details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, - *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result)); + void preprocess() { + details::distancePreprocessOrientedNode( + this->model2, this->vertices, this->tri_indices, 0, *(this->model1), + this->tf2, this->tf1, this->nsolver, *(this->result)); } - void postprocess() - { - } + void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); + FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { + if (this->enable_statistics) this->num_bv_tests++; + return distance(this->tf2.getRotation(), this->tf2.getTranslation(), + this->model1_bv, this->model2->getBV(b2).bv); } - void leafComputeDistance(unsigned int b1, unsigned int b2) const - { - details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + details::meshShapeDistanceOrientedNodeleafComputeDistance( + b2, b1, this->model2, *(this->model1), this->vertices, + this->tri_indices, this->tf2, this->tf1, this->nsolver, + this->enable_statistics, this->num_leaf_tests, this->request, + *(this->result)); } - }; /// @} -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h index c44ba298e..9e9cad789 100644 --- a/include/hpp/fcl/internal/traversal_node_bvhs.h +++ b/include/hpp/fcl/internal/traversal_node_bvhs.h @@ -54,22 +54,18 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @addtogroup Traversal_For_Collision /// @{ /// @brief Traversal node for collision between BVH models -template -class BVHCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - BVHCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase (request) - { +template +class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { + public: + BVHCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; @@ -79,57 +75,49 @@ class BVHCollisionTraversalNode : public CollisionTraversalNodeBase } /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const - { + bool isFirstNodeLeaf(unsigned int b) const { assert(model1 != NULL && "model1 is NULL"); return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const - { + bool isSecondNodeLeaf(unsigned int b) const { assert(model2 != NULL && "model2 is NULL"); return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better - bool firstOverSecond(unsigned int b1, unsigned int b2) const - { + bool firstOverSecond(unsigned int b1, unsigned int b2) const { FCL_REAL sz1 = model1->getBV(b1).bv.size(); FCL_REAL sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); - if(l2 || (!l1 && (sz1 > sz2))) - return true; + if (l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const - { + int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const - { + int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const - { + int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const - { + int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } - + /// @brief The first BVH model const BVHModel* model1; /// @brief The second BVH model @@ -142,18 +130,16 @@ class BVHCollisionTraversalNode : public CollisionTraversalNodeBase }; /// @brief Traversal node for collision between two meshes -template -class MeshCollisionTraversalNode : public BVHCollisionTraversalNode -{ -public: +template +class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { + public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; - MeshCollisionTraversalNode(const CollisionRequest& request) : - BVHCollisionTraversalNode (request) - { + MeshCollisionTraversalNode(const CollisionRequest& request) + : BVHCollisionTraversalNode(request) { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; @@ -161,31 +147,30 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode } /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int b1, unsigned int b2) const { + if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).overlap(this->model2->getBV(b2)); else - return !overlap(RT._R(), RT._T(), - this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); + return !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, + this->model2->getBV(b2).bv); } - + /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. - bool BVDisjoints(unsigned int b1, unsigned int b2, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int b1, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).overlap(this->model2->getBV(b2), - this->request, sqrDistLowerBound); + this->request, sqrDistLowerBound); else { - bool res = !overlap(RT._R(), RT._T(), - this->model1->getBV(b1).bv, this->model2->getBV(b2).bv, - this->request, sqrDistLowerBound); - assert (!res || sqrDistLowerBound > 0); + bool res = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, + this->model2->getBV(b2).bv, this->request, + sqrDistLowerBound); + assert(!res || sqrDistLowerBound > 0); return res; } } @@ -204,9 +189,9 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode /// @note If the distance between objects is less than the security margin, /// and the object are not colliding, the penetration depth is /// negative. - void leafCollides(unsigned int b1, unsigned int b2, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_leaf_tests++; + void leafCollides(unsigned int b1, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); @@ -224,30 +209,31 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode const Vec3f& Q2 = vertices2[tri_id2[1]]; const Vec3f& Q3 = vertices2[tri_id2[2]]; - TriangleP tri1 (P1, P2, P3); - TriangleP tri2 (Q1, Q2, Q3); + TriangleP tri1(P1, P2, P3); + TriangleP tri2(Q1, Q2, Q3); GJKSolver solver; - Vec3f p1, p2; // closest points if no collision contact points if collision. + Vec3f p1, + p2; // closest points if no collision contact points if collision. Vec3f normal; FCL_REAL distance; - solver.shapeDistance (tri1, this->tf1, tri2, this->tf2, - distance, p1, p2, normal); + solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2, + normal); FCL_REAL distToCollision = distance - this->request.security_margin; sqrDistLowerBound = distance * distance; - if (distToCollision <= 0) { // collision - Vec3f p (p1); // contact point - FCL_REAL penetrationDepth (0); - if(this->result->numContacts() < this->request.num_max_contacts) { + if (distToCollision <= 0) { // collision + Vec3f p(p1); // contact point + FCL_REAL penetrationDepth(0); + if (this->result->numContacts() < this->request.num_max_contacts) { // How much (Q1, Q2, Q3) should be moved so that all vertices are // above (P1, P2, P3). penetrationDepth = -distance; if (distance > 0) { - normal = (p2-p1).normalized (); - p = .5* (p1+p2); + normal = (p2 - p1).normalized(); + p = .5 * (p1 + p2); } this->result->addContact(Contact(this->model1, this->model2, - primitive_id1, primitive_id2, - p, normal, penetrationDepth)); + primitive_id1, primitive_id2, p, + normal, penetrationDepth)); } } } @@ -261,91 +247,86 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode details::RelativeTransformation RT; }; -/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) -typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBB ; -typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeRSS ; -typedef MeshCollisionTraversalNode MeshCollisionTraversalNodekIOS ; +/// @brief Traversal node for collision between two meshes if their underlying +/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) +typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBB; +typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeRSS; +typedef MeshCollisionTraversalNode MeshCollisionTraversalNodekIOS; typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBBRSS; /// @} -namespace details -{ - template struct DistanceTraversalBVDistanceLowerBound_impl - { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) - { - return b1.distance(b2); - } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, const BVNode& b2) - { - return distance(R, T, b1.bv, b2.bv); - } - }; +namespace details { +template +struct DistanceTraversalBVDistanceLowerBound_impl { + static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + return b1.distance(b2); + } + static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + const BVNode& b2) { + return distance(R, T, b1.bv, b2.bv); + } +}; - template<> struct DistanceTraversalBVDistanceLowerBound_impl - { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) - { - FCL_REAL sqrDistLowerBound; - CollisionRequest request (DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (b1.overlap(b2, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt (sqrDistLowerBound); +template <> +struct DistanceTraversalBVDistanceLowerBound_impl { + static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + FCL_REAL sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (b1.overlap(b2, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, const BVNode& b2) - { - FCL_REAL sqrDistLowerBound; - CollisionRequest request (DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt (sqrDistLowerBound); + return sqrt(sqrDistLowerBound); + } + static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + const BVNode& b2) { + FCL_REAL sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; } - }; + return sqrt(sqrDistLowerBound); + } +}; - template<> struct DistanceTraversalBVDistanceLowerBound_impl - { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) - { - FCL_REAL sqrDistLowerBound; - CollisionRequest request (DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (b1.overlap(b2, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt (sqrDistLowerBound); +template <> +struct DistanceTraversalBVDistanceLowerBound_impl { + static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + FCL_REAL sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (b1.overlap(b2, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, const BVNode& b2) - { - FCL_REAL sqrDistLowerBound; - CollisionRequest request (DISTANCE_LOWER_BOUND, 0); - // request.break_distance = ? - if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { - // TODO A penetration upper bound should be computed. - return -1; - } - return sqrt (sqrDistLowerBound); + return sqrt(sqrDistLowerBound); + } + static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + const BVNode& b2) { + FCL_REAL sqrDistLowerBound; + CollisionRequest request(DISTANCE_LOWER_BOUND, 0); + // request.break_distance = ? + if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { + // TODO A penetration upper bound should be computed. + return -1; } - }; -} // namespace details + return sqrt(sqrDistLowerBound); + } +}; +} // namespace details /// @addtogroup Traversal_For_Distance /// @{ /// @brief Traversal node for distance computation between BVH models -template -class BVHDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - BVHDistanceTraversalNode() : DistanceTraversalNodeBase() - { +template +class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { + public: + BVHDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; @@ -355,52 +336,44 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase } /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const - { + bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const - { + bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better - bool firstOverSecond(unsigned int b1, unsigned int b2) const - { + bool firstOverSecond(unsigned int b1, unsigned int b2) const { FCL_REAL sz1 = model1->getBV(b1).bv.size(); FCL_REAL sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); - if(l2 || (!l1 && (sz1 > sz2))) - return true; + if (l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const - { + int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const - { + int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const - { + int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const - { + int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } @@ -415,12 +388,10 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase mutable FCL_REAL query_time_seconds; }; - /// @brief Traversal node for distance computation between two meshes -template -class MeshDistanceTraversalNode : public BVHDistanceTraversalNode -{ -public: +template +class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { + public: enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity @@ -435,8 +406,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode using BVHDistanceTraversalNode::num_bv_tests; using BVHDistanceTraversalNode::num_leaf_tests; - MeshDistanceTraversalNode() : BVHDistanceTraversalNode() - { + MeshDistanceTraversalNode() : BVHDistanceTraversalNode() { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; @@ -446,32 +416,28 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode abs_err = this->request.abs_err; } - void preprocess() - { - if(!RTIsIdentity) preprocessOrientedNode(); + void preprocess() { + if (!RTIsIdentity) preprocessOrientedNode(); } - void postprocess() - { - if(!RTIsIdentity) postprocessOrientedNode(); + void postprocess() { + if (!RTIsIdentity) postprocessOrientedNode(); } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const - { - if(enable_statistics) num_bv_tests++; + FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { + if (enable_statistics) num_bv_tests++; if (RTIsIdentity) - return details::DistanceTraversalBVDistanceLowerBound_impl - ::run (model1->getBV(b1), model2->getBV(b2)); + return details::DistanceTraversalBVDistanceLowerBound_impl::run( + model1->getBV(b1), model2->getBV(b2)); else - return details::DistanceTraversalBVDistanceLowerBound_impl - ::run (RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); + return details::DistanceTraversalBVDistanceLowerBound_impl::run( + RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); } /// @brief Distance testing between leaves (two triangles) - void leafComputeDistance(unsigned int b1, unsigned int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; + void leafComputeDistance(unsigned int b1, unsigned int b2) const { + if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); @@ -495,12 +461,11 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode FCL_REAL d2; if (RTIsIdentity) - d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23, - P1, P2); + d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, + P2); else - d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23, - RT._R(), RT._T(), - P1, P2); + d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, + RT._R(), RT._T(), P1, P2); FCL_REAL d = sqrt(d2); this->result->update(d, this->model1, this->model2, primitive_id1, @@ -508,9 +473,9 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + bool canStop(FCL_REAL c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } @@ -527,9 +492,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode details::RelativeTransformation RT; -private: - void preprocessOrientedNode() - { + private: + void preprocessOrientedNode() { const int init_tri_id1 = 0, init_tri_id2 = 0; const Triangle& init_tri1 = tri_indices1[init_tri_id1]; const Triangle& init_tri2 = tri_indices2[init_tri_id2]; @@ -546,54 +510,53 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode init_tri2_points[2] = vertices2[init_tri2[2]]; Vec3f p1, p2, normal; - FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance - (init_tri1_points[0], init_tri1_points[1], - init_tri1_points[2], init_tri2_points[0], - init_tri2_points[1], init_tri2_points[2], - RT._R(), RT._T(), p1, p2)); + FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance( + init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], + init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), + RT._T(), p1, p2)); result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2, - normal); + normal); } - void postprocessOrientedNode() - { - /// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space. - if(request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2)) - { + void postprocessOrientedNode() { + /// the points obtained by triDistance are not in world space: both are in + /// object1's local coordinate system, so we need to convert them into the + /// world space. + if (request.enable_nearest_points && (result->o1 == model1) && + (result->o2 == model2)) { result->nearest_points[0] = tf1.transform(result->nearest_points[0]); result->nearest_points[1] = tf1.transform(result->nearest_points[1]); } } }; -/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS) -typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeRSS; -typedef MeshDistanceTraversalNode MeshDistanceTraversalNodekIOS; +/// @brief Traversal node for distance computation between two meshes if their +/// underlying BVH node is oriented node (RSS, OBBRSS, kIOS) +typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeRSS; +typedef MeshDistanceTraversalNode MeshDistanceTraversalNodekIOS; typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; /// @} -/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to be transformed -namespace details -{ +/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to +/// be transformed +namespace details { -template -inline const Matrix3f& getBVAxes(const BV& bv) -{ +template +inline const Matrix3f& getBVAxes(const BV& bv) { return bv.axes; } -template<> -inline const Matrix3f& getBVAxes(const OBBRSS& bv) -{ +template <> +inline const Matrix3f& getBVAxes(const OBBRSS& bv) { return bv.obb.axes; } -} +} // namespace details -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 3af876ac8..7b1066397 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -34,7 +34,6 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H #define HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H @@ -49,202 +48,187 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @addtogroup Traversal_For_Collision /// @{ -namespace details -{ - template - Convex buildConvexQuadrilateral(const HFNode & node, - const HeightField & model) +namespace details { +template +Convex buildConvexQuadrilateral(const HFNode& node, + const HeightField& model) { + const MatrixXf& heights = model.getHeights(); + const VecXf& x_grid = model.getXGrid(); + const VecXf& y_grid = model.getYGrid(); + + const FCL_REAL min_height = model.getMinHeight(); + + const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], + y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const Eigen::Block cell = + heights.block<2, 2>(node.y_id, node.x_id); + + assert(cell.maxCoeff() > min_height && + "max_height is lower than min_height"); // Check whether the geometry + // is degenerated + + Vec3f* pts = new Vec3f[8]; + pts[0] = Vec3f(x0, y0, min_height); + pts[1] = Vec3f(x0, y1, min_height); + pts[2] = Vec3f(x1, y1, min_height); + pts[3] = Vec3f(x1, y0, min_height); + pts[4] = Vec3f(x0, y0, cell(0, 0)); + pts[5] = Vec3f(x0, y1, cell(1, 0)); + pts[6] = Vec3f(x1, y1, cell(1, 1)); + pts[7] = Vec3f(x1, y0, cell(0, 1)); + + Quadrilateral* polygons = new Quadrilateral[6]; + polygons[0].set(0, 3, 2, 1); // x+ side + polygons[1].set(0, 1, 5, 4); // y- side + polygons[2].set(1, 2, 6, 5); // x- side + polygons[3].set(2, 3, 7, 6); // y+ side + polygons[4].set(3, 0, 4, 7); // z- side + polygons[5].set(4, 5, 6, 7); // z+ side + + return Convex(true, + pts, // points + 8, // num points + polygons, + 6 // number of polygons + ); +} + +template +void buildConvexTriangles(const HFNode& node, const HeightField& model, + Convex& convex1, + Convex& convex2) { + const MatrixXf& heights = model.getHeights(); + const VecXf& x_grid = model.getXGrid(); + const VecXf& y_grid = model.getYGrid(); + + const FCL_REAL min_height = model.getMinHeight(); + + const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], + y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const Eigen::Block cell = + heights.block<2, 2>(node.y_id, node.x_id); + const FCL_REAL max_height = cell.maxCoeff(); + + assert(max_height > min_height && + "max_height is lower than min_height"); // Check whether the geometry + // is degenerated + HPP_FCL_UNUSED_VARIABLE(max_height); + { - const MatrixXf & heights = model.getHeights(); - const VecXf & x_grid = model.getXGrid(); - const VecXf & y_grid = model.getYGrid(); - - const FCL_REAL min_height = model.getMinHeight(); - - const FCL_REAL - x0 = x_grid[node.x_id], - x1 = x_grid[node.x_id+1], - y0 = y_grid[node.y_id], - y1 = y_grid[node.y_id+1]; - const Eigen::Block cell = heights.block<2,2>(node.y_id,node.x_id); - - assert(cell.maxCoeff() > min_height && "max_height is lower than min_height"); // Check whether the geometry is degenerated - Vec3f* pts = new Vec3f[8]; - pts[0] = Vec3f( x0, y0, min_height); - pts[1] = Vec3f( x0, y1, min_height); - pts[2] = Vec3f( x1, y1, min_height); - pts[3] = Vec3f( x1, y0, min_height); - pts[4] = Vec3f( x0, y0, cell(0,0)); - pts[5] = Vec3f( x0, y1, cell(1,0)); - pts[6] = Vec3f( x1, y1, cell(1,1)); - pts[7] = Vec3f( x1, y0, cell(0,1)); - - Quadrilateral* polygons = new Quadrilateral[6]; - polygons[0].set(0, 3, 2, 1); // x+ side - polygons[1].set(0, 1, 5, 4); // y- side - polygons[2].set(1, 2, 6, 5); // x- side - polygons[3].set(2, 3, 7, 6); // y+ side - polygons[4].set(3, 0, 4, 7); // z- side - polygons[5].set(4, 5, 6, 7); // z+ side - - return Convex (true, - pts, // points - 8, // num points - polygons, - 6 // number of polygons - ); + pts[0] = Vec3f(x0, y0, min_height); + pts[1] = Vec3f(x0, y1, min_height); + pts[2] = Vec3f(x1, y1, min_height); + pts[3] = Vec3f(x1, y0, min_height); + pts[4] = Vec3f(x0, y0, cell(0, 0)); + pts[5] = Vec3f(x0, y1, cell(1, 0)); + pts[6] = Vec3f(x1, y1, cell(1, 1)); + pts[7] = Vec3f(x1, y0, cell(0, 1)); + + Triangle* triangles = new Triangle[8]; + triangles[0].set(0, 1, 3); + triangles[1].set(4, 5, 7); + triangles[2].set(0, 1, 4); + triangles[3].set(4, 1, 5); + triangles[4].set(1, 7, 3); + triangles[5].set(1, 5, 7); + triangles[6].set(0, 3, 7); + triangles[7].set(7, 4, 0); + + convex1.set(true, + pts, // points + 8, // num points + triangles, + 8 // number of polygons + ); } - template - void buildConvexTriangles(const HFNode & node, - const HeightField & model, - Convex & convex1, Convex & convex2) { - const MatrixXf & heights = model.getHeights(); - const VecXf & x_grid = model.getXGrid(); - const VecXf & y_grid = model.getYGrid(); - - const FCL_REAL min_height = model.getMinHeight(); - - const FCL_REAL - x0 = x_grid[node.x_id], - x1 = x_grid[node.x_id+1], - y0 = y_grid[node.y_id], - y1 = y_grid[node.y_id+1]; - const Eigen::Block cell = heights.block<2,2>(node.y_id,node.x_id); - const FCL_REAL max_height = cell.maxCoeff(); - - assert(max_height > min_height && "max_height is lower than min_height"); // Check whether the geometry is degenerated - HPP_FCL_UNUSED_VARIABLE(max_height); - - { - Vec3f* pts = new Vec3f[8]; - pts[0] = Vec3f( x0, y0, min_height); - pts[1] = Vec3f( x0, y1, min_height); - pts[2] = Vec3f( x1, y1, min_height); - pts[3] = Vec3f( x1, y0, min_height); - pts[4] = Vec3f( x0, y0, cell(0,0)); - pts[5] = Vec3f( x0, y1, cell(1,0)); - pts[6] = Vec3f( x1, y1, cell(1,1)); - pts[7] = Vec3f( x1, y0, cell(0,1)); - - Triangle* triangles = new Triangle[8]; - triangles[0].set(0, 1, 3); - triangles[1].set(4, 5, 7); - triangles[2].set(0, 1, 4); - triangles[3].set(4, 1, 5); - triangles[4].set(1, 7, 3); - triangles[5].set(1, 5, 7); - triangles[6].set(0, 3, 7); - triangles[7].set(7, 4, 0); - - convex1.set(true, - pts, // points - 8, // num points - triangles, - 8 // number of polygons - ); - } - - { - Vec3f* pts = new Vec3f[8]; - pts[0] = Vec3f( x0, y0, min_height); - pts[1] = Vec3f( x0, y1, min_height); - pts[2] = Vec3f( x1, y1, min_height); - pts[3] = Vec3f( x1, y0, min_height); - pts[4] = Vec3f( x0, y0, cell(0,0)); - pts[5] = Vec3f( x0, y1, cell(1,0)); - pts[6] = Vec3f( x1, y1, cell(1,1)); - pts[7] = Vec3f( x1, y0, cell(0,1)); - - Triangle* triangles = new Triangle[8]; - triangles[0].set(3, 2, 1); - triangles[1].set(5, 6, 7); - triangles[2].set(1, 2, 5); - triangles[3].set(5, 2, 6); - triangles[4].set(1, 3, 7); - triangles[5].set(1, 7, 5); - triangles[6].set(2, 3, 7); - triangles[7].set(6, 2, 3); - - convex2.set(true, - pts, // points - 8, // num points - triangles, - 8 // number of polygons - ); - } - + Vec3f* pts = new Vec3f[8]; + pts[0] = Vec3f(x0, y0, min_height); + pts[1] = Vec3f(x0, y1, min_height); + pts[2] = Vec3f(x1, y1, min_height); + pts[3] = Vec3f(x1, y0, min_height); + pts[4] = Vec3f(x0, y0, cell(0, 0)); + pts[5] = Vec3f(x0, y1, cell(1, 0)); + pts[6] = Vec3f(x1, y1, cell(1, 1)); + pts[7] = Vec3f(x1, y0, cell(0, 1)); + + Triangle* triangles = new Triangle[8]; + triangles[0].set(3, 2, 1); + triangles[1].set(5, 6, 7); + triangles[2].set(1, 2, 5); + triangles[3].set(5, 2, 6); + triangles[4].set(1, 3, 7); + triangles[5].set(1, 7, 5); + triangles[6].set(2, 3, 7); + triangles[7].set(6, 2, 3); + + convex2.set(true, + pts, // points + 8, // num points + triangles, + 8 // number of polygons + ); } } +} // namespace details /// @brief Traversal node for collision between height field and shape -template +template class HeightFieldShapeCollisionTraversalNode -: public CollisionTraversalNodeBase -{ -public: - + : public CollisionTraversalNodeBase { + public: typedef CollisionTraversalNodeBase Base; - + enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) - { + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; - + nsolver = NULL; } - + /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const - { + bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const - { + int getFirstLeftChild(unsigned int b) const { return static_cast(model1->getBV(b).leftChild()); } /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const - { + int getFirstRightChild(unsigned int b) const { return static_cast(model1->getBV(b).rightChild()); } /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const - { + bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const { std::cout << "\t BVDisjoints - 2" << std::endl; - if(this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - { + if (this->enable_statistics) this->num_bv_tests++; + if (RTIsIdentity) { std::cout << "RTIsIdentity" << std::endl; assert(false && "must never happened"); return !this->model1->getBV(b1).bv.overlap(this->model2_bv); - } - else - { + } else { std::cout << "\t call !overlap(" << std::endl; return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); @@ -256,122 +240,114 @@ class HeightFieldShapeCollisionTraversalNode /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; bool res; - if (RTIsIdentity) - { + if (RTIsIdentity) { assert(false && "must never happened"); - res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound); - } - else - { + res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, + sqrDistLowerBound); + } else { res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv, - this->request, sqrDistLowerBound); + this->model2_bv, this->model1->getBV(b1).bv, this->request, + sqrDistLowerBound); } - assert (!res || sqrDistLowerBound > 0); + assert(!res || sqrDistLowerBound > 0); return res; } - - template - bool shapeDistance(const Convex & convex1, const Convex & convex2, const Transform3f & tf1, - const S & shape, const Transform3f & tf2, - FCL_REAL & distance, Vec3f& c1, Vec3f& c2, - Vec3f& normal) const - { + + template + bool shapeDistance(const Convex& convex1, + const Convex& convex2, const Transform3f& tf1, + const S& shape, const Transform3f& tf2, FCL_REAL& distance, + Vec3f& c1, Vec3f& c2, Vec3f& normal) const { const Transform3f Id; Vec3f contact2_1, contact2_2, normal2; FCL_REAL distance2; bool collision1, collision2; if (RTIsIdentity) - collision1 = !nsolver->shapeDistance(convex1, Id, shape, tf2, - distance, c1, c2, normal); + collision1 = !nsolver->shapeDistance(convex1, Id, shape, tf2, distance, + c1, c2, normal); else - collision1 = !nsolver->shapeDistance(convex1, tf1, shape, tf2, - distance, c1, c2, normal); + collision1 = !nsolver->shapeDistance(convex1, tf1, shape, tf2, distance, + c1, c2, normal); if (RTIsIdentity) - collision2 = !nsolver->shapeDistance(convex2, Id, shape, tf2, - distance2, c1, c2, normal); + collision2 = !nsolver->shapeDistance(convex2, Id, shape, tf2, distance2, + c1, c2, normal); else - collision2 = !nsolver->shapeDistance(convex2, tf1, shape, tf2, - distance2, contact2_1, contact2_2, normal2); - - if(collision1 && collision2) - { - if(distance > distance2) // switch values + collision2 = !nsolver->shapeDistance(convex2, tf1, shape, tf2, distance2, + contact2_1, contact2_2, normal2); + + if (collision1 && collision2) { + if (distance > distance2) // switch values { distance = distance2; - c1 = contact2_1; c2 = contact2_2; + c1 = contact2_1; + c2 = contact2_2; normal = normal2; } return true; - } - else if(collision1) - { + } else if (collision1) { return true; - } - else if(collision2) - { + } else if (collision2) { distance = distance2; - c1 = contact2_1; c2 = contact2_2; + c1 = contact2_1; + c2 = contact2_2; normal = normal2; return true; } - + return false; } /// @brief Intersection testing between leaves (one Convex and one shape) - void leafCollides(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_leaf_tests++; - const HFNode & node = this->model1->getBV(b1); - - // Split quadrilateral primitives into two convex shapes corresponding to polyhedron with triangular bases. - // This is essential to keep the convexity - -// typedef Convex ConvexQuadrilateral; -// const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model1); - + void leafCollides(unsigned int b1, unsigned int /*b2*/, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_leaf_tests++; + const HFNode& node = this->model1->getBV(b1); + + // Split quadrilateral primitives into two convex shapes corresponding to + // polyhedron with triangular bases. This is essential to keep the convexity + + // typedef Convex ConvexQuadrilateral; + // const ConvexQuadrilateral convex = + // details::buildConvexQuadrilateral(node,*this->model1); + typedef Convex ConvexTriangle; ConvexTriangle convex1, convex2; - details::buildConvexTriangles(node,*this->model1,convex1,convex2); - + details::buildConvexTriangles(node, *this->model1, convex1, convex2); + FCL_REAL distance; Vec3f c1, c2, normal; - - bool collision = this->shapeDistance(convex1, convex2, this->tf1, - *(this->model2), this->tf2, - distance, c1, c2, normal); - if(collision) { - if(this->request.num_max_contacts > this->result->numContacts()) - { - this->result->addContact(Contact(this->model1, this->model2, - (int)b1, (int)Contact::NONE, - c1, normal, distance)); - assert (this->result->isCollision()); + bool collision = + this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), + this->tf2, distance, c1, c2, normal); + + if (collision) { + if (this->request.num_max_contacts > this->result->numContacts()) { + this->result->addContact(Contact(this->model1, this->model2, (int)b1, + (int)Contact::NONE, c1, normal, + distance)); + assert(this->result->isCollision()); return; } } sqrDistLowerBound = distance * distance; -// assert (distance > 0); - if ( this->request.security_margin > 0 - && distance <= this->request.security_margin) - { - this->result->addContact(Contact(this->model1, this->model2, - (int)b1, (int)Contact::NONE, - .5 * (c1+c2), (c2-c1).normalized (), - distance)); + // assert (distance > 0); + if (this->request.security_margin > 0 && + distance <= this->request.security_margin) { + this->result->addContact(Contact(this->model1, this->model2, (int)b1, + (int)Contact::NONE, .5 * (c1 + c2), + (c2 - c1).normalized(), distance)); } - assert (!this->result->isCollision () || sqrDistLowerBound > 0); + assert(!this->result->isCollision() || sqrDistLowerBound > 0); } const GJKSolver* nsolver; - + const HeightField* model1; const S* model2; BV model2_bv; @@ -382,179 +358,167 @@ class HeightFieldShapeCollisionTraversalNode }; /// @brief Traversal node for collision between shape and mesh -template +template class ShapeHeightFieldCollisionTraversalNode -: public CollisionTraversalNodeBase -{ -public: - + : public CollisionTraversalNodeBase { + public: typedef CollisionTraversalNodeBase Base; - + enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; ShapeHeightFieldCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) - { + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; - + nsolver = NULL; } - + /// @brief Alway extend the second model, which is a BVH model - bool firstOverSecond(unsigned int, unsigned int) const - { - return false; - } + bool firstOverSecond(unsigned int, unsigned int) const { return false; } /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const - { + bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const - { + int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const - { + int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// BV test between b1 and b2 /// @param b2 Bounding volumes to test, - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const { + if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model2->getBV(b2).bv.overlap(this->model1_bv); else - return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); + return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), + this->model1_bv, this->model2->getBV(b2).bv); } /// BV test between b1 and b2 /// @param b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_bv_tests++; + bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_bv_tests++; bool res; if (RTIsIdentity) - res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, sqrDistLowerBound); + res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, + sqrDistLowerBound); else res = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv, sqrDistLowerBound); - assert (!res || sqrDistLowerBound > 0); + assert(!res || sqrDistLowerBound > 0); return res; } - - template - bool shapeDistance(const S & shape, const Transform3f & tf1, - const Convex & convex1, const Convex & convex2, const Transform3f & tf2, - FCL_REAL & distance, Vec3f& c1, Vec3f& c2, - Vec3f& normal) const - { + + template + bool shapeDistance(const S& shape, const Transform3f& tf1, + const Convex& convex1, + const Convex& convex2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& c1, Vec3f& c2, + Vec3f& normal) const { const Transform3f Id; Vec3f contact2_1, contact2_2, normal2; FCL_REAL distance2; bool collision1, collision2; if (RTIsIdentity) - collision1 = !nsolver->shapeDistance(shape, tf1, convex1, Id, - distance, c1, c2, normal); + collision1 = !nsolver->shapeDistance(shape, tf1, convex1, Id, distance, + c1, c2, normal); else - collision1 = !nsolver->shapeDistance(shape, tf1, convex1, tf2, - distance, c1, c2, normal); + collision1 = !nsolver->shapeDistance(shape, tf1, convex1, tf2, distance, + c1, c2, normal); if (RTIsIdentity) - collision2 = !nsolver->shapeDistance(shape, tf1, convex2, Id, - distance2, c1, c2, normal); + collision2 = !nsolver->shapeDistance(shape, tf1, convex2, Id, distance2, + c1, c2, normal); else - collision2 = !nsolver->shapeDistance(shape, tf1, convex2, tf2, - distance2, contact2_1, contact2_2, normal2); - - if(collision1 && collision2) - { - if(distance > distance2) // switch values + collision2 = !nsolver->shapeDistance(shape, tf1, convex2, tf2, distance2, + contact2_1, contact2_2, normal2); + + if (collision1 && collision2) { + if (distance > distance2) // switch values { distance = distance2; - c1 = contact2_1; c2 = contact2_2; + c1 = contact2_1; + c2 = contact2_2; normal = normal2; } return true; - } - else if(collision1) - { + } else if (collision1) { return true; - } - else if(collision2) - { + } else if (collision2) { distance = distance2; - c1 = contact2_1; c2 = contact2_2; + c1 = contact2_1; + c2 = contact2_2; normal = normal2; return true; } - + return false; } /// @brief Intersection testing between leaves (one shape and one triangle) - void leafCollides(unsigned int /*b1*/, unsigned int b2, FCL_REAL& sqrDistLowerBound) const - { - if(this->enable_statistics) this->num_leaf_tests++; + void leafCollides(unsigned int /*b1*/, unsigned int b2, + FCL_REAL& sqrDistLowerBound) const { + if (this->enable_statistics) this->num_leaf_tests++; const HFNode& node = this->model2->getBV(b2); - -// typedef Convex ConvexQuadrilateral; -// const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model2); + + // typedef Convex ConvexQuadrilateral; + // const ConvexQuadrilateral convex = + // details::buildConvexQuadrilateral(node,*this->model2); typedef Convex ConvexTriangle; ConvexTriangle convex1, convex2; - details::buildConvexTriangles(node,*this->model1,convex1,convex2); - + details::buildConvexTriangles(node, *this->model1, convex1, convex2); + FCL_REAL distance; Vec3f normal; - Vec3f c1, c2; // closest points + Vec3f c1, c2; // closest points - bool collision = this->shapeDistance(*(this->model1), this->tf1, - convex1, convex2, this->tf2, - distance, c1, c2, normal); + bool collision = + this->shapeDistance(*(this->model1), this->tf1, convex1, convex2, + this->tf2, distance, c1, c2, normal); if (collision) { - if(this->request.num_max_contacts > this->result->numContacts()) - { - this->result->addContact (Contact(this->model1 , this->model2, - Contact::NONE, b2, - c1, normal, distance)); - assert (this->result->isCollision ()); + if (this->request.num_max_contacts > this->result->numContacts()) { + this->result->addContact(Contact(this->model1, this->model2, + Contact::NONE, b2, c1, normal, + distance)); + assert(this->result->isCollision()); return; } } sqrDistLowerBound = distance * distance; - if ( this->request.security_margin == 0 - && distance <= this->request.security_margin) - { - this->result->addContact (Contact(this->model1 , this->model2, - Contact::NONE, b2, - .5 * (c1+c2), (c2-c1).normalized (), - distance)); + if (this->request.security_margin == 0 && + distance <= this->request.security_margin) { + this->result->addContact(Contact(this->model1, this->model2, + Contact::NONE, b2, .5 * (c1 + c2), + (c2 - c1).normalized(), distance)); } - assert (!this->result->isCollision () || sqrDistLowerBound > 0); + assert(!this->result->isCollision() || sqrDistLowerBound > 0); } const GJKSolver* nsolver; - + const S* model1; const HeightField* model2; BV model1_bv; @@ -570,92 +534,82 @@ class ShapeHeightFieldCollisionTraversalNode /// @{ /// @brief Traversal node for distance between height field and shape -template -class HeightFieldShapeDistanceTraversalNode -: public DistanceTraversalNodeBase -{ -public: - +template +class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { + public: typedef DistanceTraversalNodeBase Base; - + enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; - HeightFieldShapeDistanceTraversalNode() - : DistanceTraversalNodeBase() - { + HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_leaf_tests = 0; query_time_seconds = 0.0; - + rel_err = 0; abs_err = 0; nsolver = NULL; } - + /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(unsigned int b) const - { + bool isFirstNodeLeaf(unsigned int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(unsigned int b) const - { + int getFirstLeftChild(unsigned int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(unsigned int b) const - { + int getFirstRightChild(unsigned int b) const { return model1->getBV(b).rightChild(); } - + /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const - { + FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { return model1->getBV(b1).bv.distance(model2_bv); } /// @brief Distance testing between leaves (one triangle and one shape) - void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const - { - if(this->enable_statistics) this->num_leaf_tests++; - + void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { + if (this->enable_statistics) this->num_leaf_tests++; + const BVNode& node = this->model1->getBV(b1); - + typedef Convex ConvexQuadrilateral; - const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model1); - + const ConvexQuadrilateral convex = + details::buildConvexQuadrilateral(node, *this->model1); + FCL_REAL d; Vec3f closest_p1, closest_p2, normal; - - nsolver->shapeDistance(convex, this->tf1, - *(this->model2), this->tf2, - d, closest_p1, closest_p2, normal); + + nsolver->shapeDistance(convex, this->tf1, *(this->model2), this->tf2, d, + closest_p1, closest_p2, normal); this->result->update(d, this->model1, this->model2, b1, - DistanceResult::NONE, closest_p1, closest_p2, - normal); + DistanceResult::NONE, closest_p1, closest_p2, normal); } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + bool canStop(FCL_REAL c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } FCL_REAL rel_err; FCL_REAL abs_err; - + const GJKSolver* nsolver; - + const HeightField* model1; const S* model2; BV model2_bv; @@ -666,82 +620,71 @@ class HeightFieldShapeDistanceTraversalNode }; /// @brief Traversal node for distance computation between shape and mesh -template -class ShapeHeightFieldDistanceTraversalNode -: public DistanceTraversalNodeBase -{ -public: - +template +class ShapeHeightFieldDistanceTraversalNode : public DistanceTraversalNodeBase { + public: typedef DistanceTraversalNodeBase Base; - + enum { Options = _Options, RTIsIdentity = _Options & RelativeTransformationIsIdentity }; - ShapeHeightFieldDistanceTraversalNode() - { + ShapeHeightFieldDistanceTraversalNode() { model1 = NULL; model2 = NULL; num_leaf_tests = 0; query_time_seconds = 0.0; - + rel_err = 0; abs_err = 0; nsolver = NULL; } - + /// @brief Alway extend the second model, which is a BVH model - bool firstOverSecond(unsigned int, unsigned int) const - { - return false; - } + bool firstOverSecond(unsigned int, unsigned int) const { return false; } /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const - { + bool isSecondNodeLeaf(unsigned int b) const { return model2->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const - { + int getSecondLeftChild(unsigned int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const - { + int getSecondRightChild(unsigned int b) const { return model2->getBV(b).rightChild(); } /// @brief Distance testing between leaves (one triangle and one shape) - void leafComputeDistance(unsigned int /*b1*/, unsigned int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - + void leafComputeDistance(unsigned int /*b1*/, unsigned int b2) const { + if (this->enable_statistics) this->num_leaf_tests++; + const BVNode& node = this->model2->getBV(b2); - + typedef Convex ConvexQuadrilateral; - const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node,*this->model2); - + const ConvexQuadrilateral convex = + details::buildConvexQuadrilateral(node, *this->model2); + FCL_REAL d; Vec3f closest_p1, closest_p2, normal; - - nsolver->shapeDistance(*(this->model1), this->tf1, - convex, this->tf2, - d, closest_p1, closest_p2, normal); - - this->result->update(d, this->model1, this->model2, - DistanceResult::NONE, b2, closest_p1, closest_p2, - normal); + + nsolver->shapeDistance(*(this->model1), this->tf1, convex, this->tf2, d, + closest_p1, closest_p2, normal); + + this->result->update(d, this->model1, this->model2, DistanceResult::NONE, + b2, closest_p1, closest_p2, normal); } - + /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + bool canStop(FCL_REAL c) const { + if ((c >= this->result->min_distance - abs_err) && + (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } @@ -750,7 +693,7 @@ class ShapeHeightFieldDistanceTraversalNode FCL_REAL abs_err; const GJKSolver* nsolver; - + const S* model1; const HeightField* model2; BV model1_bv; @@ -761,7 +704,8 @@ class ShapeHeightFieldDistanceTraversalNode /// @} -}} // namespace hpp::fcl +} // namespace fcl +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h index 5ebf2b6b5..3e6a88d22 100644 --- a/include/hpp/fcl/internal/traversal_node_octree.h +++ b/include/hpp/fcl/internal/traversal_node_octree.h @@ -47,15 +47,12 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Algorithms for collision related with octree -class HPP_FCL_DLLAPI OcTreeSolver -{ -private: +class HPP_FCL_DLLAPI OcTreeSolver { + private: const GJKSolver* solver; mutable const CollisionRequest* crequest; @@ -64,111 +61,98 @@ class HPP_FCL_DLLAPI OcTreeSolver mutable CollisionResult* cresult; mutable DistanceResult* dresult; -public: - OcTreeSolver(const GJKSolver* solver_) : solver(solver_), - crequest(NULL), - drequest(NULL), - cresult(NULL), - dresult(NULL) - { - } + public: + OcTreeSolver(const GJKSolver* solver_) + : solver(solver_), + crequest(NULL), + drequest(NULL), + cresult(NULL), + dresult(NULL) {} /// @brief collision between two octrees void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, - CollisionResult& result_) const - { + CollisionResult& result_) const { crequest = &request_; cresult = &result_; - - OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, tree2->getRoot(), tree2->getRootBV(), - tf1, tf2); + + OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, + tree2->getRoot(), tree2->getRootBV(), tf1, tf2); } /// @brief distance between two octrees void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, - DistanceResult& result_) const - { + DistanceResult& result_) const { drequest = &request_; dresult = &result_; - OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, tree2->getRoot(), tree2->getRootBV(), - tf1, tf2); + OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, + tree2->getRoot(), tree2->getRootBV(), tf1, tf2); } /// @brief collision between octree and mesh - template + template void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel* tree2, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, - CollisionResult& result_) const - { + CollisionResult& result_) const { crequest = &request_; cresult = &result_; OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, 0, - tf1, tf2); + tree2, 0, tf1, tf2); } /// @brief distance between octree and mesh - template + template void OcTreeMeshDistance(const OcTree* tree1, const BVHModel* tree2, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, - DistanceResult& result_) const - { + DistanceResult& result_) const { drequest = &request_; dresult = &result_; OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, 0, - tf1, tf2); + tree2, 0, tf1, tf2); } /// @brief collision between mesh and octree - template + template void MeshOcTreeIntersect(const BVHModel* tree1, const OcTree* tree2, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, CollisionResult& result_) const - + { crequest = &request_; cresult = &result_; OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), - tree1, 0, - tf2, tf1); + tree1, 0, tf2, tf1); } /// @brief distance between mesh and octree - template + template void MeshOcTreeDistance(const BVHModel* tree1, const OcTree* tree2, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, - DistanceResult& result_) const - { + DistanceResult& result_) const { drequest = &request_; dresult = &result_; - OcTreeMeshDistanceRecurse(tree1, 0, - tree2, tree2->getRoot(), tree2->getRootBV(), - tf1, tf2); + OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(), + tree2->getRootBV(), tf1, tf2); } /// @brief collision between octree and shape - template + template void OcTreeShapeIntersect(const OcTree* tree, const S& s, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, - CollisionResult& result_) const - { + CollisionResult& result_) const { crequest = &request_; cresult = &result_; @@ -176,19 +160,16 @@ class HPP_FCL_DLLAPI OcTreeSolver computeBV(s, Transform3f(), bv2); OBB obb2; convertBV(bv2, tf2, obb2); - OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), - s, obb2, - tf1, tf2); - + OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, + obb2, tf1, tf2); } /// @brief collision between shape and octree - template + template void ShapeOcTreeIntersect(const S& s, const OcTree* tree, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, - CollisionResult& result_) const - { + CollisionResult& result_) const { crequest = &request_; cresult = &result_; @@ -196,117 +177,105 @@ class HPP_FCL_DLLAPI OcTreeSolver computeBV(s, Transform3f(), bv1); OBB obb1; convertBV(bv1, tf1, obb1); - OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), - s, obb1, - tf2, tf1); + OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, + obb1, tf2, tf1); } /// @brief distance between octree and shape - template + template void OcTreeShapeDistance(const OcTree* tree, const S& s, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, - DistanceResult& result_) const - { + DistanceResult& result_) const { drequest = &request_; dresult = &result_; AABB aabb2; computeBV(s, tf2, aabb2); - OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), - s, aabb2, - tf1, tf2); + OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, + aabb2, tf1, tf2); } /// @brief distance between shape and octree - template + template void ShapeOcTreeDistance(const S& s, const OcTree* tree, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, - DistanceResult& result_) const - { + DistanceResult& result_) const { drequest = &request_; dresult = &result_; AABB aabb1; computeBV(s, tf1, aabb1); - OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), - s, aabb1, - tf2, tf1); + OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, + aabb1, tf2, tf1); } - -private: - template - bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const S& s, const AABB& aabb2, - const Transform3f& tf1, const Transform3f& tf2) const - { - if(!tree1->nodeHasChildren(root1)) - { - if(tree1->isNodeOccupied(root1)) - { + private: + template + bool OcTreeShapeDistanceRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, + const AABB& bv1, const S& s, + const AABB& aabb2, const Transform3f& tf1, + const Transform3f& tf2) const { + if (!tree1->nodeHasChildren(root1)) { + if (tree1->isNodeOccupied(root1)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); - + FCL_REAL dist; Vec3f closest_p1, closest_p2, normal; - solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1, - closest_p2, normal); - - dresult->update(dist, tree1, &s, (int) (root1 - tree1->getRoot()), - DistanceResult::NONE, closest_p1, closest_p2, - normal); - + solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1, closest_p2, + normal); + + dresult->update(dist, tree1, &s, (int)(root1 - tree1->getRoot()), + DistanceResult::NONE, closest_p1, closest_p2, normal); + return drequest->isSatisfied(*dresult); - } - else + } else return false; } - if(!tree1->isNodeOccupied(root1)) return false; - - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { + if (!tree1->isNodeOccupied(root1)) return false; + + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); - + AABB aabb1; convertBV(child_bv, tf1, aabb1); FCL_REAL d = aabb1.distance(aabb2); - if(d < dresult->min_distance) - { - if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2)) + if (d < dresult->min_distance) { + if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, + tf2)) return true; - } + } } } return false; } - template - bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const S& s, const OBB& obb2, - const Transform3f& tf1, const Transform3f& tf2) const - { - if(!root1) - { + template + bool OcTreeShapeIntersectRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, + const AABB& bv1, const S& s, const OBB& obb2, + const Transform3f& tf1, + const Transform3f& tf2) const { + if (!root1) { OBB obb1; convertBV(bv1, tf1, obb1); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); FCL_REAL distance; - if(solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, NULL)) - { + if (solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, + NULL)) { AABB overlap_part; AABB aabb1, aabb2; computeBV(box, box_tf, aabb1); @@ -316,85 +285,82 @@ class HPP_FCL_DLLAPI OcTreeSolver } return false; - } - else if(!tree1->nodeHasChildren(root1)) - { - if(tree1->isNodeOccupied(root1)) // occupied area + } else if (!tree1->nodeHasChildren(root1)) { + if (tree1->isNodeOccupied(root1)) // occupied area { OBB obb1; convertBV(bv1, tf1, obb1); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); FCL_REAL distance; - if(!crequest->enable_contact) - { - if(solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, NULL)) - { - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, &s, static_cast(root1 - tree1->getRoot()), Contact::NONE)); + if (!crequest->enable_contact) { + if (solver->shapeIntersect(box, box_tf, s, tf2, distance, false, + NULL, NULL)) { + if (cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact(Contact( + tree1, &s, static_cast(root1 - tree1->getRoot()), + Contact::NONE)); } - } - else - { + } else { Vec3f contact; Vec3f normal; - if(solver->shapeIntersect(box, box_tf, s, tf2, distance, true, &contact, &normal)) - { - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, &s, static_cast(root1 - tree1->getRoot()), Contact::NONE, contact, normal, distance)); + if (solver->shapeIntersect(box, box_tf, s, tf2, distance, true, + &contact, &normal)) { + if (cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact(Contact( + tree1, &s, static_cast(root1 - tree1->getRoot()), + Contact::NONE, contact, normal, distance)); } } return crequest->isSatisfied(*cresult); - } - else return false; - } - else // free area + } else + return false; + } else // free area return false; } /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least of one the nodes is free; OR - /// 2) (two uncertain nodes or one node occupied and one node uncertain) AND cost not required - if(tree1->isNodeFree(root1)) return false; - else if((tree1->isNodeUncertain(root1) || s.isUncertain())) return false; - else - { + /// 2) (two uncertain nodes or one node occupied and one node + /// uncertain) AND cost not required + if (tree1->isNodeFree(root1)) + return false; + else if ((tree1->isNodeUncertain(root1) || s.isUncertain())) + return false; + else { OBB obb1; convertBV(bv1, tf1, obb1); - if(!obb1.overlap(obb2)) return false; + if (!obb1.overlap(obb2)) return false; } - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); - - if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2)) + + if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, + tf2)) return true; } } - return false; + return false; } - template - bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const BVHModel* tree2, unsigned int root2, - const Transform3f& tf1, const Transform3f& tf2) const - { - if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) - { - if(tree1->isNodeOccupied(root1)) - { + template + bool OcTreeMeshDistanceRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, + const AABB& bv1, const BVHModel* tree2, + unsigned int root2, const Transform3f& tf1, + const Transform3f& tf2) const { + if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { + if (tree1->isNodeOccupied(root1)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); @@ -404,29 +370,27 @@ class HPP_FCL_DLLAPI OcTreeSolver const Vec3f& p1 = tree2->vertices[tri_id[0]]; const Vec3f& p2 = tree2->vertices[tri_id[1]]; const Vec3f& p3 = tree2->vertices[tri_id[2]]; - + FCL_REAL dist; Vec3f closest_p1, closest_p2, normal; solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, dist, closest_p1, closest_p2, normal); - dresult->update(dist, tree1, tree2, (int) (root1 - tree1->getRoot()), + dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()), primitive_id, closest_p1, closest_p2, normal); return drequest->isSatisfied(*dresult); - } - else + } else return false; } - if(!tree1->isNodeOccupied(root1)) return false; + if (!tree1->isNodeOccupied(root1)) return false; - if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { + if (tree2->getBV(root2).isLeaf() || + (tree1->nodeHasChildren(root1) && + (bv1.size() > tree2->getBV(root2).bv.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); @@ -436,17 +400,15 @@ class HPP_FCL_DLLAPI OcTreeSolver convertBV(child_bv, tf1, aabb1); convertBV(tree2->getBV(root2).bv, tf2, aabb2); d = aabb1.distance(aabb2); - - if(d < dresult->min_distance) - { - if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) + + if (d < dresult->min_distance) { + if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, + tf1, tf2)) return true; } } } - } - else - { + } else { FCL_REAL d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); @@ -454,41 +416,38 @@ class HPP_FCL_DLLAPI OcTreeSolver convertBV(tree2->getBV(child).bv, tf2, aabb2); d = aabb1.distance(aabb2); - if(d < dresult->min_distance) - { - if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) + if (d < dresult->min_distance) { + if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, + tf2)) return true; } child = (unsigned int)tree2->getBV(root2).rightChild(); convertBV(tree2->getBV(child).bv, tf2, aabb2); d = aabb1.distance(aabb2); - - if(d < dresult->min_distance) - { - if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) - return true; + + if (d < dresult->min_distance) { + if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, + tf2)) + return true; } } return false; } - - template - bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const BVHModel* tree2, unsigned int root2, - const Transform3f& tf1, const Transform3f& tf2) const - { - if(!root1) - { - if(tree2->getBV(root2).isLeaf()) - { + template + bool OcTreeMeshIntersectRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, + const AABB& bv1, const BVHModel* tree2, + unsigned int root2, const Transform3f& tf1, + const Transform3f& tf2) const { + if (!root1) { + if (tree2->getBV(root2).isLeaf()) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(tree2->getBV(root2).bv, tf2, obb2); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); @@ -500,9 +459,8 @@ class HPP_FCL_DLLAPI OcTreeSolver const Vec3f& p3 = tree2->vertices[tri_id[2]]; Vec3f c1, c2, normal; FCL_REAL distance; - if(solver->shapeTriangleInteraction - (box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal)) - { + if (solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, + distance, c1, c2, normal)) { AABB overlap_part; AABB aabb1; computeBV(box, box_tf, aabb1); @@ -512,27 +470,25 @@ class HPP_FCL_DLLAPI OcTreeSolver } return false; - } - else - { - if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, (unsigned int)tree2->getBV(root2).leftChild(), tf1, tf2)) + } else { + if (OcTreeMeshIntersectRecurse( + tree1, root1, bv1, tree2, + (unsigned int)tree2->getBV(root2).leftChild(), tf1, tf2)) return true; - if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, (unsigned int)tree2->getBV(root2).rightChild(), tf1, tf2)) + if (OcTreeMeshIntersectRecurse( + tree1, root1, bv1, tree2, + (unsigned int)tree2->getBV(root2).rightChild(), tf1, tf2)) return true; return false; } - } - else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) - { - if(tree1->isNodeOccupied(root1)) - { + } else if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { + if (tree1->isNodeOccupied(root1)) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(tree2->getBV(root2).bv, tf2, obb2); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); @@ -542,96 +498,91 @@ class HPP_FCL_DLLAPI OcTreeSolver const Vec3f& p1 = tree2->vertices[tri_id[0]]; const Vec3f& p2 = tree2->vertices[tri_id[1]]; const Vec3f& p3 = tree2->vertices[tri_id[2]]; - - if(!crequest->enable_contact) - { + + if (!crequest->enable_contact) { Vec3f c1, c2, normal; FCL_REAL distance; - if(solver->shapeTriangleInteraction - (box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal)) - { - if(cresult->numContacts() < crequest->num_max_contacts) + if (solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, + distance, c1, c2, normal)) { + if (cresult->numContacts() < crequest->num_max_contacts) cresult->addContact(Contact(tree1, tree2, (int)(root1 - tree1->getRoot()), primitive_id)); } - } - else - { + } else { Vec3f c1, c2; FCL_REAL distance; Vec3f normal; - if(solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, - distance, c1, c2, normal)) - { - assert (crequest->security_margin == 0); - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact - (Contact(tree1, tree2, (int) (root1 - tree1->getRoot()), - primitive_id, c1, normal, -distance)); + if (solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, + distance, c1, c2, normal)) { + assert(crequest->security_margin == 0); + if (cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact( + Contact(tree1, tree2, (int)(root1 - tree1->getRoot()), + primitive_id, c1, normal, -distance)); } } return crequest->isSatisfied(*cresult); - } - else + } else return false; - } - else // free area + } else // free area return false; } /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR - /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required - if(tree1->isNodeFree(root1)) return false; - else if((tree1->isNodeUncertain(root1) || tree2->isUncertain())) + /// 2) (two uncertain nodes OR one node occupied and one node + /// uncertain) AND cost not required + if (tree1->isNodeFree(root1)) + return false; + else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) return false; - else - { + else { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(tree2->getBV(root2).bv, tf2, obb2); - if(!obb1.overlap(obb2)) return false; + if (!obb1.overlap(obb2)) return false; } - - if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { + + if (tree2->getBV(root2).isLeaf() || + (tree1->nodeHasChildren(root1) && + (bv1.size() > tree2->getBV(root2).bv.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); - - if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) + + if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, + tf1, tf2)) return true; } } - } - else - { - if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, (unsigned int)tree2->getBV(root2).leftChild(), tf1, tf2)) + } else { + if (OcTreeMeshIntersectRecurse( + tree1, root1, bv1, tree2, + (unsigned int)tree2->getBV(root2).leftChild(), tf1, tf2)) return true; - if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, (unsigned int)tree2->getBV(root2).rightChild(), tf1, tf2)) - return true; - + if (OcTreeMeshIntersectRecurse( + tree1, root1, bv1, tree2, + (unsigned int)tree2->getBV(root2).rightChild(), tf1, tf2)) + return true; } return false; } - bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, - const Transform3f& tf1, const Transform3f& tf2) const - { - if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) - { - if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) - { + bool OcTreeDistanceRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, const AABB& bv1, + const OcTree* tree2, + const OcTree::OcTreeNode* root2, const AABB& bv2, + const Transform3f& tf1, + const Transform3f& tf2) const { + if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { + if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { Box box1, box2; Transform3f box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); @@ -642,24 +593,22 @@ class HPP_FCL_DLLAPI OcTreeSolver solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist, closest_p1, closest_p2, normal); - dresult->update(dist, tree1, tree2, (int) (root1 - tree1->getRoot()), - (int) (root2 - tree2->getRoot()), - closest_p1, closest_p2, normal); - + dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()), + (int)(root2 - tree2->getRoot()), closest_p1, closest_p2, + normal); + return drequest->isSatisfied(*dresult); - } - else + } else return false; } - if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false; + if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) + return false; - if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { + if (!tree2->nodeHasChildren(root2) || + (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); @@ -670,21 +619,16 @@ class HPP_FCL_DLLAPI OcTreeSolver convertBV(bv2, tf2, aabb2); d = aabb1.distance(aabb2); - if(d < dresult->min_distance) - { - - if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2)) + if (d < dresult->min_distance) { + if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, + tf1, tf2)) return true; } } } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); @@ -695,36 +639,35 @@ class HPP_FCL_DLLAPI OcTreeSolver convertBV(bv2, tf2, aabb2); d = aabb1.distance(aabb2); - if(d < dresult->min_distance) - { - if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2)) + if (d < dresult->min_distance) { + if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, + tf1, tf2)) return true; } } } } - + return false; } - - bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, - const Transform3f& tf1, const Transform3f& tf2) const - { - if(!root1 && !root2) - { + bool OcTreeIntersectRecurse(const OcTree* tree1, + const OcTree::OcTreeNode* root1, const AABB& bv1, + const OcTree* tree2, + const OcTree::OcTreeNode* root2, const AABB& bv2, + const Transform3f& tf1, + const Transform3f& tf2) const { + if (!root1 && !root2) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box box1, box2; Transform3f box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); - + AABB overlap_part; AABB aabb1, aabb2; computeBV(box1, box1_tf, aabb1); @@ -733,87 +676,73 @@ class HPP_FCL_DLLAPI OcTreeSolver } return false; - } - else if(!root1 && root2) - { - if(tree2->nodeHasChildren(root2)) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else if (!root1 && root2) { + if (tree2->nodeHasChildren(root2)) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); - if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, tf1, tf2)) + if (OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, + tf1, tf2)) return true; - } - else - { + } else { AABB child_bv; computeChildBV(bv2, i, child_bv); - if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, child_bv, tf1, tf2)) + if (OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, child_bv, + tf1, tf2)) return true; } } - } - else - { - if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2)) + } else { + if (OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, + tf2)) return true; } - + return false; - } - else if(root1 && !root2) - { - if(tree1->nodeHasChildren(root1)) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { + } else if (root1 && !root2) { + if (tree1->nodeHasChildren(root1)) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; - computeChildBV(bv1, i, child_bv); - if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, tf1, tf2)) + computeChildBV(bv1, i, child_bv); + if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, + tf1, tf2)) return true; - } - else - { + } else { AABB child_bv; computeChildBV(bv1, i, child_bv); - if(OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, NULL, bv2, tf1, tf2)) + if (OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, NULL, bv2, + tf1, tf2)) return true; } } - } - else - { - if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2)) + } else { + if (OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, + tf2)) return true; } - + return false; - } - else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) - { - if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area + } else if (!tree1->nodeHasChildren(root1) && + !tree2->nodeHasChildren(root2)) { + if (tree1->isNodeOccupied(root1) && + tree2->isNodeOccupied(root2)) // occupied area { - if(!crequest->enable_contact) - { + if (!crequest->enable_contact) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); - - if(obb1.overlap(obb2)) - { - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), static_cast(root2 - tree2->getRoot()))); + + if (obb1.overlap(obb2)) { + if (cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact(Contact( + tree1, tree2, static_cast(root1 - tree1->getRoot()), + static_cast(root2 - tree2->getRoot()))); } - } - else - { + } else { Box box1, box2; Transform3f box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); @@ -822,63 +751,58 @@ class HPP_FCL_DLLAPI OcTreeSolver Vec3f contact; FCL_REAL distance; Vec3f normal; - if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, distance, true, &contact, &normal)) - { - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), static_cast(root2 - tree2->getRoot()), contact, normal, distance)); + if (solver->shapeIntersect(box1, box1_tf, box2, box2_tf, distance, + true, &contact, &normal)) { + if (cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact(Contact( + tree1, tree2, static_cast(root1 - tree1->getRoot()), + static_cast(root2 - tree2->getRoot()), contact, normal, + distance)); } } return crequest->isSatisfied(*cresult); - } - else // free area (at least one node is free) + } else // free area (at least one node is free) return false; } /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR - /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required - if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false; - else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2))) + /// 2) (two uncertain nodes OR one node occupied and one node + /// uncertain) AND cost not required + if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) + return false; + else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2))) return false; - else - { + else { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); - if(!obb1.overlap(obb2)) return false; + if (!obb1.overlap(obb2)) return false; } - if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { + if (!tree2->nodeHasChildren(root2) || + (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { + for (unsigned int i = 0; i < 8; ++i) { + if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); - - if(OcTreeIntersectRecurse(tree1, child, child_bv, - tree2, root2, bv2, - tf1, tf2)) + + if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2, + tf1, tf2)) return true; } } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); - - if(OcTreeIntersectRecurse(tree1, root1, bv1, - tree2, child, child_bv, - tf1, tf2)) + + if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv, + tf1, tf2)) return true; } } @@ -892,30 +816,22 @@ class HPP_FCL_DLLAPI OcTreeSolver /// @{ /// @brief Traversal node for octree collision -class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - OcTreeCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase (request) - { +class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + OcTreeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned) const - { - return false; - } + bool BVDisjoints(unsigned int, unsigned) const { return false; } - bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const - { - return false; - } + bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; } - void leafCollides(unsigned, unsigned, FCL_REAL&) const - { + void leafCollides(unsigned, unsigned, FCL_REAL&) const { otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); } @@ -928,31 +844,25 @@ class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode : public CollisionTraversalNod }; /// @brief Traversal node for shape-octree collision -template -class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase (request) - { +template +class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const - { - return false; - } + bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const - { + bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } - void leafCollides(unsigned int, unsigned int, FCL_REAL&) const - { + void leafCollides(unsigned int, unsigned int, FCL_REAL&) const { otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); } @@ -966,31 +876,25 @@ class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode : public CollisionTravers /// @brief Traversal node for octree-shape collision -template -class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase (request) - { +template +class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const - { - return false; - } + bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const - { + bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const { return false; } - void leafCollides(unsigned int, unsigned int, FCL_REAL&) const - { + void leafCollides(unsigned int, unsigned int, FCL_REAL&) const { otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); } @@ -998,36 +902,30 @@ class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode : public CollisionTravers const S* model2; Transform3f tf1, tf2; - - const OcTreeSolver* otsolver; + + const OcTreeSolver* otsolver; }; /// @brief Traversal node for mesh-octree collision -template -class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase (request) - { +template +class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const - { - return false; - } + bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const - { + bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } - void leafCollides(unsigned int, unsigned int, FCL_REAL&) const - { + void leafCollides(unsigned int, unsigned int, FCL_REAL&) const { otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); } @@ -1035,36 +933,30 @@ class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode : public CollisionTraversa const OcTree* model2; Transform3f tf1, tf2; - + const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-mesh collision -template -class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase (request) - { +template +class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const - { - return false; - } + bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const - { + bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } - void leafCollides(unsigned int, unsigned int, FCL_REAL&) const - { + void leafCollides(unsigned int, unsigned int, FCL_REAL&) const { otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); } @@ -1072,7 +964,7 @@ class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode : public CollisionTraversa const BVHModel* model2; Transform3f tf1, tf2; - + const OcTreeSolver* otsolver; }; @@ -1082,30 +974,23 @@ class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode : public CollisionTraversa /// @{ /// @brief Traversal node for octree distance -class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - OcTreeDistanceTraversalNode() - { +class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + OcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } + FCL_REAL BVDistanceLowerBound(unsigned, unsigned) const { return -1; } - FCL_REAL BVDistanceLowerBound(unsigned, unsigned) const - { - return -1; - } - - bool BVDistanceLowerBound(unsigned, unsigned, FCL_REAL&) const - { + bool BVDistanceLowerBound(unsigned, unsigned, FCL_REAL&) const { return false; } - void leafComputeDistance(unsigned, unsigned int) const - { + void leafComputeDistance(unsigned, unsigned int) const { otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result); } @@ -1115,28 +1000,21 @@ class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode : public DistanceTraversalNodeB const OcTreeSolver* otsolver; }; - - /// @brief Traversal node for shape-octree distance -template -class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - ShapeOcTreeDistanceTraversalNode() - { +template +class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + ShapeOcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; - + otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const - { - return -1; - } + FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } - void leafComputeDistance(unsigned int, unsigned int) const - { + void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); } @@ -1147,25 +1025,20 @@ class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode : public DistanceTraversal }; /// @brief Traversal node for octree-shape distance -template -class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - OcTreeShapeDistanceTraversalNode() - { +template +class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + OcTreeShapeDistanceTraversalNode() { model1 = NULL; model2 = NULL; - + otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const - { - return -1; - } + FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } - void leafComputeDistance(unsigned int, unsigned int) const - { + void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); } @@ -1176,25 +1049,20 @@ class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode : public DistanceTraversal }; /// @brief Traversal node for mesh-octree distance -template -class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - MeshOcTreeDistanceTraversalNode() - { +template +class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + MeshOcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; - + otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const - { - return -1; - } + FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } - void leafComputeDistance(unsigned int, unsigned int) const - { + void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); } @@ -1202,29 +1070,23 @@ class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode : public DistanceTraversalN const OcTree* model2; const OcTreeSolver* otsolver; - }; /// @brief Traversal node for octree-mesh distance -template -class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - OcTreeMeshDistanceTraversalNode() - { +template +class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + OcTreeMeshDistanceTraversalNode() { model1 = NULL; model2 = NULL; - + otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const - { - return -1; - } + FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } - void leafComputeDistance(unsigned int, unsigned int) const - { + void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); } @@ -1232,14 +1094,13 @@ class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode : public DistanceTraversalN const BVHModel* model2; const OcTreeSolver* otsolver; - }; /// @} -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h index 977ee7a32..41f79dc67 100644 --- a/include/hpp/fcl/internal/traversal_node_setup.h +++ b/include/hpp/fcl/internal/traversal_node_setup.h @@ -55,25 +55,21 @@ #include - -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { #ifdef HPP_FCL_HAS_OCTOMAP -/// @brief Initialize traversal node for collision between two octrees, given current object transform -inline bool initialize(OcTreeCollisionTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - CollisionResult& result) -{ +/// @brief Initialize traversal node for collision between two octrees, given +/// current object transform +inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1, + const Transform3f& tf1, const OcTree& model2, + const Transform3f& tf2, const OcTreeSolver* otsolver, + CollisionResult& result) { node.result = &result; node.model1 = &model1; node.model2 = &model2; - + node.otsolver = otsolver; node.tf1 = tf1; @@ -82,37 +78,35 @@ inline bool initialize(OcTreeCollisionTraversalNode& node, return true; } -/// @brief Initialize traversal node for distance between two octrees, given current object transform -inline bool initialize(OcTreeDistanceTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) +/// @brief Initialize traversal node for distance between two octrees, given +/// current object transform +inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1, + const Transform3f& tf1, const OcTree& model2, + const Transform3f& tf2, const OcTreeSolver* otsolver, + const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; - + node.model1 = &model1; node.model2 = &model2; - + node.otsolver = otsolver; - + node.tf1 = tf1; node.tf2 = tf2; return true; } -/// @brief Initialize traversal node for collision between one shape and one octree, given current object transform -template -bool initialize(ShapeOcTreeCollisionTraversalNode& node, - const S& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - CollisionResult& result) -{ +/// @brief Initialize traversal node for collision between one shape and one +/// octree, given current object transform +template +bool initialize(ShapeOcTreeCollisionTraversalNode& node, const S& model1, + const Transform3f& tf1, const OcTree& model2, + const Transform3f& tf2, const OcTreeSolver* otsolver, + CollisionResult& result) { node.result = &result; node.model1 = &model1; @@ -126,14 +120,13 @@ bool initialize(ShapeOcTreeCollisionTraversalNode& node, return true; } -/// @brief Initialize traversal node for collision between one octree and one shape, given current object transform -template +/// @brief Initialize traversal node for collision between one octree and one +/// shape, given current object transform +template bool initialize(OcTreeShapeCollisionTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - CollisionResult& result) -{ + const OcTree& model1, const Transform3f& tf1, const S& model2, + const Transform3f& tf2, const OcTreeSolver* otsolver, + CollisionResult& result) { node.result = &result; node.model1 = &model1; @@ -147,15 +140,13 @@ bool initialize(OcTreeShapeCollisionTraversalNode& node, return true; } -/// @brief Initialize traversal node for distance between one shape and one octree, given current object transform -template -bool initialize(ShapeOcTreeDistanceTraversalNode& node, - const S& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ +/// @brief Initialize traversal node for distance between one shape and one +/// octree, given current object transform +template +bool initialize(ShapeOcTreeDistanceTraversalNode& node, const S& model1, + const Transform3f& tf1, const OcTree& model2, + const Transform3f& tf2, const OcTreeSolver* otsolver, + const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; @@ -170,15 +161,13 @@ bool initialize(ShapeOcTreeDistanceTraversalNode& node, return true; } -/// @brief Initialize traversal node for distance between one octree and one shape, given current object transform -template -bool initialize(OcTreeShapeDistanceTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ +/// @brief Initialize traversal node for distance between one octree and one +/// shape, given current object transform +template +bool initialize(OcTreeShapeDistanceTraversalNode& node, const OcTree& model1, + const Transform3f& tf1, const S& model2, const Transform3f& tf2, + const OcTreeSolver* otsolver, const DistanceRequest& request, + DistanceResult& result) { node.request = request; node.result = &result; @@ -193,14 +182,13 @@ bool initialize(OcTreeShapeDistanceTraversalNode& node, return true; } -/// @brief Initialize traversal node for collision between one mesh and one octree, given current object transform -template +/// @brief Initialize traversal node for collision between one mesh and one +/// octree, given current object transform +template bool initialize(MeshOcTreeCollisionTraversalNode& node, const BVHModel& model1, const Transform3f& tf1, const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - CollisionResult& result) -{ + const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; @@ -214,14 +202,13 @@ bool initialize(MeshOcTreeCollisionTraversalNode& node, return true; } -/// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform -template +/// @brief Initialize traversal node for collision between one octree and one +/// mesh, given current object transform +template bool initialize(OcTreeMeshCollisionTraversalNode& node, const OcTree& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - CollisionResult& result) -{ + const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; node.model1 = &model1; @@ -235,21 +222,20 @@ bool initialize(OcTreeMeshCollisionTraversalNode& node, return true; } -/// @brief Initialize traversal node for distance between one mesh and one octree, given current object transform -template +/// @brief Initialize traversal node for distance between one mesh and one +/// octree, given current object transform +template bool initialize(MeshOcTreeDistanceTraversalNode& node, const BVHModel& model1, const Transform3f& tf1, const OcTree& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ + const OcTreeSolver* otsolver, const DistanceRequest& request, + DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; - + node.otsolver = otsolver; node.tf1 = tf1; @@ -258,21 +244,19 @@ bool initialize(MeshOcTreeDistanceTraversalNode& node, return true; } -/// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform -template -bool initialize(OcTreeMeshDistanceTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ +/// @brief Initialize traversal node for collision between one octree and one +/// mesh, given current object transform +template +bool initialize(OcTreeMeshDistanceTraversalNode& node, const OcTree& model1, + const Transform3f& tf1, const BVHModel& model2, + const Transform3f& tf2, const OcTreeSolver* otsolver, + const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; - + node.otsolver = otsolver; node.tf1 = tf1; @@ -283,15 +267,13 @@ bool initialize(OcTreeMeshDistanceTraversalNode& node, #endif - -/// @brief Initialize traversal node for collision between two geometric shapes, given current object transform -template -bool initialize(ShapeCollisionTraversalNode& node, - const S1& shape1, const Transform3f& tf1, - const S2& shape2, const Transform3f& tf2, - const GJKSolver* nsolver, - CollisionResult& result) -{ +/// @brief Initialize traversal node for collision between two geometric shapes, +/// given current object transform +template +bool initialize(ShapeCollisionTraversalNode& node, const S1& shape1, + const Transform3f& tf1, const S2& shape2, + const Transform3f& tf2, const GJKSolver* nsolver, + CollisionResult& result) { node.model1 = &shape1; node.tf1 = tf1; node.model2 = &shape2; @@ -299,28 +281,28 @@ bool initialize(ShapeCollisionTraversalNode& node, node.nsolver = nsolver; node.result = &result; - + return true; } -/// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform -template +/// @brief Initialize traversal node for collision between one mesh and one +/// shape, given current object transform +template bool initialize(MeshShapeCollisionTraversalNode& node, - BVHModel& model1, Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - CollisionResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - - if(!tf1.isIdentity()) // TODO(jcarpent): vectorized version + BVHModel& model1, Transform3f& tf1, const S& model2, + const Transform3f& tf2, const GJKSolver* nsolver, + CollisionResult& result, bool use_refit = false, + bool refit_bottomup = false) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf1.isIdentity()) // TODO(jcarpent): vectorized version { std::vector vertices_transformed(model1.num_vertices); - for(unsigned int i = 0; i < model1.num_vertices; ++i) - { - const Vec3f & p = model1.vertices[i]; + for (unsigned int i = 0; i < model1.num_vertices; ++i) { + const Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed[i] = new_v; } @@ -348,16 +330,17 @@ bool initialize(MeshShapeCollisionTraversalNode& node, return true; } -/// @brief Initialize traversal node for collision between one mesh and one shape -template +/// @brief Initialize traversal node for collision between one mesh and one +/// shape +template bool initialize(MeshShapeCollisionTraversalNode& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - CollisionResult& result) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) + const GJKSolver* nsolver, CollisionResult& result) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) node.model1 = &model1; node.tf1 = tf1; @@ -375,21 +358,18 @@ bool initialize(MeshShapeCollisionTraversalNode& node, return true; } -/// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform -template +/// @brief Initialize traversal node for collision between one mesh and one +/// shape, given current object transform +template bool initialize(HeightFieldShapeCollisionTraversalNode& node, - HeightField& model1, Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - CollisionResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(!tf1.isIdentity()) - { + HeightField& model1, Transform3f& tf1, const S& model2, + const Transform3f& tf2, const GJKSolver* nsolver, + CollisionResult& result, bool use_refit = false, + bool refit_bottomup = false) { + if (!tf1.isIdentity()) { std::vector vertices_transformed(model1.num_vertices); - for(unsigned int i = 0; i < model1.num_vertices; ++i) - { - const Vec3f & p = model1.vertices[i]; + for (unsigned int i = 0; i < model1.num_vertices; ++i) { + const Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed[i] = new_v; } @@ -417,14 +397,13 @@ bool initialize(HeightFieldShapeCollisionTraversalNode& node, return true; } -/// @brief Initialize traversal node for collision between one mesh and one shape -template +/// @brief Initialize traversal node for collision between one mesh and one +/// shape +template bool initialize(HeightFieldShapeCollisionTraversalNode& node, const HeightField& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - CollisionResult& result) -{ + const GJKSolver* nsolver, CollisionResult& result) { node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; @@ -439,17 +418,16 @@ bool initialize(HeightFieldShapeCollisionTraversalNode& node, } /// @cond IGNORE -namespace details -{ -template class OrientedNode> -static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode& node, - const S& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - CollisionResult& result) -{ - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) +namespace details { +template class OrientedNode> +static inline bool setupShapeMeshCollisionOrientedNode( + OrientedNode& node, const S& model1, const Transform3f& tf1, + const BVHModel& model2, const Transform3f& tf2, + const GJKSolver* nsolver, CollisionResult& result) { + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) node.model1 = &model1; node.tf1 = tf1; @@ -466,28 +444,29 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode& node, return true; } -} +} // namespace details /// @endcond - -/// @brief Initialize traversal node for collision between two meshes, given the current transforms -template -bool initialize(MeshCollisionTraversalNode& node, - BVHModel& model1, Transform3f& tf1, - BVHModel& model2, Transform3f& tf2, - CollisionResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - - if(!tf1.isIdentity()) - { +/// @brief Initialize traversal node for collision between two meshes, given the +/// current transforms +template +bool initialize( + MeshCollisionTraversalNode& node, + BVHModel& model1, Transform3f& tf1, BVHModel& model2, + Transform3f& tf2, CollisionResult& result, bool use_refit = false, + bool refit_bottomup = false) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf1.isIdentity()) { std::vector vertices_transformed1(model1.num_vertices); - for(unsigned int i = 0; i < model1.num_vertices; ++i) - { + for (unsigned int i = 0; i < model1.num_vertices; ++i) { Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed1[i] = new_v; @@ -500,11 +479,9 @@ bool initialize(MeshCollisionTraversalNode tf1.setIdentity(); } - if(!tf2.isIdentity()) - { + if (!tf2.isIdentity()) { std::vector vertices_transformed2(model2.num_vertices); - for(unsigned int i = 0; i < model2.num_vertices; ++i) - { + for (unsigned int i = 0; i < model2.num_vertices; ++i) { Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed2[i] = new_v; @@ -533,16 +510,19 @@ bool initialize(MeshCollisionTraversalNode return true; } -template +template bool initialize(MeshCollisionTraversalNode& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, - CollisionResult& result) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) + CollisionResult& result) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) node.vertices1 = model1.vertices; node.vertices2 = model2.vertices; @@ -558,20 +538,18 @@ bool initialize(MeshCollisionTraversalNode& node, node.result = &result; node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation(); - node.RT.T.noalias() = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation()); + node.RT.T.noalias() = tf1.getRotation().transpose() * + (tf2.getTranslation() - tf1.getTranslation()); return true; } /// @brief Initialize traversal node for distance between two geometric shapes -template -bool initialize(ShapeDistanceTraversalNode& node, - const S1& shape1, const Transform3f& tf1, - const S2& shape2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ +template +bool initialize(ShapeDistanceTraversalNode& node, const S1& shape1, + const Transform3f& tf1, const S2& shape2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; @@ -584,26 +562,27 @@ bool initialize(ShapeDistanceTraversalNode& node, return true; } -/// @brief Initialize traversal node for distance computation between two meshes, given the current transforms -template -bool initialize(MeshDistanceTraversalNode& node, - BVHModel& model1, Transform3f& tf1, - BVHModel& model2, Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - - if(!tf1.isIdentity()) - { +/// @brief Initialize traversal node for distance computation between two +/// meshes, given the current transforms +template +bool initialize( + MeshDistanceTraversalNode& node, + BVHModel& model1, Transform3f& tf1, BVHModel& model2, + Transform3f& tf2, const DistanceRequest& request, DistanceResult& result, + bool use_refit = false, bool refit_bottomup = false) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf1.isIdentity()) { std::vector vertices_transformed1(model1.num_vertices); - for(unsigned int i = 0; i < model1.num_vertices; ++i) - { - const Vec3f & p = model1.vertices[i]; + for (unsigned int i = 0; i < model1.num_vertices; ++i) { + const Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } @@ -615,12 +594,10 @@ bool initialize(MeshDistanceTraversalNode& tf1.setIdentity(); } - if(!tf2.isIdentity()) - { + if (!tf2.isIdentity()) { std::vector vertices_transformed2(model2.num_vertices); - for(unsigned int i = 0; i < model2.num_vertices; ++i) - { - const Vec3f & p = model2.vertices[i]; + for (unsigned int i = 0; i < model2.num_vertices; ++i) { + const Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed2[i] = new_v; } @@ -650,17 +627,19 @@ bool initialize(MeshDistanceTraversalNode& } /// @brief Initialize traversal node for distance computation between two meshes -template +template bool initialize(MeshDistanceTraversalNode& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) + const DistanceRequest& request, DistanceResult& result) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) node.request = request; node.result = &result; @@ -676,32 +655,29 @@ bool initialize(MeshDistanceTraversalNode& node, node.tri_indices1 = model1.tri_indices; node.tri_indices2 = model2.tri_indices; - relativeTransform(tf1.getRotation(), tf1.getTranslation(), - tf2.getRotation(), tf2.getTranslation(), - node.RT.R, node.RT.T); + relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), + tf2.getTranslation(), node.RT.R, node.RT.T); return true; } -/// @brief Initialize traversal node for distance computation between one mesh and one shape, given the current transforms -template +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, given the current transforms +template bool initialize(MeshShapeDistanceTraversalNode& node, - BVHModel& model1, Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - - if(!tf1.isIdentity()) - { + BVHModel& model1, Transform3f& tf1, const S& model2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result, + bool use_refit = false, bool refit_bottomup = false) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf1.isIdentity()) { std::vector vertices_transformed1(model1.num_vertices); - for(unsigned int i = 0; i < model1.num_vertices; ++i) - { - const Vec3f & p = model1.vertices[i]; + for (unsigned int i = 0; i < model1.num_vertices; ++i) { + const Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } @@ -721,7 +697,7 @@ bool initialize(MeshShapeDistanceTraversalNode& node, node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; - + node.vertices = model1.vertices; node.tri_indices = model1.tri_indices; @@ -730,25 +706,23 @@ bool initialize(MeshShapeDistanceTraversalNode& node, return true; } -/// @brief Initialize traversal node for distance computation between one shape and one mesh, given the current transforms -template -bool initialize(ShapeMeshDistanceTraversalNode& node, - const S& model1, const Transform3f& tf1, - BVHModel& model2, Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - - if(!tf2.isIdentity()) - { +/// @brief Initialize traversal node for distance computation between one shape +/// and one mesh, given the current transforms +template +bool initialize(ShapeMeshDistanceTraversalNode& node, const S& model1, + const Transform3f& tf1, BVHModel& model2, Transform3f& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result, bool use_refit = false, + bool refit_bottomup = false) { + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) + + if (!tf2.isIdentity()) { std::vector vertices_transformed(model2.num_vertices); - for(unsigned int i = 0; i < model2.num_vertices; ++i) - { - const Vec3f & p = model2.vertices[i]; + for (unsigned int i = 0; i < model2.num_vertices; ++i) { + const Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed[i] = new_v; } @@ -773,24 +747,22 @@ bool initialize(ShapeMeshDistanceTraversalNode& node, node.tri_indices = model2.tri_indices; computeBV(model1, tf1, node.model1_bv); - + return true; } /// @cond IGNORE -namespace details -{ - -template class OrientedNode> -static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) +namespace details { + +template class OrientedNode> +static inline bool setupMeshShapeDistanceOrientedNode( + OrientedNode& node, const BVHModel& model1, const Transform3f& tf1, + const S& model2, const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result) { + if (model1.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) node.request = request; node.result = &result; @@ -808,59 +780,57 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode& node, return true; } -} +} // namespace details /// @endcond -/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for RSS type -template +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, specialized for RSS type +template bool initialize(MeshShapeDistanceTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { + return details::setupMeshShapeDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); } -/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type -template +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, specialized for kIOS type +template bool initialize(MeshShapeDistanceTraversalNodekIOS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { + return details::setupMeshShapeDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); } -/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type -template +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, specialized for OBBRSS type +template bool initialize(MeshShapeDistanceTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { + return details::setupMeshShapeDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +namespace details { +template class OrientedNode> +static inline bool setupShapeMeshDistanceOrientedNode( + OrientedNode& node, const S& model1, const Transform3f& tf1, + const BVHModel& model2, const Transform3f& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { + if (model2.getModelType() != BVH_MODEL_TRIANGLES) + HPP_FCL_THROW_PRETTY( + "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", + std::invalid_argument) -namespace details -{ -template class OrientedNode> -static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode& node, - const S& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument) - node.request = request; node.result = &result; @@ -877,50 +847,46 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode& node, node.R = tf2.getRotation(); node.T = tf2.getTranslation(); - return true; -} + return true; } +} // namespace details - -/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type -template -bool initialize(ShapeMeshDistanceTraversalNodeRSS& node, - const S& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +/// @brief Initialize traversal node for distance computation between one shape +/// and one mesh, specialized for RSS type +template +bool initialize(ShapeMeshDistanceTraversalNodeRSS& node, const S& model1, + const Transform3f& tf1, const BVHModel& model2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result) { + return details::setupShapeMeshDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); } -/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type -template -bool initialize(ShapeMeshDistanceTraversalNodekIOS& node, - const S& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +/// @brief Initialize traversal node for distance computation between one shape +/// and one mesh, specialized for kIOS type +template +bool initialize(ShapeMeshDistanceTraversalNodekIOS& node, const S& model1, + const Transform3f& tf1, const BVHModel& model2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result) { + return details::setupShapeMeshDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); } -/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type -template -bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS& node, - const S& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +/// @brief Initialize traversal node for distance computation between one shape +/// and one mesh, specialized for OBBRSS type +template +bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS& node, const S& model1, + const Transform3f& tf1, const BVHModel& model2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, DistanceResult& result) { + return details::setupShapeMeshDistanceOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/hpp/fcl/internal/traversal_node_shapes.h index 0f965d9f4..9c54bc434 100644 --- a/include/hpp/fcl/internal/traversal_node_shapes.h +++ b/include/hpp/fcl/internal/traversal_node_shapes.h @@ -46,22 +46,19 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @addtogroup Traversal_For_Collision /// @{ /// @brief Traversal node for collision between two shapes -template -class HPP_FCL_DLLAPI ShapeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - ShapeCollisionTraversalNode(const CollisionRequest& request) : - CollisionTraversalNodeBase(request) - { +template +class HPP_FCL_DLLAPI ShapeCollisionTraversalNode + : public CollisionTraversalNodeBase { + public: + ShapeCollisionTraversalNode(const CollisionRequest& request) + : CollisionTraversalNodeBase(request) { model1 = NULL; model2 = NULL; @@ -69,46 +66,37 @@ class HPP_FCL_DLLAPI ShapeCollisionTraversalNode : public CollisionTraversalNode } /// @brief BV culling test in one BVTT node - bool BVDisjoints(int, int) const - { - return false; - } + bool BVDisjoints(int, int) const { return false; } /// @brief BV culling test in one BVTT node - bool BVDisjoints(int, int, FCL_REAL&) const - { - throw std::runtime_error ("Not implemented"); + bool BVDisjoints(int, int, FCL_REAL&) const { + throw std::runtime_error("Not implemented"); } /// @brief Intersection testing between leaves (two shapes) - void leafCollides(int, int, FCL_REAL&) const - { + void leafCollides(int, int, FCL_REAL&) const { bool is_collision = false; FCL_REAL distance; - if(request.enable_contact && request.num_max_contacts > result->numContacts()) - { + if (request.enable_contact && + request.num_max_contacts > result->numContacts()) { Vec3f contact_point, normal; - if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance, true, - &contact_point, &normal)) - { + if (nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance, true, + &contact_point, &normal)) { is_collision = true; - result->addContact(Contact(model1, model2, Contact::NONE, - Contact::NONE, contact_point, - normal, distance)); + result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, + contact_point, normal, distance)); } - } - else - { + } else { bool res = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance, - request.enable_distance_lower_bound, NULL, NULL); + request.enable_distance_lower_bound, + NULL, NULL); if (request.enable_distance_lower_bound) - result->updateDistanceLowerBound (distance); - if(res) - { + result->updateDistanceLowerBound(distance); + if (res) { is_collision = true; - if(request.num_max_contacts > result->numContacts()) - result->addContact(Contact(model1, model2, Contact::NONE, - Contact::NONE)); + if (request.num_max_contacts > result->numContacts()) + result->addContact( + Contact(model1, model2, Contact::NONE, Contact::NONE)); } } } @@ -125,12 +113,11 @@ class HPP_FCL_DLLAPI ShapeCollisionTraversalNode : public CollisionTraversalNode /// @{ /// @brief Traversal node for distance between two shapes -template -class HPP_FCL_DLLAPI ShapeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() - { +template +class HPP_FCL_DLLAPI ShapeDistanceTraversalNode + : public DistanceTraversalNodeBase { + public: + ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; @@ -138,14 +125,12 @@ class HPP_FCL_DLLAPI ShapeDistanceTraversalNode : public DistanceTraversalNodeBa } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const - { - return -1; // should not be used + FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; // should not be used } /// @brief Distance testing between leaves (two shapes) - void leafComputeDistance(unsigned int, unsigned int) const - { + void leafComputeDistance(unsigned int, unsigned int) const { FCL_REAL distance; Vec3f closest_p1, closest_p2, normal; nsolver->shapeDistance(*model1, tf1, *model2, tf2, distance, closest_p1, @@ -162,9 +147,9 @@ class HPP_FCL_DLLAPI ShapeDistanceTraversalNode : public DistanceTraversalNodeBa /// @} -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/internal/traversal_recurse.h b/include/hpp/fcl/internal/traversal_recurse.h index a5386d6b9..1f15fe4f2 100644 --- a/include/hpp/fcl/internal/traversal_recurse.h +++ b/include/hpp/fcl/internal/traversal_recurse.h @@ -45,35 +45,38 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// Recurse function for collision /// @param node collision node, /// @param b1, b2 ids of bounding volume nodes for object 1 and object 2 /// @retval sqrDistLowerBound squared lower bound on distance between objects. -void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, unsigned int b2, - BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound); +void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list, + FCL_REAL& sqrDistLowerBound); void collisionNonRecurse(CollisionTraversalNodeBase* node, - BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound); + BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound); /// @brief Recurse function for distance -void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list); +void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list); /// @brief Recurse function for distance, using queue acceleration -void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, unsigned int qsize); +void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list, + unsigned int qsize); /// @brief Recurse function for front list propagation -void propagateBVHFrontListCollisionRecurse - (CollisionTraversalNodeBase* node, const CollisionRequest& request, - CollisionResult& result, BVHFrontList* front_list); +void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, + const CollisionRequest& request, + CollisionResult& result, + BVHFrontList* front_list); -} +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h index f36c9a937..9c1bdb0b5 100644 --- a/include/hpp/fcl/math/matrix_3f.h +++ b/include/hpp/fcl/math/matrix_3f.h @@ -38,9 +38,9 @@ #ifndef HPP_FCL_MATRIX_3F_H #define HPP_FCL_MATRIX_3F_H -# warning "This file is deprecated. Include instead." +#warning "This file is deprecated. Include instead." // List of equivalent includes. -# include +#include #endif diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h index 5bd9b6032..11e3e02e5 100644 --- a/include/hpp/fcl/math/transform.h +++ b/include/hpp/fcl/math/transform.h @@ -35,239 +35,179 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_TRANSFORM_H #define HPP_FCL_TRANSFORM_H #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { typedef Eigen::Quaternion Quaternion3f; -static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q) -{ +static inline std::ostream& operator<<(std::ostream& o, const Quaternion3f& q) { o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")"; return o; } /// @brief Simple transform class used locally by InterpMotion -class HPP_FCL_DLLAPI Transform3f -{ +class HPP_FCL_DLLAPI Transform3f { /// @brief Matrix cache Matrix3f R; /// @brief Tranlation vector Vec3f T; -public: - + + public: /// @brief Default transform is no movement - Transform3f() - { - setIdentity(); // set matrix_set true + Transform3f() { + setIdentity(); // set matrix_set true } - + static Transform3f Identity() { return Transform3f(); } /// @brief Construct transform from rotation and translation template Transform3f(const Eigen::MatrixBase& R_, - const Eigen::MatrixBase& T_) : - R(R_), - T(T_) - {} + const Eigen::MatrixBase& T_) + : R(R_), T(T_) {} /// @brief Construct transform from rotation and translation template - Transform3f(const Quaternion3f& q_, - const Eigen::MatrixBase& T_) : - R(q_.toRotationMatrix()), - T(T_) - {} + Transform3f(const Quaternion3f& q_, const Eigen::MatrixBase& T_) + : R(q_.toRotationMatrix()), T(T_) {} /// @brief Construct transform from rotation - Transform3f(const Matrix3f& R_) : R(R_), - T(Vec3f::Zero()) - {} + Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {} /// @brief Construct transform from rotation - Transform3f(const Quaternion3f& q_) : R(q_), - T(Vec3f::Zero()) - {} + Transform3f(const Quaternion3f& q_) : R(q_), T(Vec3f::Zero()) {} /// @brief Construct transform from translation - Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), - T(T_) - {} + Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {} /// @brief Construct transform from other transform - Transform3f(const Transform3f& tf) : R(tf.R), - T(tf.T) - {} + Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {} - /// @brief operator = - Transform3f& operator = (const Transform3f& tf) - { + /// @brief operator = + Transform3f& operator=(const Transform3f& tf) { R = tf.R; T = tf.T; return *this; } /// @brief get translation - inline const Vec3f& getTranslation() const - { - return T; - } - + inline const Vec3f& getTranslation() const { return T; } + /// @brief get translation - inline const Vec3f& translation() const - { - return T; - } - + inline const Vec3f& translation() const { return T; } + /// @brief get translation - inline Vec3f& translation() - { - return T; - } + inline Vec3f& translation() { return T; } /// @brief get rotation - inline const Matrix3f& getRotation() const - { - return R; - } - + inline const Matrix3f& getRotation() const { return R; } + /// @brief get rotation - inline const Matrix3f& rotation() const - { - return R; - } - + inline const Matrix3f& rotation() const { return R; } + /// @brief get rotation - inline Matrix3f& rotation() - { - return R; - } + inline Matrix3f& rotation() { return R; } /// @brief get quaternion - inline Quaternion3f getQuatRotation() const - { - return Quaternion3f (R); - } + inline Quaternion3f getQuatRotation() const { return Quaternion3f(R); } /// @brief set transform from rotation and translation template inline void setTransform(const Eigen::MatrixBase& R_, - const Eigen::MatrixBase& T_) - { + const Eigen::MatrixBase& T_) { R.noalias() = R_; T.noalias() = T_; } /// @brief set transform from rotation and translation - inline void setTransform(const Quaternion3f& q_, const Vec3f& T_) - { + inline void setTransform(const Quaternion3f& q_, const Vec3f& T_) { R = q_.toRotationMatrix(); T = T_; } /// @brief set transform from rotation - template - inline void setRotation(const Eigen::MatrixBase& R_) - { + template + inline void setRotation(const Eigen::MatrixBase& R_) { R.noalias() = R_; } /// @brief set transform from translation - template - inline void setTranslation(const Eigen::MatrixBase& T_) - { + template + inline void setTranslation(const Eigen::MatrixBase& T_) { T.noalias() = T_; } /// @brief set transform from rotation - inline void setQuatRotation(const Quaternion3f& q_) - { + inline void setQuatRotation(const Quaternion3f& q_) { R = q_.toRotationMatrix(); } /// @brief transform a given vector by the transform template - inline Vec3f transform(const Eigen::MatrixBase& v) const - { + inline Vec3f transform(const Eigen::MatrixBase& v) const { return R * v + T; } /// @brief inverse transform - inline Transform3f& inverseInPlace() - { + inline Transform3f& inverseInPlace() { R.transposeInPlace(); - T = - R * T; + T = -R * T; return *this; } /// @brief inverse transform - inline Transform3f inverse() - { - return Transform3f (R.transpose(), - R.transpose() * T); + inline Transform3f inverse() { + return Transform3f(R.transpose(), -R.transpose() * T); } /// @brief inverse the transform and multiply with another - inline Transform3f inverseTimes(const Transform3f& other) const - { + inline Transform3f inverseTimes(const Transform3f& other) const { return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T)); } /// @brief multiply with another transform - inline const Transform3f& operator *= (const Transform3f& other) - { + inline const Transform3f& operator*=(const Transform3f& other) { T += R * other.T; R *= other.R; return *this; } /// @brief multiply with another transform - inline Transform3f operator * (const Transform3f& other) const - { + inline Transform3f operator*(const Transform3f& other) const { return Transform3f(R * other.R, R * other.T + T); } /// @brief check whether the transform is identity - inline bool isIdentity() const - { - return R.isIdentity() && T.isZero(); - } + inline bool isIdentity() const { return R.isIdentity() && T.isZero(); } /// @brief set the transform to be identity transform - inline void setIdentity() - { + inline void setIdentity() { R.setIdentity(); T.setZero(); } - bool operator == (const Transform3f& other) const - { + bool operator==(const Transform3f& other) const { return R == other.R && (T == other.getTranslation()); } - bool operator != (const Transform3f& other) const - { - return !(*this == other); - } - + bool operator!=(const Transform3f& other) const { return !(*this == other); } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -template -inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase& axis, FCL_REAL angle) -{ - return Quaternion3f (Eigen::AngleAxis(angle, axis)); +template +inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase& axis, + FCL_REAL angle) { + return Quaternion3f(Eigen::AngleAxis(angle, axis)); } -} -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/include/hpp/fcl/math/types.h b/include/hpp/fcl/math/types.h index ec431e7a9..ece985db8 100644 --- a/include/hpp/fcl/math/types.h +++ b/include/hpp/fcl/math/types.h @@ -38,9 +38,9 @@ #ifndef HPP_FCL_MATH_TYPES_H #define HPP_FCL_MATH_TYPES_H -# warning "This file is deprecated. Include instead." +#warning "This file is deprecated. Include instead." // List of equivalent includes. -# include +#include #endif \ No newline at end of file diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h index 046e01dc1..4389bba75 100644 --- a/include/hpp/fcl/math/vec_3f.h +++ b/include/hpp/fcl/math/vec_3f.h @@ -38,9 +38,9 @@ #ifndef HPP_FCL_VEC_3F_H #define HPP_FCL_VEC_3F_H -# warning "This file is deprecated. Include instead." +#warning "This file is deprecated. Include instead." // List of equivalent includes. -# include +#include #endif diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/hpp/fcl/mesh_loader/assimp.h index 8b64e70fb..e78d18d05 100644 --- a/include/hpp/fcl/mesh_loader/assimp.h +++ b/include/hpp/fcl/mesh_loader/assimp.h @@ -45,28 +45,24 @@ struct aiScene; namespace Assimp { - class Importer; +class Importer; } -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace internal -{ +namespace internal { -struct HPP_FCL_DLLAPI TriangleAndVertices -{ - std::vector vertices_; - std::vector triangles_; +struct HPP_FCL_DLLAPI TriangleAndVertices { + std::vector vertices_; + std::vector triangles_; }; struct HPP_FCL_DLLAPI Loader { - Loader (); - ~Loader (); + Loader(); + ~Loader(); - void load (const std::string& resource_path); + void load(const std::string& resource_path); Assimp::Importer* importer; aiScene const* scene; @@ -80,10 +76,9 @@ struct HPP_FCL_DLLAPI Loader { * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ -HPP_FCL_DLLAPI void buildMesh (const fcl::Vec3f & scale, - const aiScene* scene, - unsigned vertices_offset, - TriangleAndVertices & tv); +HPP_FCL_DLLAPI void buildMesh(const fcl::Vec3f& scale, const aiScene* scene, + unsigned vertices_offset, + TriangleAndVertices& tv); /** * @brief Convert an assimp scene to a mesh @@ -92,30 +87,27 @@ HPP_FCL_DLLAPI void buildMesh (const fcl::Vec3f & scale, * @param[in] scene Pointer to the assimp scene * @param[out] mesh The mesh that must be built */ -template +template inline void meshFromAssimpScene( - const fcl::Vec3f & scale, - const aiScene* scene, - const shared_ptr < BVHModel > & mesh) -{ + const fcl::Vec3f& scale, const aiScene* scene, + const shared_ptr >& mesh) { TriangleAndVertices tv; - - int res = mesh->beginModel (); - - if (res != fcl::BVH_OK) - { + + int res = mesh->beginModel(); + + if (res != fcl::BVH_OK) { std::ostringstream error; error << "fcl BVHReturnCode = " << res; - throw std::runtime_error (error.str ()); + throw std::runtime_error(error.str()); } - - buildMesh (scale, scene, (unsigned) mesh->num_vertices, tv); - mesh->addSubModel (tv.vertices_, tv.triangles_); - - mesh->endModel (); + + buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv); + mesh->addSubModel(tv.vertices_, tv.triangles_); + + mesh->endModel(); } -} // namespace internal +} // namespace internal /** * @brief Read a mesh file and convert it to a polyhedral mesh @@ -124,18 +116,17 @@ inline void meshFromAssimpScene( * @param[in] scale Scale to apply when reading the ressource * @param[out] polyhedron The resulted polyhedron */ -template -inline void loadPolyhedronFromResource (const std::string & resource_path, - const fcl::Vec3f & scale, - const shared_ptr < BVHModel > & polyhedron) -{ +template +inline void loadPolyhedronFromResource( + const std::string& resource_path, const fcl::Vec3f& scale, + const shared_ptr >& polyhedron) { internal::Loader scene; - scene.load (resource_path); + scene.load(resource_path); - internal::meshFromAssimpScene (scale, scene.scene, polyhedron); + internal::meshFromAssimpScene(scale, scene.scene, polyhedron); } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp -#endif // HPP_FCL_MESH_LOADER_ASSIMP_H +#endif // HPP_FCL_MESH_LOADER_ASSIMP_H diff --git a/include/hpp/fcl/mesh_loader/loader.h b/include/hpp/fcl/mesh_loader/loader.h index 660bfeec7..d2443cc21 100644 --- a/include/hpp/fcl/mesh_loader/loader.h +++ b/include/hpp/fcl/mesh_loader/loader.h @@ -45,60 +45,56 @@ #include -namespace hpp -{ +namespace hpp { namespace fcl { - /// Base class for building polyhedron from files. - /// This class builds a new object for each file. - class HPP_FCL_DLLAPI MeshLoader - { - public: - virtual ~MeshLoader() {} +/// Base class for building polyhedron from files. +/// This class builds a new object for each file. +class HPP_FCL_DLLAPI MeshLoader { + public: + virtual ~MeshLoader() {} - virtual BVHModelPtr_t load (const std::string& filename, - const Vec3f& scale = Vec3f::Ones()); + virtual BVHModelPtr_t load(const std::string& filename, + const Vec3f& scale = Vec3f::Ones()); - /// Create an OcTree from a file in binary octomap format. - /// \todo add OctreePtr_t - virtual CollisionGeometryPtr_t loadOctree (const std::string& filename); + /// Create an OcTree from a file in binary octomap format. + /// \todo add OctreePtr_t + virtual CollisionGeometryPtr_t loadOctree(const std::string& filename); - MeshLoader (const NODE_TYPE& bvType = BV_OBBRSS) : bvType_ (bvType) {} + MeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : bvType_(bvType) {} - private: - const NODE_TYPE bvType_; - }; - - /// Class for building polyhedron from files with cache mechanism. - /// This class builds a new object for each different file. - /// If method CachedMeshLoader::load is called twice with the same arguments, - /// the second call returns the result of the first call. - class HPP_FCL_DLLAPI CachedMeshLoader : public MeshLoader - { - public: - virtual ~CachedMeshLoader() {} + private: + const NODE_TYPE bvType_; +}; - CachedMeshLoader (const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader (bvType) {} +/// Class for building polyhedron from files with cache mechanism. +/// This class builds a new object for each different file. +/// If method CachedMeshLoader::load is called twice with the same arguments, +/// the second call returns the result of the first call. +class HPP_FCL_DLLAPI CachedMeshLoader : public MeshLoader { + public: + virtual ~CachedMeshLoader() {} - virtual BVHModelPtr_t load (const std::string& filename, - const Vec3f& scale); + CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {} - struct HPP_FCL_DLLAPI Key { - std::string filename; - Vec3f scale; + virtual BVHModelPtr_t load(const std::string& filename, const Vec3f& scale); - Key (const std::string& f, const Vec3f& s) - : filename (f), scale (s) {} + struct HPP_FCL_DLLAPI Key { + std::string filename; + Vec3f scale; - bool operator< (const CachedMeshLoader::Key& b) const; - }; - typedef std::map Cache_t; + Key(const std::string& f, const Vec3f& s) : filename(f), scale(s) {} - const Cache_t & cache () const { return cache_; } - private: - Cache_t cache_; + bool operator<(const CachedMeshLoader::Key& b) const; }; -} + typedef std::map Cache_t; + + const Cache_t& cache() const { return cache_; } + + private: + Cache_t cache_; +}; +} // namespace fcl -} // namespace hpp +} // namespace hpp -#endif // FCL_MESH_LOADER_LOADER_H +#endif // FCL_MESH_LOADER_LOADER_H diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index ea4f2f876..1c8068297 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -43,23 +43,20 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace details -{ +namespace details { /// @brief the support function for shape /// \param hint use to initialize the search when shape is a ConvexBase object. -Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint); +Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, + int& hint); /// @brief Minkowski difference class of two shapes /// /// @note The Minkowski difference is expressed in the frame of the first shape. -struct HPP_FCL_DLLAPI MinkowskiDiff -{ +struct HPP_FCL_DLLAPI MinkowskiDiff { /// @brief points to two shapes const ShapeBase* shapes[2]; @@ -89,58 +86,59 @@ struct HPP_FCL_DLLAPI MinkowskiDiff /// \note It must set before the call to \ref set. int linear_log_convex_threshold; - typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff, - const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1, - support_func_guess_t& hint, ShapeData data[2]); + typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff, + const Vec3f& dir, bool dirIsNormalized, + Vec3f& support0, Vec3f& support1, + support_func_guess_t& hint, + ShapeData data[2]); GetSupportFunction getSupportFunc; - MinkowskiDiff() : linear_log_convex_threshold (32), getSupportFunc (NULL) {} + MinkowskiDiff() : linear_log_convex_threshold(32), getSupportFunc(NULL) {} /// Set the two shapes, /// assuming the relative transformation between them is identity. - void set (const ShapeBase* shape0, const ShapeBase* shape1); + void set(const ShapeBase* shape0, const ShapeBase* shape1); /// Set the two shapes, with a relative transformation. - void set (const ShapeBase* shape0, const ShapeBase* shape1, - const Transform3f& tf0, const Transform3f& tf1); + void set(const ShapeBase* shape0, const ShapeBase* shape1, + const Transform3f& tf0, const Transform3f& tf1); /// @brief support function for shape0 - inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const - { + inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const { return getSupport(shapes[0], d, dIsNormalized, hint); } /// @brief support function for shape1 - inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const - { - return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + ot1; + inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const { + return oR1 * + getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + + ot1; } /// @brief support function for the pair of shapes - inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, support_func_guess_t& hint) const - { + inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, + Vec3f& supp1, support_func_guess_t& hint) const { assert(getSupportFunc != NULL); - getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint, const_cast(data)); + getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint, + const_cast(data)); } }; /// @brief class for GJK algorithm /// /// @note The computations are performed in the frame of the first shape. -struct HPP_FCL_DLLAPI GJK -{ - struct HPP_FCL_DLLAPI SimplexV - { +struct HPP_FCL_DLLAPI GJK { + struct HPP_FCL_DLLAPI SimplexV { /// @brief support vector for shape 0 and 1. - Vec3f w0, w1; - /// @brief support vector (i.e., the furthest point on the shape along the support direction) + Vec3f w0, w1; + /// @brief support vector (i.e., the furthest point on the shape along the + /// support direction) Vec3f w; }; typedef unsigned char vertex_id_t; - struct HPP_FCL_DLLAPI Simplex - { + struct HPP_FCL_DLLAPI Simplex { /// @brief simplex vertex SimplexV* vertex[4]; /// @brief size of simplex (number of vertices) @@ -149,7 +147,7 @@ struct HPP_FCL_DLLAPI GJK Simplex() {} }; - enum Status {Valid, Inside, Failed}; + enum Status { Valid, Inside, Failed }; MinkowskiDiff const* shape; Vec3f ray; @@ -170,7 +168,6 @@ struct HPP_FCL_DLLAPI GJK FCL_REAL distance; Simplex simplices[2]; - /// \param max_iterations_ number of iteration before GJK returns failure. /// \param tolerance_ precision of the algorithm. /// @@ -178,22 +175,22 @@ struct HPP_FCL_DLLAPI GJK /// with some vertices closer than this threshold. /// /// Suggested values are 100 iterations and a tolerance of 1e-6. - GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_), - tolerance(tolerance_) - { - initialize(); + GJK(unsigned int max_iterations_, FCL_REAL tolerance_) + : max_iterations(max_iterations_), tolerance(tolerance_) { + initialize(); } - + void initialize(); /// @brief GJK algorithm, given the initial value guess - Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess, + Status evaluate( + const MinkowskiDiff& shape, const Vec3f& guess, const support_func_guess_t& supportHint = support_func_guess_t::Zero()); - /// @brief apply the support function along a direction, the result is return in sv + /// @brief apply the support function along a direction, the result is return + /// in sv inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv, - support_func_guess_t& hint) const - { + support_func_guess_t& hint) const { shape->support(d, dIsNormalized, sv.w0, sv.w1, hint); sv.w.noalias() = sv.w0 - sv.w1; } @@ -201,30 +198,24 @@ struct HPP_FCL_DLLAPI GJK /// @brief whether the simplex enclose the origin bool encloseOrigin(); - /// @brief get the underlying simplex using in GJK, can be used for cache in next iteration - inline Simplex* getSimplex() const - { - return simplex; - } + /// @brief get the underlying simplex using in GJK, can be used for cache in + /// next iteration + inline Simplex* getSimplex() const { return simplex; } /// Tells whether the closest points are available. - bool hasClosestPoints () - { - return distance < distance_upper_bound; - } + bool hasClosestPoints() { return distance < distance_upper_bound; } /// Tells whether the penetration information. /// /// In such case, most indepth points and penetration depth can be retrieved /// from GJK. Calling EPA has an undefined behaviour. - bool hasPenetrationInformation (const MinkowskiDiff& shape) - { - return distance > - shape.inflation.sum(); + bool hasPenetrationInformation(const MinkowskiDiff& shape) { + return distance > -shape.inflation.sum(); } /// Get the closest points on each object. /// @return true on success - bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); + bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); /// @brief get the guess from current simplex Vec3f getGuessFromSimplex() const; @@ -233,12 +224,11 @@ struct HPP_FCL_DLLAPI GJK /// GJK stops when it proved the distance is more than this threshold. /// @note The closest points will be erroneous in this case. /// If you want the closest points, set this to infinity (the default). - void setDistanceEarlyBreak (const FCL_REAL& dup) - { + void setDistanceEarlyBreak(const FCL_REAL& dup) { distance_upper_bound = dup; } -private: + private: SimplexV store_v[4]; SimplexV* free_v[4]; vertex_id_t nfree; @@ -255,7 +245,7 @@ struct HPP_FCL_DLLAPI GJK /// @brief append one vertex to the simplex inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, - support_func_guess_t& hint); + support_func_guess_t& hint); /// @brief Project origin (0) onto line a-b bool projectLineOrigin(const Simplex& current, Simplex& next); @@ -267,86 +257,79 @@ struct HPP_FCL_DLLAPI GJK bool projectTetrahedraOrigin(const Simplex& current, Simplex& next); }; - static const size_t EPA_MAX_FACES = 128; static const size_t EPA_MAX_VERTICES = 64; static const FCL_REAL EPA_EPS = 0.000001; static const size_t EPA_MAX_ITERATIONS = 255; /// @brief class for EPA algorithm -struct HPP_FCL_DLLAPI EPA -{ +struct HPP_FCL_DLLAPI EPA { typedef GJK::SimplexV SimplexV; - struct HPP_FCL_DLLAPI SimplexF - { + struct HPP_FCL_DLLAPI SimplexF { Vec3f n; FCL_REAL d; - SimplexV* vertex[3]; // a face has three vertices - SimplexF* f[3]; // a face has three adjacent faces - SimplexF* l[2]; // the pre and post faces in the list + SimplexV* vertex[3]; // a face has three vertices + SimplexF* f[3]; // a face has three adjacent faces + SimplexF* l[2]; // the pre and post faces in the list size_t e[3]; size_t pass; - SimplexF () : n(Vec3f::Zero()) {}; + SimplexF() : n(Vec3f::Zero()){}; }; - struct HPP_FCL_DLLAPI SimplexList - { + struct HPP_FCL_DLLAPI SimplexList { SimplexF* root; size_t count; SimplexList() : root(NULL), count(0) {} - void append(SimplexF* face) - { + void append(SimplexF* face) { face->l[0] = NULL; face->l[1] = root; - if(root) root->l[0] = face; + if (root) root->l[0] = face; root = face; ++count; } - void remove(SimplexF* face) - { - if(face->l[1]) face->l[1]->l[0] = face->l[0]; - if(face->l[0]) face->l[0]->l[1] = face->l[1]; - if(face == root) root = face->l[1]; + void remove(SimplexF* face) { + if (face->l[1]) face->l[1]->l[0] = face->l[0]; + if (face->l[0]) face->l[0]->l[1] = face->l[1]; + if (face == root) root = face->l[1]; --count; } }; - static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb) - { - fa->e[ea] = eb; fa->f[ea] = fb; - fb->e[eb] = ea; fb->f[eb] = fa; + static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb) { + fa->e[ea] = eb; + fa->f[ea] = fb; + fb->e[eb] = ea; + fb->f[eb] = fa; } - struct HPP_FCL_DLLAPI SimplexHorizon - { - SimplexF* cf; // current face in the horizon - SimplexF* ff; // first face in the horizon - size_t nf; // number of faces in the horizon + struct HPP_FCL_DLLAPI SimplexHorizon { + SimplexF* cf; // current face in the horizon + SimplexF* ff; // first face in the horizon + size_t nf; // number of faces in the horizon SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {} }; -private: + private: unsigned int max_face_num; unsigned int max_vertex_num; unsigned int max_iterations; FCL_REAL tolerance; -public: - + public: enum Status { - Failed = 0, - Valid = 1, - AccuracyReached = 1 << 1 | Valid , - Degenerated = 1 << 1 | Failed, - NonConvex = 2 << 1 | Failed, - InvalidHull = 3 << 1 | Failed, - OutOfFaces = 4 << 1 | Failed, - OutOfVertices = 5 << 1 | Failed, - FallBack = 6 << 1 | Failed + Failed = 0, + Valid = 1, + AccuracyReached = 1 << 1 | Valid, + Degenerated = 1 << 1 | Failed, + NonConvex = 2 << 1 | Failed, + InvalidHull = 3 << 1 | Failed, + OutOfFaces = 4 << 1 | Failed, + OutOfVertices = 5 << 1 | Failed, + FallBack = 6 << 1 | Failed }; - + Status status; GJK::Simplex result; Vec3f normal; @@ -356,18 +339,18 @@ struct HPP_FCL_DLLAPI EPA size_t nextsv; SimplexList hull, stock; - EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_), - max_vertex_num(max_vertex_num_), - max_iterations(max_iterations_), - tolerance(tolerance_) - { + EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, + unsigned int max_iterations_, FCL_REAL tolerance_) + : max_face_num(max_face_num_), + max_vertex_num(max_vertex_num_), + max_iterations(max_iterations_), + tolerance(tolerance_) { initialize(); } - ~EPA() - { - delete [] sv_store; - delete [] fc_store; + ~EPA() { + delete[] sv_store; + delete[] fc_store; } void initialize(); @@ -379,9 +362,9 @@ struct HPP_FCL_DLLAPI EPA /// Get the closest points on each object. /// @return true on success - bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); + bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); -private: + private: bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist); SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex, bool forced); @@ -389,18 +372,15 @@ struct HPP_FCL_DLLAPI EPA /// @brief Find the best polytope face to split SimplexF* findBest(); - /// @brief the goal is to add a face connecting vertex w and face edge f[e] - bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); + /// @brief the goal is to add a face connecting vertex w and face edge f[e] + bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, + SimplexHorizon& horizon); }; +} // namespace details -} // details - - - -} - +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index d92e1c5bd..316f0bbc4 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -44,421 +44,395 @@ #include -namespace hpp -{ -namespace fcl -{ - - /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet) - struct HPP_FCL_DLLAPI GJKSolver - { - /// @brief intersection checking between two shapes - template - bool shapeIntersect(const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - FCL_REAL& distance_lower_bound, - bool enable_penetration, - Vec3f* contact_points, Vec3f* normal) const - { - Vec3f guess(1, 0, 0); - support_func_guess_t support_hint; - if(enable_cached_guess) { - guess = cached_guess; - support_hint = support_func_cached_guess; - } else - support_hint.setZero(); - - details::MinkowskiDiff shape; - shape.set (&s1, &s2, tf1, tf2); - - details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance); - - gjk.setDistanceEarlyBreak(distance_upper_bound); - - details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); - if(enable_cached_guess) { - cached_guess = gjk.getGuessFromSimplex(); - support_func_cached_guess = gjk.support_hint; - } - - Vec3f w0, w1; - switch(gjk_status) { - case details::GJK::Inside: - if (!enable_penetration && contact_points == NULL && normal == NULL) - return true; - if (gjk.hasPenetrationInformation(shape)) { - gjk.getClosestPoints (shape, w0, w1); - distance_lower_bound = gjk.distance; - if(normal) (*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized(); - if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2); - return true; - } else { - details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); - details::EPA::Status epa_status = epa.evaluate(gjk, -guess); - if(epa_status & details::EPA::Valid - || epa_status == details::EPA::OutOfFaces // Warnings - || epa_status == details::EPA::OutOfVertices // Warnings - ) - { - epa.getClosestPoints (shape, w0, w1); - distance_lower_bound = -epa.depth; - if(normal) (*normal).noalias() = tf1.getRotation() * epa.normal; - if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); - return true; - } - distance_lower_bound = -(std::numeric_limits::max)(); - // EPA failed but we know there is a collision so we should - return true; - } - break; - case details::GJK::Valid: - distance_lower_bound = gjk.distance; - break; - default: - ; - } - - return false; +namespace hpp { +namespace fcl { + +/// @brief collision and distance solver based on GJK algorithm implemented in +/// fcl (rewritten the code from the GJK in bullet) +struct HPP_FCL_DLLAPI GJKSolver { + /// @brief intersection checking between two shapes + template + bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, + const Transform3f& tf2, FCL_REAL& distance_lower_bound, + bool enable_penetration, Vec3f* contact_points, + Vec3f* normal) const { + Vec3f guess(1, 0, 0); + support_func_guess_t support_hint; + if (enable_cached_guess) { + guess = cached_guess; + support_hint = support_func_cached_guess; + } else + support_hint.setZero(); + + details::MinkowskiDiff shape; + shape.set(&s1, &s2, tf1, tf2); + + details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); + + gjk.setDistanceEarlyBreak(distance_upper_bound); + + details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); + if (enable_cached_guess) { + cached_guess = gjk.getGuessFromSimplex(); + support_func_cached_guess = gjk.support_hint; } - //// @brief intersection checking between one shape and a triangle with transformation - /// @return true if the shape are colliding. - template - bool shapeTriangleInteraction - (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const - { - bool col; - // Express everything in frame 1 - const Transform3f tf_1M2 (tf1.inverseTimes(tf2)); - TriangleP tri( - tf_1M2.transform (P1), - tf_1M2.transform (P2), - tf_1M2.transform (P3)); - - Vec3f guess(1, 0, 0); - support_func_guess_t support_hint; - if(enable_cached_guess) { - guess = cached_guess; - support_hint = support_func_cached_guess; - } else - support_hint.setZero(); - - details::MinkowskiDiff shape; - shape.set (&s, &tri); - - details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance); - - gjk.setDistanceEarlyBreak(distance_upper_bound); - - details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); - if(enable_cached_guess) { - cached_guess = gjk.getGuessFromSimplex(); - support_func_cached_guess = gjk.support_hint; - } - - Vec3f w0, w1; - switch(gjk_status) { - case details::GJK::Inside: - col = true; - if (gjk.hasPenetrationInformation(shape)) { - gjk.getClosestPoints (shape, w0, w1); - distance = gjk.distance; - normal = tf1.getRotation() * (w0 - w1).normalized(); - p1 = p2 = tf1.transform((w0 + w1) / 2); - } else { - details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); - details::EPA::Status epa_status = epa.evaluate(gjk, -guess); - if(epa_status & details::EPA::Valid - || epa_status == details::EPA::OutOfFaces // Warnings - || epa_status == details::EPA::OutOfVertices // Warnings - ) - { - epa.getClosestPoints (shape, w0, w1); - distance = -epa.depth; - normal.noalias() = tf1.getRotation() * epa.normal; - p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); - assert (distance <= 1e-6); - } else { - distance = -(std::numeric_limits::max)(); - gjk.getClosestPoints (shape, w0, w1); - p1 = p2 = tf1.transform (w0); - } + Vec3f w0, w1; + switch (gjk_status) { + case details::GJK::Inside: + if (!enable_penetration && contact_points == NULL && normal == NULL) + return true; + if (gjk.hasPenetrationInformation(shape)) { + gjk.getClosestPoints(shape, w0, w1); + distance_lower_bound = gjk.distance; + if (normal) + (*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized(); + if (contact_points) *contact_points = tf1.transform((w0 + w1) / 2); + return true; + } else { + details::EPA epa(epa_max_face_num, epa_max_vertex_num, + epa_max_iterations, epa_tolerance); + details::EPA::Status epa_status = epa.evaluate(gjk, -guess); + if (epa_status & details::EPA::Valid || + epa_status == details::EPA::OutOfFaces // Warnings + || epa_status == details::EPA::OutOfVertices // Warnings + ) { + epa.getClosestPoints(shape, w0, w1); + distance_lower_bound = -epa.depth; + if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal; + if (contact_points) + *contact_points = + tf1.transform(w0 - epa.normal * (epa.depth * 0.5)); + return true; } - break; - case details::GJK::Valid: - case details::GJK::Failed: - col = false; - - gjk.getClosestPoints (shape, p1, p2); - // TODO On degenerated case, the closest point may be wrong - // (i.e. an object face normal is colinear to gjk.ray - // assert (distance == (w0 - w1).norm()); - distance = gjk.distance; - - p1 = tf1.transform (p1); - p2 = tf1.transform (p2); - assert (distance > 0); - break; - default: - assert (false && "should not reach type part."); + distance_lower_bound = -(std::numeric_limits::max)(); + // EPA failed but we know there is a collision so we should return true; } - return col; + break; + case details::GJK::Valid: + distance_lower_bound = gjk.distance; + break; + default:; } - /// @brief distance computation between two shapes - template - bool shapeDistance(const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) const - { -#ifndef NDEBUG - FCL_REAL eps (sqrt(std::numeric_limits::epsilon())); -#endif - Vec3f guess(1, 0, 0); - support_func_guess_t support_hint; - if(enable_cached_guess) { - guess = cached_guess; - support_hint = support_func_cached_guess; - } else - support_hint.setZero(); - - details::MinkowskiDiff shape; - shape.set (&s1, &s2, tf1, tf2); - - details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance); - - gjk.setDistanceEarlyBreak(distance_upper_bound); - - details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); - if(enable_cached_guess) { - cached_guess = gjk.getGuessFromSimplex(); - support_func_cached_guess = gjk.support_hint; - } + return false; + } + + //// @brief intersection checking between one shape and a triangle with + /// transformation + /// @return true if the shape are colliding. + template + bool shapeTriangleInteraction(const S& s, const Transform3f& tf1, + const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { + bool col; + // Express everything in frame 1 + const Transform3f tf_1M2(tf1.inverseTimes(tf2)); + TriangleP tri(tf_1M2.transform(P1), tf_1M2.transform(P2), + tf_1M2.transform(P3)); + + Vec3f guess(1, 0, 0); + support_func_guess_t support_hint; + if (enable_cached_guess) { + guess = cached_guess; + support_hint = support_func_cached_guess; + } else + support_hint.setZero(); + + details::MinkowskiDiff shape; + shape.set(&s, &tri); + + details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); + + gjk.setDistanceEarlyBreak(distance_upper_bound); + + details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); + if (enable_cached_guess) { + cached_guess = gjk.getGuessFromSimplex(); + support_func_cached_guess = gjk.support_hint; + } - if(gjk_status == details::GJK::Failed) - { - // TODO: understand why GJK fails between cylinder and box - assert (distance * distance < sqrt (eps)); - Vec3f w0, w1; - gjk.getClosestPoints (shape, w0, w1); - distance = 0; - p1 = tf1.transform (w0); - p2 = tf1.transform (w1); - normal.setZero(); - return false; - } - else if(gjk_status == details::GJK::Valid) - { - gjk.getClosestPoints (shape, p1, p2); - // TODO On degenerated case, the closest point may be wrong - // (i.e. an object face normal is colinear to gjk.ray - // assert (distance == (w0 - w1).norm()); + Vec3f w0, w1; + switch (gjk_status) { + case details::GJK::Inside: + col = true; + if (gjk.hasPenetrationInformation(shape)) { + gjk.getClosestPoints(shape, w0, w1); distance = gjk.distance; - - normal.noalias() = tf1.getRotation() * gjk.ray; - normal.normalize(); - p1 = tf1.transform (p1); - p2 = tf1.transform (p2); - return true; - } - else - { - assert (gjk_status == details::GJK::Inside); - if (gjk.hasPenetrationInformation (shape)) { - gjk.getClosestPoints (shape, p1, p2); - distance = gjk.distance; - // Return contact points in case of collision - //p1 = tf1.transform (p1); - //p2 = tf1.transform (p2); - normal.noalias() = tf1.getRotation() * (p1 - p2); - normal.normalize(); - p1 = tf1.transform(p1); - p2 = tf1.transform(p2); + normal = tf1.getRotation() * (w0 - w1).normalized(); + p1 = p2 = tf1.transform((w0 + w1) / 2); + } else { + details::EPA epa(epa_max_face_num, epa_max_vertex_num, + epa_max_iterations, epa_tolerance); + details::EPA::Status epa_status = epa.evaluate(gjk, -guess); + if (epa_status & details::EPA::Valid || + epa_status == details::EPA::OutOfFaces // Warnings + || epa_status == details::EPA::OutOfVertices // Warnings + ) { + epa.getClosestPoints(shape, w0, w1); + distance = -epa.depth; + normal.noalias() = tf1.getRotation() * epa.normal; + p1 = p2 = tf1.transform(w0 - epa.normal * (epa.depth * 0.5)); + assert(distance <= 1e-6); } else { - details::EPA epa(epa_max_face_num, epa_max_vertex_num, - epa_max_iterations, epa_tolerance); - details::EPA::Status epa_status = epa.evaluate(gjk, -guess); - if(epa_status & details::EPA::Valid - || epa_status == details::EPA::OutOfFaces // Warnings - || epa_status == details::EPA::OutOfVertices // Warnings - ) - { - Vec3f w0, w1; - epa.getClosestPoints (shape, w0, w1); - assert (epa.depth >= -eps); - distance = (std::min) (0., -epa.depth); - normal.noalias() = tf1.getRotation() * epa.normal; - p1 = tf1.transform(w0); - p2 = tf1.transform(w1); - return false; - } distance = -(std::numeric_limits::max)(); - gjk.getClosestPoints (shape, p1, p2); - p1 = tf1.transform(p1); - p2 = tf1.transform(p2); + gjk.getClosestPoints(shape, w0, w1); + p1 = p2 = tf1.transform(w0); } - return false; } + break; + case details::GJK::Valid: + case details::GJK::Failed: + col = false; + + gjk.getClosestPoints(shape, p1, p2); + // TODO On degenerated case, the closest point may be wrong + // (i.e. an object face normal is colinear to gjk.ray + // assert (distance == (w0 - w1).norm()); + distance = gjk.distance; + + p1 = tf1.transform(p1); + p2 = tf1.transform(p2); + assert(distance > 0); + break; + default: + assert(false && "should not reach type part."); + return true; } + return col; + } + + /// @brief distance computation between two shapes + template + bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, + const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, + Vec3f& p2, Vec3f& normal) const { +#ifndef NDEBUG + FCL_REAL eps(sqrt(std::numeric_limits::epsilon())); +#endif + Vec3f guess(1, 0, 0); + support_func_guess_t support_hint; + if (enable_cached_guess) { + guess = cached_guess; + support_hint = support_func_cached_guess; + } else + support_hint.setZero(); - /// @brief default setting for GJK algorithm - GJKSolver() - { - gjk_max_iterations = 128; - gjk_tolerance = 1e-6; - epa_max_face_num = 128; - epa_max_vertex_num = 64; - epa_max_iterations = 255; - epa_tolerance = 1e-6; - enable_cached_guess = false; - cached_guess = Vec3f(1, 0, 0); - support_func_cached_guess = support_func_guess_t::Zero(); - distance_upper_bound = (std::numeric_limits::max)(); - } + details::MinkowskiDiff shape; + shape.set(&s1, &s2, tf1, tf2); - void enableCachedGuess(bool if_enable) const - { - enable_cached_guess = if_enable; - } + details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); - void setCachedGuess(const Vec3f& guess) const - { - cached_guess = guess; - } + gjk.setDistanceEarlyBreak(distance_upper_bound); - Vec3f getCachedGuess() const - { - return cached_guess; - } - - bool operator==(const GJKSolver & other) const - { - return - epa_max_face_num == other.epa_max_face_num - && epa_max_vertex_num == other.epa_max_vertex_num - && epa_max_iterations == other.epa_max_iterations - && epa_tolerance == other.epa_tolerance - && gjk_max_iterations == other.gjk_max_iterations - && enable_cached_guess == other.enable_cached_guess - && cached_guess == other.cached_guess - && support_func_cached_guess == other.support_func_cached_guess - && distance_upper_bound == other.distance_upper_bound - ; - } - - bool operator!=(const GJKSolver & other) const - { - return !(*this == other); + details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); + if (enable_cached_guess) { + cached_guess = gjk.getGuessFromSimplex(); + support_func_cached_guess = gjk.support_hint; } - /// @brief maximum number of simplex face used in EPA algorithm - unsigned int epa_max_face_num; - - /// @brief maximum number of simplex vertex used in EPA algorithm - unsigned int epa_max_vertex_num; - - /// @brief maximum number of iterations used for EPA iterations - unsigned int epa_max_iterations; - - /// @brief the threshold used in EPA to stop iteration - FCL_REAL epa_tolerance; - - /// @brief the threshold used in GJK to stop iteration - FCL_REAL gjk_tolerance; - - /// @brief maximum number of iterations used for GJK iterations - FCL_REAL gjk_max_iterations; - - /// @brief Whether smart guess can be provided - mutable bool enable_cached_guess; - - /// @brief smart guess - mutable Vec3f cached_guess; - - /// @brief smart guess for the support function - mutable support_func_guess_t support_func_cached_guess; - - /// @brief Distance above which the GJK solver stoppes its computations and processes to an early stopping. - /// The two witness points are incorrect, but with the guaranty that the two shapes have a distance greather than distance_upper_bound. - mutable FCL_REAL distance_upper_bound; - - public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - template<> - HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction - (const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const; - - template<> - HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction - (const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const; - - template<> - HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction - (const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const; - -#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1,S2) \ -template<> \ -HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect \ - (const S1 &s1, const Transform3f& tf1, \ - const S2 &s2, const Transform3f& tf2, \ - FCL_REAL& distance_lower_bound, \ - bool, \ - Vec3f* contact_points, Vec3f* normal) const - -#define SHAPE_INTERSECT_SPECIALIZATION(S1,S2) \ - SHAPE_INTERSECT_SPECIALIZATION_BASE(S1,S2); \ - SHAPE_INTERSECT_SPECIALIZATION_BASE(S2,S1) - - SHAPE_INTERSECT_SPECIALIZATION(Sphere,Capsule); - SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere,Sphere); - SHAPE_INTERSECT_SPECIALIZATION(Sphere,Box); - SHAPE_INTERSECT_SPECIALIZATION(Sphere,Halfspace); - SHAPE_INTERSECT_SPECIALIZATION(Sphere,Plane); - - SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Box); - SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Capsule); - SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Cylinder); - SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Cone); - SHAPE_INTERSECT_SPECIALIZATION(Halfspace,Plane); - - SHAPE_INTERSECT_SPECIALIZATION(Plane,Box); - SHAPE_INTERSECT_SPECIALIZATION(Plane,Capsule); - SHAPE_INTERSECT_SPECIALIZATION(Plane,Cylinder); - SHAPE_INTERSECT_SPECIALIZATION(Plane,Cone); + if (gjk_status == details::GJK::Failed) { + // TODO: understand why GJK fails between cylinder and box + assert(distance * distance < sqrt(eps)); + Vec3f w0, w1; + gjk.getClosestPoints(shape, w0, w1); + distance = 0; + p1 = tf1.transform(w0); + p2 = tf1.transform(w1); + normal.setZero(); + return false; + } else if (gjk_status == details::GJK::Valid) { + gjk.getClosestPoints(shape, p1, p2); + // TODO On degenerated case, the closest point may be wrong + // (i.e. an object face normal is colinear to gjk.ray + // assert (distance == (w0 - w1).norm()); + distance = gjk.distance; + + normal.noalias() = tf1.getRotation() * gjk.ray; + normal.normalize(); + p1 = tf1.transform(p1); + p2 = tf1.transform(p2); + return true; + } else { + assert(gjk_status == details::GJK::Inside); + if (gjk.hasPenetrationInformation(shape)) { + gjk.getClosestPoints(shape, p1, p2); + distance = gjk.distance; + // Return contact points in case of collision + // p1 = tf1.transform (p1); + // p2 = tf1.transform (p2); + normal.noalias() = tf1.getRotation() * (p1 - p2); + normal.normalize(); + p1 = tf1.transform(p1); + p2 = tf1.transform(p2); + } else { + details::EPA epa(epa_max_face_num, epa_max_vertex_num, + epa_max_iterations, epa_tolerance); + details::EPA::Status epa_status = epa.evaluate(gjk, -guess); + if (epa_status & details::EPA::Valid || + epa_status == details::EPA::OutOfFaces // Warnings + || epa_status == details::EPA::OutOfVertices // Warnings + ) { + Vec3f w0, w1; + epa.getClosestPoints(shape, w0, w1); + assert(epa.depth >= -eps); + distance = (std::min)(0., -epa.depth); + normal.noalias() = tf1.getRotation() * epa.normal; + p1 = tf1.transform(w0); + p2 = tf1.transform(w1); + return false; + } + distance = -(std::numeric_limits::max)(); + gjk.getClosestPoints(shape, p1, p2); + p1 = tf1.transform(p1); + p2 = tf1.transform(p2); + } + return false; + } + } + + /// @brief default setting for GJK algorithm + GJKSolver() { + gjk_max_iterations = 128; + gjk_tolerance = 1e-6; + epa_max_face_num = 128; + epa_max_vertex_num = 64; + epa_max_iterations = 255; + epa_tolerance = 1e-6; + enable_cached_guess = false; + cached_guess = Vec3f(1, 0, 0); + support_func_cached_guess = support_func_guess_t::Zero(); + distance_upper_bound = (std::numeric_limits::max)(); + } + + void enableCachedGuess(bool if_enable) const { + enable_cached_guess = if_enable; + } + + void setCachedGuess(const Vec3f& guess) const { cached_guess = guess; } + + Vec3f getCachedGuess() const { return cached_guess; } + + bool operator==(const GJKSolver& other) const { + return epa_max_face_num == other.epa_max_face_num && + epa_max_vertex_num == other.epa_max_vertex_num && + epa_max_iterations == other.epa_max_iterations && + epa_tolerance == other.epa_tolerance && + gjk_max_iterations == other.gjk_max_iterations && + enable_cached_guess == other.enable_cached_guess && + cached_guess == other.cached_guess && + support_func_cached_guess == other.support_func_cached_guess && + distance_upper_bound == other.distance_upper_bound; + } + + bool operator!=(const GJKSolver& other) const { return !(*this == other); } + + /// @brief maximum number of simplex face used in EPA algorithm + unsigned int epa_max_face_num; + + /// @brief maximum number of simplex vertex used in EPA algorithm + unsigned int epa_max_vertex_num; + + /// @brief maximum number of iterations used for EPA iterations + unsigned int epa_max_iterations; + + /// @brief the threshold used in EPA to stop iteration + FCL_REAL epa_tolerance; + + /// @brief the threshold used in GJK to stop iteration + FCL_REAL gjk_tolerance; + + /// @brief maximum number of iterations used for GJK iterations + FCL_REAL gjk_max_iterations; + + /// @brief Whether smart guess can be provided + mutable bool enable_cached_guess; + + /// @brief smart guess + mutable Vec3f cached_guess; + + /// @brief smart guess for the support function + mutable support_func_guess_t support_func_cached_guess; + + /// @brief Distance above which the GJK solver stoppes its computations and + /// processes to an early stopping. + /// The two witness points are incorrect, but with the guaranty that + /// the two shapes have a distance greather than distance_upper_bound. + mutable FCL_REAL distance_upper_bound; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +template <> +HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( + const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, + Vec3f& p2, Vec3f& normal) const; + +template <> +HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( + const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, + const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const; + +template <> +HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( + const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, + Vec3f& p2, Vec3f& normal) const; + +#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2) \ + template <> \ + HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect( \ + const S1& s1, const Transform3f& tf1, const S2& s2, \ + const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, \ + Vec3f* contact_points, Vec3f* normal) const + +#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2) \ + SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2); \ + SHAPE_INTERSECT_SPECIALIZATION_BASE(S2, S1) + +SHAPE_INTERSECT_SPECIALIZATION(Sphere, Capsule); +SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere, Sphere); +SHAPE_INTERSECT_SPECIALIZATION(Sphere, Box); +SHAPE_INTERSECT_SPECIALIZATION(Sphere, Halfspace); +SHAPE_INTERSECT_SPECIALIZATION(Sphere, Plane); + +SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Box); +SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Capsule); +SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cylinder); +SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cone); +SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Plane); + +SHAPE_INTERSECT_SPECIALIZATION(Plane, Box); +SHAPE_INTERSECT_SPECIALIZATION(Plane, Capsule); +SHAPE_INTERSECT_SPECIALIZATION(Plane, Cylinder); +SHAPE_INTERSECT_SPECIALIZATION(Plane, Cone); #undef SHAPE_INTERSECT_SPECIALIZATION #undef SHAPE_INTERSECT_SPECIALIZATION_BASE -#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1,S2) \ -template<> \ -HPP_FCL_DLLAPI bool GJKSolver::shapeDistance \ - (const S1& s1, const Transform3f& tf1, \ - const S2& s2, const Transform3f& tf2, \ - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const +#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2) \ + template <> \ + HPP_FCL_DLLAPI bool GJKSolver::shapeDistance( \ + const S1& s1, const Transform3f& tf1, const S2& s2, \ + const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \ + Vec3f& normal) const -#define SHAPE_DISTANCE_SPECIALIZATION(S1,S2) \ - SHAPE_DISTANCE_SPECIALIZATION_BASE(S1,S2); \ - SHAPE_DISTANCE_SPECIALIZATION_BASE(S2,S1) +#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2) \ + SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2); \ + SHAPE_DISTANCE_SPECIALIZATION_BASE(S2, S1) - SHAPE_DISTANCE_SPECIALIZATION(Sphere,Capsule); - SHAPE_DISTANCE_SPECIALIZATION(Sphere,Box); - SHAPE_DISTANCE_SPECIALIZATION(Sphere,Cylinder); - SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere,Sphere); - SHAPE_DISTANCE_SPECIALIZATION_BASE(Capsule,Capsule); - SHAPE_DISTANCE_SPECIALIZATION_BASE(TriangleP,TriangleP); +SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule); +SHAPE_DISTANCE_SPECIALIZATION(Sphere, Box); +SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder); +SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere, Sphere); +SHAPE_DISTANCE_SPECIALIZATION_BASE(Capsule, Capsule); +SHAPE_DISTANCE_SPECIALIZATION_BASE(TriangleP, TriangleP); #undef SHAPE_DISTANCE_SPECIALIZATION #undef SHAPE_DISTANCE_SPECIALIZATION_BASE @@ -467,136 +441,134 @@ HPP_FCL_DLLAPI bool GJKSolver::shapeDistance \ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wc99-extensions" #endif - /// \name Shape intersection specializations - /// \{ +/// \name Shape intersection specializations +/// \{ // param doc is the doxygen detailled description (should be enclosed in /** */ // and contain no dot for some obscure reasons). -#define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \ - /** @brief Fast implementation for Shape1-Shape2 collision. */ \ - doc \ - template<> \ - HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect \ - (const Shape1& s1, const Transform3f& tf1, \ - const Shape2& s2, const Transform3f& tf2, \ - FCL_REAL& distance_lower_bound, bool enable_penetration, \ - Vec3f* contact_points, Vec3f* normal) const -#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \ - HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc) -#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \ - HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc); \ - HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2,Shape1,doc) - - HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,); - - template<> - HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect - (const Box& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& distance_lower_bound, bool enable_penetration, - Vec3f* contact_points, Vec3f* normal) const; - -#ifdef IS_DOXYGEN // for doxygen only - /** \todo currently disabled and to re-enable it, API of function - * \ref obbDisjointAndLowerBoundDistance should be modified. - * */ - template<> - HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect - (const Box& s1, const Transform3f& tf1, - const Box& s2, const Transform3f& tf2, - FCL_REAL& distance_lower_bound, bool enable_penetration, - Vec3f* contact_points, Vec3f* normal) const; +#define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc) \ + /** @brief Fast implementation for Shape1-Shape2 collision. */ \ + doc template <> \ + HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect( \ + const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ + const Transform3f& tf2, FCL_REAL& distance_lower_bound, \ + bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const +#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc) \ + HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape, Shape, doc) +#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc) \ + HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc); \ + HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2, Shape1, doc) + +HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere, ); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule, ); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace, ); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane, ); + +template <> +HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect( + const Box& s1, const Transform3f& tf1, const Sphere& s2, + const Transform3f& tf2, FCL_REAL& distance_lower_bound, + bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const; + +#ifdef IS_DOXYGEN // for doxygen only +/** \todo currently disabled and to re-enable it, API of function + * \ref obbDisjointAndLowerBoundDistance should be modified. + * */ +template <> +HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect( + const Box& s1, const Transform3f& tf1, const Box& s2, + const Transform3f& tf2, FCL_REAL& distance_lower_bound, + bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const; #endif - //HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane,); +// HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace, ); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane, ); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane,); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace, ); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane, ); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane,); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace, ); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane, ); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane,); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace, ); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane, ); - HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace,); +HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace, ); - HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane,); - HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace,); +HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane, ); +HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace, ); #undef HPP_FCL_DECLARE_SHAPE_INTERSECT #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR - /// \} +/// \} - /// \name Shape triangle interaction specializations - /// \{ +/// \name Shape triangle interaction specializations +/// \{ // param doc is the doxygen detailled description (should be enclosed in /** */ // and contain no dot for some obscure reasons). -#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \ - /** @brief Fast implementation for Shape-Triangle interaction. */ \ - doc \ - template<> HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction \ - (const Shape& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, \ - const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, \ - Vec3f& p1, Vec3f& p2, Vec3f& normal) const - - HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,); - HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace,); - HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane,); +#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc) \ + /** @brief Fast implementation for Shape-Triangle interaction. */ \ + doc template <> \ + HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( \ + const Shape& s, const Transform3f& tf1, const Vec3f& P1, \ + const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, \ + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const + +HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere, ); +HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace, ); +HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane, ); #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE - /// \} +/// \} - /// \name Shape distance specializations - /// \{ +/// \name Shape distance specializations +/// \{ // param doc is the doxygen detailled description (should be enclosed in /** */ // and contain no dot for some obscure reasons). -#define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \ - /** @brief Fast implementation for Shape1-Shape2 distance. */ \ - doc \ - template<> \ - bool HPP_FCL_DLLAPI GJKSolver::shapeDistance \ - (const Shape1& s1, const Transform3f& tf1, \ - const Shape2& s2, const Transform3f& tf2, \ - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape,doc) \ - HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape,Shape,doc) -#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1,Shape2,doc) \ - HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc); \ - HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2,Shape1,doc) - - HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,); - HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule,); - HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder,); - HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,); - - HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Capsule, - /** Closest points are based on two line-segments. */ - ); - - HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(TriangleP, - /** Do not run EPA algorithm to compute penetration depth. Use a dedicated method. */ - ); +#define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc) \ + /** @brief Fast implementation for Shape1-Shape2 distance. */ \ + doc template <> \ + bool HPP_FCL_DLLAPI GJKSolver::shapeDistance( \ + const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ + const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \ + Vec3f& normal) const +#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc) \ + HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape, Shape, doc) +#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc) \ + HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc); \ + HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2, Shape1, doc) + +HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box, ); +HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule, ); +HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder, ); +HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere, ); + +HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF( + Capsule, + /** Closest points are based on two line-segments. */ +); + +HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF( + TriangleP, + /** Do not run EPA algorithm to compute penetration depth. Use a dedicated + method. */ +); #undef HPP_FCL_DECLARE_SHAPE_DISTANCE #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR - /// \} +/// \} #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) #pragma GCC diagnostic pop #endif -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/octree.h b/include/hpp/fcl/octree.h index 3f242f060..19d9d1fd8 100644 --- a/include/hpp/fcl/octree.h +++ b/include/hpp/fcl/octree.h @@ -35,11 +35,9 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_OCTREE_H #define HPP_FCL_OCTREE_H - #include #include @@ -47,15 +45,13 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -/// @brief Octree is one type of collision geometry which can encode uncertainty information in the sensor data. -class HPP_FCL_DLLAPI OcTree : public CollisionGeometry -{ -private: +/// @brief Octree is one type of collision geometry which can encode uncertainty +/// information in the sensor data. +class HPP_FCL_DLLAPI OcTree : public CollisionGeometry { + private: shared_ptr tree; FCL_REAL default_occupancy; @@ -63,108 +59,93 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry FCL_REAL occupancy_threshold; FCL_REAL free_threshold; -public: - + public: typedef octomap::OcTreeNode OcTreeNode; /// @brief construct octree with a given resolution explicit OcTree(FCL_REAL resolution) - : tree(shared_ptr(new octomap::OcTree(resolution))) - { + : tree(shared_ptr( + new octomap::OcTree(resolution))) { default_occupancy = tree->getOccupancyThres(); - // default occupancy/free threshold is consistent with default setting from octomap + // default occupancy/free threshold is consistent with default setting from + // octomap occupancy_threshold = tree->getOccupancyThres(); free_threshold = 0; } /// @brief construct octree from octomap explicit OcTree(const shared_ptr& tree_) - : tree(tree_) - { + : tree(tree_) { default_occupancy = tree->getOccupancyThres(); - // default occupancy/free threshold is consistent with default setting from octomap + // default occupancy/free threshold is consistent with default setting from + // octomap occupancy_threshold = tree->getOccupancyThres(); free_threshold = 0; } - - OcTree(const OcTree & other) - : CollisionGeometry(other) - , tree(other.tree) - , default_occupancy(other.default_occupancy) - , occupancy_threshold(other.occupancy_threshold) - , free_threshold(other.free_threshold) - { - } - - OcTree * clone() const { return new OcTree(*this); } + + OcTree(const OcTree& other) + : CollisionGeometry(other), + tree(other.tree), + default_occupancy(other.default_occupancy), + occupancy_threshold(other.occupancy_threshold), + free_threshold(other.free_threshold) {} + + OcTree* clone() const { return new OcTree(*this); } void exportAsObjFile(const std::string& filename) const; /// @brief compute the AABB for the octree in its local coordinate system - void computeLocalAABB() - { + void computeLocalAABB() { aabb_local = getRootBV(); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } /// @brief get the bounding volume for the root - AABB getRootBV() const - { + AABB getRootBV() const { FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; // std::cout << "octree size " << delta << std::endl; return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)); } - + /// @brief Returns the depth of octree - unsigned int getTreeDepth() const - { - return tree->getTreeDepth(); - } + unsigned int getTreeDepth() const { return tree->getTreeDepth(); } /// @brief get the root node of the octree - OcTreeNode* getRoot() const - { - return tree->getRoot(); - } + OcTreeNode* getRoot() const { return tree->getRoot(); } /// @brief whether one node is completely occupied - bool isNodeOccupied(const OcTreeNode* node) const - { + bool isNodeOccupied(const OcTreeNode* node) const { // return tree->isNodeOccupied(node); return node->getOccupancy() >= occupancy_threshold; - } + } /// @brief whether one node is completely free - bool isNodeFree(const OcTreeNode* node) const - { + bool isNodeFree(const OcTreeNode* node) const { // return false; // default no definitely free node return node->getOccupancy() <= free_threshold; } /// @brief whether one node is uncertain - bool isNodeUncertain(const OcTreeNode* node) const - { + bool isNodeUncertain(const OcTreeNode* node) const { return (!isNodeOccupied(node)) && (!isNodeFree(node)); } - /// @brief transform the octree into a bunch of boxes; uncertainty information is kept in the boxes. However, we - /// only keep the occupied boxes (i.e., the boxes whose occupied probability is higher enough). - std::vector > toBoxes() const - { + /// @brief transform the octree into a bunch of boxes; uncertainty information + /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the + /// boxes whose occupied probability is higher enough). + std::vector > toBoxes() const { std::vector > boxes; boxes.reserve(tree->size() / 2); - for(octomap::OcTree::iterator it = - tree->begin((unsigned char) tree->getTreeDepth()), end = tree->end(); - it != end; - ++it) - { + for (octomap::OcTree::iterator + it = tree->begin((unsigned char)tree->getTreeDepth()), + end = tree->end(); + it != end; ++it) { // if(tree->isNodeOccupied(*it)) - if(isNodeOccupied(&*it)) - { + if (isNodeOccupied(&*it)) { FCL_REAL size = it.getSize(); FCL_REAL x = it.getX(); FCL_REAL y = it.getY(); @@ -179,62 +160,44 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry return boxes; } - /// @brief the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold - FCL_REAL getOccupancyThres() const - { - return occupancy_threshold; - } + /// @brief the threshold used to decide whether one node is occupied, this is + /// NOT the octree occupied_thresold + FCL_REAL getOccupancyThres() const { return occupancy_threshold; } - /// @brief the threshold used to decide whether one node is free, this is NOT the octree free_threshold - FCL_REAL getFreeThres() const - { - return free_threshold; - } + /// @brief the threshold used to decide whether one node is free, this is NOT + /// the octree free_threshold + FCL_REAL getFreeThres() const { return free_threshold; } - FCL_REAL getDefaultOccupancy() const - { - return default_occupancy; - } + FCL_REAL getDefaultOccupancy() const { return default_occupancy; } - void setCellDefaultOccupancy(FCL_REAL d) - { - default_occupancy = d; - } + void setCellDefaultOccupancy(FCL_REAL d) { default_occupancy = d; } - void setOccupancyThres(FCL_REAL d) - { - occupancy_threshold = d; - } + void setOccupancyThres(FCL_REAL d) { occupancy_threshold = d; } - void setFreeThres(FCL_REAL d) - { - free_threshold = d; - } + void setFreeThres(FCL_REAL d) { free_threshold = d; } /// @return ptr to child number childIdx of node - OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) - { -#if OCTOMAP_VERSION_AT_LEAST(1,8,0) + OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) { +#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) return tree->getNodeChild(node, childIdx); #else return node->getChild(childIdx); #endif - } + } /// @return const ptr to child number childIdx of node - const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const - { -#if OCTOMAP_VERSION_AT_LEAST(1,8,0) + const OcTreeNode* getNodeChild(const OcTreeNode* node, + unsigned int childIdx) const { +#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) return tree->getNodeChild(node, childIdx); #else return node->getChild(childIdx); #endif - } - + } + /// @brief return true if the child at childIdx exists - bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const - { -#if OCTOMAP_VERSION_AT_LEAST(1,8,0) + bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const { +#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) return tree->nodeChildExists(node, childIdx); #else return node->childExists(childIdx); @@ -242,9 +205,8 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry } /// @brief return true if node has at least one child - bool nodeHasChildren(const OcTreeNode* node) const - { -#if OCTOMAP_VERSION_AT_LEAST(1,8,0) + bool nodeHasChildren(const OcTreeNode* node) const { +#if OCTOMAP_VERSION_AT_LEAST(1, 8, 0) return tree->nodeHasChildren(node); #else return node->hasChildren(); @@ -256,74 +218,65 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry /// @brief return node type, it is an octree NODE_TYPE getNodeType() const { return GEOM_OCTREE; } - -private: - virtual bool isEqual(const CollisionGeometry & _other) const - { - const OcTree * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const OcTree & other = *other_ptr; - - return tree.get() == other.tree.get() - && default_occupancy == other.default_occupancy - && occupancy_threshold == other.occupancy_threshold - && free_threshold == other.free_threshold; + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const OcTree* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const OcTree& other = *other_ptr; + + return tree.get() == other.tree.get() && + default_occupancy == other.default_occupancy && + occupancy_threshold == other.occupancy_threshold && + free_threshold == other.free_threshold; } - -public: + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief compute the bounding volume of an octree node's i-th child -static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv) -{ - if(i&1) - { +static inline void computeChildBV(const AABB& root_bv, unsigned int i, + AABB& child_bv) { + if (i & 1) { child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5; child_bv.max_[0] = root_bv.max_[0]; - } - else - { + } else { child_bv.min_[0] = root_bv.min_[0]; child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5; } - if(i&2) - { + if (i & 2) { child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5; child_bv.max_[1] = root_bv.max_[1]; - } - else - { + } else { child_bv.min_[1] = root_bv.min_[1]; child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5; } - if(i&4) - { + if (i & 4) { child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5; child_bv.max_[2] = root_bv.max_[2]; - } - else - { + } else { child_bv.min_[2] = root_bv.min_[2]; child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5; } } - /// - /// \brief Build an OcTree from a point cloud and a given resolution - /// - /// \param[in] point_cloud The input points to insert in the OcTree - /// \param[in] resolution of the octree. - /// - /// \returns An OcTree that can be used for collision checking and more. - /// - HPP_FCL_DLLAPI OcTreePtr_t makeOctree(const Eigen::Matrix & point_cloud, - const FCL_REAL resolution); - -} // namespace hpp::fcl - -} // namespace hpp +/// +/// \brief Build an OcTree from a point cloud and a given resolution +/// +/// \param[in] point_cloud The input points to insert in the OcTree +/// \param[in] resolution of the octree. +/// +/// \returns An OcTree that can be used for collision checking and more. +/// +HPP_FCL_DLLAPI OcTreePtr_t +makeOctree(const Eigen::Matrix& point_cloud, + const FCL_REAL resolution); + +} // namespace fcl + +} // namespace hpp #endif diff --git a/include/hpp/fcl/serialization/AABB.h b/include/hpp/fcl/serialization/AABB.h index 7fcecab39..85ff64915 100644 --- a/include/hpp/fcl/serialization/AABB.h +++ b/include/hpp/fcl/serialization/AABB.h @@ -8,21 +8,17 @@ #include "hpp/fcl/BV/AABB.h" #include "hpp/fcl/serialization/fwd.h" -namespace boost -{ - namespace serialization - { - - template - void serialize(Archive & ar, - hpp::fcl::AABB & aabb, - const unsigned int /*version*/) - { - ar & make_nvp("min_",aabb.min_); - ar & make_nvp("max_",aabb.max_); - } - - } +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, hpp::fcl::AABB& aabb, + const unsigned int /*version*/) { + ar& make_nvp("min_", aabb.min_); + ar& make_nvp("max_", aabb.max_); } -#endif // ifndef HPP_FCL_SERIALIZATION_AABB_H +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_AABB_H diff --git a/include/hpp/fcl/serialization/BVH_model.h b/include/hpp/fcl/serialization/BVH_model.h index baa447fc6..78fd175c7 100644 --- a/include/hpp/fcl/serialization/BVH_model.h +++ b/include/hpp/fcl/serialization/BVH_model.h @@ -13,324 +13,310 @@ #include "hpp/fcl/serialization/collision_object.h" #include "hpp/fcl/serialization/memory.h" -namespace boost -{ - namespace serialization - { - - namespace internal - { - struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase - { - typedef hpp::fcl::BVHModelBase Base; - using Base::num_tris_allocated; - using Base::num_vertices_allocated; - }; - } - - template - void serialize(Archive & ar, - hpp::fcl::Triangle & triangle, - const unsigned int /*version*/) - { - ar & make_nvp("p0",triangle[0]); - ar & make_nvp("p1",triangle[1]); - ar & make_nvp("p2",triangle[2]); - } - - template - void save(Archive & ar, - const hpp::fcl::BVHModelBase & bvh_model, - const unsigned int /*version*/) - { - using namespace hpp::fcl; - if(!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED || bvh_model.build_state == BVH_BUILD_STATE_UPDATED) - && (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) - { - throw std::invalid_argument("The BVH model is not in a BVH_BUILD_STATE_PROCESSED or BVH_BUILD_STATE_UPDATED state.\n" - "The BVHModel could not be serialized."); - } - - ar & make_nvp("base",boost::serialization::base_object(bvh_model)); - - ar & make_nvp("num_vertices",bvh_model.num_vertices); - if(bvh_model.num_vertices > 0) - { - typedef Eigen::Matrix AsVertixMatrix; - const Eigen::Map vertices_map(reinterpret_cast(bvh_model.vertices),3,bvh_model.num_vertices); - ar & make_nvp("vertices",vertices_map); - } - - ar & make_nvp("num_tris",bvh_model.num_tris); - if(bvh_model.num_tris > 0) - { - typedef Eigen::Matrix AsTriangleMatrix; - const Eigen::Map tri_indices_map(reinterpret_cast(bvh_model.tri_indices),3,bvh_model.num_tris); - ar & make_nvp("tri_indices",tri_indices_map); - } - ar & make_nvp("build_state",bvh_model.build_state); - - if(bvh_model.prev_vertices) - { - const bool has_prev_vertices = true; - ar << make_nvp("has_prev_vertices",has_prev_vertices); - typedef Eigen::Matrix AsVertixMatrix; - const Eigen::Map prev_vertices_map(reinterpret_cast(bvh_model.prev_vertices),3,bvh_model.num_vertices); - ar & make_nvp("prev_vertices",prev_vertices_map); - } - else - { - const bool has_prev_vertices = false; - ar & make_nvp("has_prev_vertices",has_prev_vertices); - } - -// if(bvh_model.convex) -// { -// const bool has_convex = true; -// ar << make_nvp("has_convex",has_convex); -// } -// else -// { -// const bool has_convex = false; -// ar << make_nvp("has_convex",has_convex); -// } - } - - template - void load(Archive & ar, - hpp::fcl::BVHModelBase & bvh_model, - const unsigned int /*version*/) - { - using namespace hpp::fcl; - - ar >> make_nvp("base",boost::serialization::base_object(bvh_model)); - - unsigned int num_vertices; - ar >> make_nvp("num_vertices",num_vertices); - if(num_vertices != bvh_model.num_vertices) - { - delete[] bvh_model.vertices; - bvh_model.vertices = NULL; - bvh_model.num_vertices = num_vertices; - if(num_vertices > 0) - bvh_model.vertices = new Vec3f[num_vertices]; - } - if(num_vertices > 0) - { - typedef Eigen::Matrix AsVertixMatrix; - Eigen::Map vertices_map(reinterpret_cast(bvh_model.vertices),3,bvh_model.num_vertices); - ar >> make_nvp("vertices",vertices_map); - } - else - bvh_model.vertices = NULL; - - unsigned int num_tris; - ar >> make_nvp("num_tris",num_tris); - - if(num_tris != bvh_model.num_tris) - { - delete[] bvh_model.tri_indices; - bvh_model.tri_indices = NULL; - bvh_model.num_tris = num_tris; - if(num_tris> 0) - bvh_model.tri_indices = new Triangle[num_tris]; - } - if(num_tris > 0) - { - typedef Eigen::Matrix AsTriangleMatrix; - Eigen::Map tri_indices_map(reinterpret_cast(bvh_model.tri_indices),3,bvh_model.num_tris); - ar & make_nvp("tri_indices",tri_indices_map); - } - else - bvh_model.tri_indices = NULL; - - ar >> make_nvp("build_state",bvh_model.build_state); - - typedef internal::BVHModelBaseAccessor Accessor; - reinterpret_cast(bvh_model).num_tris_allocated = num_tris; - reinterpret_cast(bvh_model).num_vertices_allocated = num_vertices; - - bool has_prev_vertices; - ar >> make_nvp("has_prev_vertices",has_prev_vertices); - if(has_prev_vertices) - { - if(num_vertices != bvh_model.num_vertices) - { - delete[] bvh_model.prev_vertices; - bvh_model.prev_vertices = NULL; - if(num_vertices > 0) - bvh_model.prev_vertices = new Vec3f[num_vertices]; - } - if(num_vertices > 0) - { - typedef Eigen::Matrix AsVertixMatrix; - Eigen::Map prev_vertices_map(reinterpret_cast(bvh_model.prev_vertices),3,bvh_model.num_vertices); - ar & make_nvp("prev_vertices",prev_vertices_map); - } - } - else - bvh_model.prev_vertices = NULL; - -// bool has_convex = true; -// ar >> make_nvp("has_convex",has_convex); - } - - HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::BVHModelBase) - - namespace internal - { - template - struct BVHModelAccessor : hpp::fcl::BVHModel - { - typedef hpp::fcl::BVHModel Base; - using Base::num_bvs_allocated; - using Base::primitive_indices; - using Base::bvs; - using Base::num_bvs; - }; - } - - template - void serialize(Archive & ar, - hpp::fcl::BVHModel & bvh_model, - const unsigned int version) - { - split_free(ar,bvh_model,version); +namespace boost { +namespace serialization { + +namespace internal { +struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase { + typedef hpp::fcl::BVHModelBase Base; + using Base::num_tris_allocated; + using Base::num_vertices_allocated; +}; +} // namespace internal + +template +void serialize(Archive &ar, hpp::fcl::Triangle &triangle, + const unsigned int /*version*/) { + ar &make_nvp("p0", triangle[0]); + ar &make_nvp("p1", triangle[1]); + ar &make_nvp("p2", triangle[2]); +} + +template +void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model, + const unsigned int /*version*/) { + using namespace hpp::fcl; + if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED || + bvh_model.build_state == BVH_BUILD_STATE_UPDATED) && + (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) { + throw std::invalid_argument( + "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or " + "BVH_BUILD_STATE_UPDATED state.\n" + "The BVHModel could not be serialized."); + } + + ar &make_nvp("base", + boost::serialization::base_object( + bvh_model)); + + ar &make_nvp("num_vertices", bvh_model.num_vertices); + if (bvh_model.num_vertices > 0) { + typedef Eigen::Matrix AsVertixMatrix; + const Eigen::Map vertices_map( + reinterpret_cast(bvh_model.vertices), 3, + bvh_model.num_vertices); + ar &make_nvp("vertices", vertices_map); + } + + ar &make_nvp("num_tris", bvh_model.num_tris); + if (bvh_model.num_tris > 0) { + typedef Eigen::Matrix + AsTriangleMatrix; + const Eigen::Map tri_indices_map( + reinterpret_cast(bvh_model.tri_indices), + 3, bvh_model.num_tris); + ar &make_nvp("tri_indices", tri_indices_map); + } + ar &make_nvp("build_state", bvh_model.build_state); + + if (bvh_model.prev_vertices) { + const bool has_prev_vertices = true; + ar << make_nvp("has_prev_vertices", has_prev_vertices); + typedef Eigen::Matrix AsVertixMatrix; + const Eigen::Map prev_vertices_map( + reinterpret_cast(bvh_model.prev_vertices), 3, + bvh_model.num_vertices); + ar &make_nvp("prev_vertices", prev_vertices_map); + } else { + const bool has_prev_vertices = false; + ar &make_nvp("has_prev_vertices", has_prev_vertices); + } + + // if(bvh_model.convex) + // { + // const bool has_convex = true; + // ar << make_nvp("has_convex",has_convex); + // } + // else + // { + // const bool has_convex = false; + // ar << make_nvp("has_convex",has_convex); + // } +} + +template +void load(Archive &ar, hpp::fcl::BVHModelBase &bvh_model, + const unsigned int /*version*/) { + using namespace hpp::fcl; + + ar >> make_nvp("base", + boost::serialization::base_object( + bvh_model)); + + unsigned int num_vertices; + ar >> make_nvp("num_vertices", num_vertices); + if (num_vertices != bvh_model.num_vertices) { + delete[] bvh_model.vertices; + bvh_model.vertices = NULL; + bvh_model.num_vertices = num_vertices; + if (num_vertices > 0) bvh_model.vertices = new Vec3f[num_vertices]; + } + if (num_vertices > 0) { + typedef Eigen::Matrix AsVertixMatrix; + Eigen::Map vertices_map( + reinterpret_cast(bvh_model.vertices), 3, + bvh_model.num_vertices); + ar >> make_nvp("vertices", vertices_map); + } else + bvh_model.vertices = NULL; + + unsigned int num_tris; + ar >> make_nvp("num_tris", num_tris); + + if (num_tris != bvh_model.num_tris) { + delete[] bvh_model.tri_indices; + bvh_model.tri_indices = NULL; + bvh_model.num_tris = num_tris; + if (num_tris > 0) bvh_model.tri_indices = new Triangle[num_tris]; + } + if (num_tris > 0) { + typedef Eigen::Matrix + AsTriangleMatrix; + Eigen::Map tri_indices_map( + reinterpret_cast(bvh_model.tri_indices), 3, + bvh_model.num_tris); + ar &make_nvp("tri_indices", tri_indices_map); + } else + bvh_model.tri_indices = NULL; + + ar >> make_nvp("build_state", bvh_model.build_state); + + typedef internal::BVHModelBaseAccessor Accessor; + reinterpret_cast(bvh_model).num_tris_allocated = num_tris; + reinterpret_cast(bvh_model).num_vertices_allocated = num_vertices; + + bool has_prev_vertices; + ar >> make_nvp("has_prev_vertices", has_prev_vertices); + if (has_prev_vertices) { + if (num_vertices != bvh_model.num_vertices) { + delete[] bvh_model.prev_vertices; + bvh_model.prev_vertices = NULL; + if (num_vertices > 0) bvh_model.prev_vertices = new Vec3f[num_vertices]; } - - template - void save(Archive & ar, - const hpp::fcl::BVHModel & bvh_model_, - const unsigned int /*version*/) - { - using namespace hpp::fcl; - typedef internal::BVHModelAccessor Accessor; - typedef BVNode Node; - - const Accessor & bvh_model = reinterpret_cast(bvh_model_); - ar & make_nvp("base",boost::serialization::base_object(bvh_model)); - -// if(bvh_model.primitive_indices) -// { -// const bool with_primitive_indices = true; -// ar & make_nvp("with_primitive_indices",with_primitive_indices); -// -// int num_primitives = 0; -// switch(bvh_model.getModelType()) -// { -// case BVH_MODEL_TRIANGLES: -// num_primitives = bvh_model.num_tris; -// break; -// case BVH_MODEL_POINTCLOUD: -// num_primitives = bvh_model.num_vertices; -// break; -// default: -// ; -// } -// -// ar & make_nvp("num_primitives",num_primitives); -// if(num_primitives > 0) -// { -// typedef Eigen::Matrix AsPrimitiveIndexVector; -// const Eigen::Map primitive_indices_map(reinterpret_cast(bvh_model.primitive_indices),1,num_primitives); -// ar & make_nvp("primitive_indices",primitive_indices_map); -//// ar & make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); -// } -// } -// else -// { -// const bool with_primitive_indices = false; -// ar & make_nvp("with_primitive_indices",with_primitive_indices); -// } -// - - if(bvh_model.bvs) - { - const bool with_bvs = true; - ar & make_nvp("with_bvs",with_bvs); - ar & make_nvp("num_bvs",bvh_model.num_bvs); - ar & make_nvp("bvs",make_array(reinterpret_cast(bvh_model.bvs),sizeof(Node)*(std::size_t)bvh_model.num_bvs)); // Assuming BVs are POD. - } - else - { - const bool with_bvs = false; - ar & make_nvp("with_bvs",with_bvs); - } + if (num_vertices > 0) { + typedef Eigen::Matrix AsVertixMatrix; + Eigen::Map prev_vertices_map( + reinterpret_cast(bvh_model.prev_vertices), 3, + bvh_model.num_vertices); + ar &make_nvp("prev_vertices", prev_vertices_map); } - - template - void load(Archive & ar, - hpp::fcl::BVHModel & bvh_model_, - const unsigned int /*version*/) - { - using namespace hpp::fcl; - typedef internal::BVHModelAccessor Accessor; - typedef BVNode Node; - - Accessor & bvh_model = reinterpret_cast(bvh_model_); - - ar >> make_nvp("base",boost::serialization::base_object(bvh_model)); - -// bool with_primitive_indices; -// ar >> make_nvp("with_primitive_indices",with_primitive_indices); -// if(with_primitive_indices) -// { -// int num_primitives; -// ar >> make_nvp("num_primitives",num_primitives); -// -// delete[] bvh_model.primitive_indices; -// if(num_primitives > 0) -// { -// bvh_model.primitive_indices = new unsigned int[num_primitives]; -// ar & make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); -// } -// else -// bvh_model.primitive_indices = NULL; -// } - - bool with_bvs; - ar >> make_nvp("with_bvs",with_bvs); - if(with_bvs) - { - unsigned int num_bvs; - ar >> make_nvp("num_bvs",num_bvs); - - if(num_bvs != bvh_model.num_bvs) - { - delete[] bvh_model.bvs; - bvh_model.bvs = NULL; - bvh_model.num_bvs = num_bvs; - if(num_bvs > 0) - bvh_model.bvs = new BVNode[num_bvs]; - } - if(num_bvs > 0) - { - ar >> make_nvp("bvs",make_array(reinterpret_cast(bvh_model.bvs),sizeof(Node)*(std::size_t)num_bvs)); - } - else - bvh_model.bvs = NULL; - } + } else + bvh_model.prev_vertices = NULL; + + // bool has_convex = true; + // ar >> make_nvp("has_convex",has_convex); +} + +HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::BVHModelBase) + +namespace internal { +template +struct BVHModelAccessor : hpp::fcl::BVHModel { + typedef hpp::fcl::BVHModel Base; + using Base::bvs; + using Base::num_bvs; + using Base::num_bvs_allocated; + using Base::primitive_indices; +}; +} // namespace internal + +template +void serialize(Archive &ar, hpp::fcl::BVHModel &bvh_model, + const unsigned int version) { + split_free(ar, bvh_model, version); +} + +template +void save(Archive &ar, const hpp::fcl::BVHModel &bvh_model_, + const unsigned int /*version*/) { + using namespace hpp::fcl; + typedef internal::BVHModelAccessor Accessor; + typedef BVNode Node; + + const Accessor &bvh_model = reinterpret_cast(bvh_model_); + ar &make_nvp("base", + boost::serialization::base_object(bvh_model)); + + // if(bvh_model.primitive_indices) + // { + // const bool with_primitive_indices = true; + // ar & make_nvp("with_primitive_indices",with_primitive_indices); + // + // int num_primitives = 0; + // switch(bvh_model.getModelType()) + // { + // case BVH_MODEL_TRIANGLES: + // num_primitives = bvh_model.num_tris; + // break; + // case BVH_MODEL_POINTCLOUD: + // num_primitives = bvh_model.num_vertices; + // break; + // default: + // ; + // } + // + // ar & make_nvp("num_primitives",num_primitives); + // if(num_primitives > 0) + // { + // typedef Eigen::Matrix + // AsPrimitiveIndexVector; const Eigen::Map + // primitive_indices_map(reinterpret_cast(bvh_model.primitive_indices),1,num_primitives); ar & + // make_nvp("primitive_indices",primitive_indices_map); + //// ar & + /// make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); + // } + // } + // else + // { + // const bool with_primitive_indices = false; + // ar & make_nvp("with_primitive_indices",with_primitive_indices); + // } + // + + if (bvh_model.bvs) { + const bool with_bvs = true; + ar &make_nvp("with_bvs", with_bvs); + ar &make_nvp("num_bvs", bvh_model.num_bvs); + ar &make_nvp( + "bvs", + make_array( + reinterpret_cast(bvh_model.bvs), + sizeof(Node) * + (std::size_t)bvh_model.num_bvs)); // Assuming BVs are POD. + } else { + const bool with_bvs = false; + ar &make_nvp("with_bvs", with_bvs); + } +} + +template +void load(Archive &ar, hpp::fcl::BVHModel &bvh_model_, + const unsigned int /*version*/) { + using namespace hpp::fcl; + typedef internal::BVHModelAccessor Accessor; + typedef BVNode Node; + + Accessor &bvh_model = reinterpret_cast(bvh_model_); + + ar >> make_nvp("base", + boost::serialization::base_object(bvh_model)); + + // bool with_primitive_indices; + // ar >> make_nvp("with_primitive_indices",with_primitive_indices); + // if(with_primitive_indices) + // { + // int num_primitives; + // ar >> make_nvp("num_primitives",num_primitives); + // + // delete[] bvh_model.primitive_indices; + // if(num_primitives > 0) + // { + // bvh_model.primitive_indices = new unsigned int[num_primitives]; + // ar & + // make_nvp("primitive_indices",make_array(bvh_model.primitive_indices,num_primitives)); + // } + // else + // bvh_model.primitive_indices = NULL; + // } + + bool with_bvs; + ar >> make_nvp("with_bvs", with_bvs); + if (with_bvs) { + unsigned int num_bvs; + ar >> make_nvp("num_bvs", num_bvs); + + if (num_bvs != bvh_model.num_bvs) { + delete[] bvh_model.bvs; + bvh_model.bvs = NULL; + bvh_model.num_bvs = num_bvs; + if (num_bvs > 0) bvh_model.bvs = new BVNode[num_bvs]; } - + if (num_bvs > 0) { + ar >> make_nvp("bvs", make_array(reinterpret_cast(bvh_model.bvs), + sizeof(Node) * (std::size_t)num_bvs)); + } else + bvh_model.bvs = NULL; } } +} // namespace serialization +} // namespace boost + namespace hpp { namespace fcl { -namespace internal -{ - template - struct memory_footprint_evaluator< ::hpp::fcl::BVHModel > - { - static size_t run(const ::hpp::fcl::BVHModel & bvh_model) - { - return static_cast(bvh_model.memUsage(false)); - } - }; -} +namespace internal { +template +struct memory_footprint_evaluator< ::hpp::fcl::BVHModel > { + static size_t run(const ::hpp::fcl::BVHModel &bvh_model) { + return static_cast(bvh_model.memUsage(false)); + } +}; +} // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp -#endif // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H +#endif // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H diff --git a/include/hpp/fcl/serialization/BV_node.h b/include/hpp/fcl/serialization/BV_node.h index 8cedfdec1..78193cda5 100644 --- a/include/hpp/fcl/serialization/BV_node.h +++ b/include/hpp/fcl/serialization/BV_node.h @@ -10,31 +10,26 @@ #include "hpp/fcl/serialization/fwd.h" #include "hpp/fcl/serialization/OBBRSS.h" -namespace boost -{ - namespace serialization - { - - template - void serialize(Archive & ar, - hpp::fcl::BVNodeBase & node, - const unsigned int /*version*/) - { - ar & make_nvp("first_child",node.first_child); - ar & make_nvp("first_primitive",node.first_primitive); - ar & make_nvp("num_primitives",node.num_primitives); - } - - template - void serialize(Archive & ar, - hpp::fcl::BVNode & node, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(node)); - ar & make_nvp("bv",node.bv); - } - - } +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, hpp::fcl::BVNodeBase& node, + const unsigned int /*version*/) { + ar& make_nvp("first_child", node.first_child); + ar& make_nvp("first_primitive", node.first_primitive); + ar& make_nvp("num_primitives", node.num_primitives); +} + +template +void serialize(Archive& ar, hpp::fcl::BVNode& node, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(node)); + ar& make_nvp("bv", node.bv); } -#endif // ifndef HPP_FCL_SERIALIZATION_BV_NODE_H +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_BV_NODE_H diff --git a/include/hpp/fcl/serialization/BV_splitter.h b/include/hpp/fcl/serialization/BV_splitter.h index eb7015bf2..24eac7649 100644 --- a/include/hpp/fcl/serialization/BV_splitter.h +++ b/include/hpp/fcl/serialization/BV_splitter.h @@ -9,62 +9,54 @@ #include "hpp/fcl/serialization/fwd.h" -namespace boost -{ - namespace serialization - { - - namespace internal - { - template - struct BVSplitterAccessor : hpp::fcl::BVSplitter - { - typedef hpp::fcl::BVSplitter Base; - using Base::split_axis; - using Base::split_vector; - using Base::split_value; - using Base::vertices; - using Base::tri_indices; - using Base::type; - using Base::split_method; - }; - } - - template - void save(Archive & ar, - const hpp::fcl::BVSplitter & splitter_, - const unsigned int /*version*/) - { - using namespace hpp::fcl; - typedef internal::BVSplitterAccessor Accessor; - const Accessor & splitter = reinterpret_cast(splitter_); - - ar & make_nvp("split_axis",splitter.split_axis); - ar & make_nvp("split_vector",splitter.split_vector); - ar & make_nvp("split_value",splitter.split_value); - ar & make_nvp("type",splitter.type); - ar & make_nvp("split_method",splitter.split_method); - } - - template - void load(Archive & ar, - hpp::fcl::BVSplitter & splitter_, - const unsigned int /*version*/) - { - using namespace hpp::fcl; - typedef internal::BVSplitterAccessor Accessor; - Accessor & splitter = reinterpret_cast(splitter_); - - ar >> make_nvp("split_axis",splitter.split_axis); - ar >> make_nvp("split_vector",splitter.split_vector); - ar >> make_nvp("split_value",splitter.split_value); - ar >> make_nvp("type",splitter.type); - ar >> make_nvp("split_method",splitter.split_method); - - splitter.vertices = NULL; - splitter.tri_indices = NULL; - } - } +namespace boost { +namespace serialization { + +namespace internal { +template +struct BVSplitterAccessor : hpp::fcl::BVSplitter { + typedef hpp::fcl::BVSplitter Base; + using Base::split_axis; + using Base::split_method; + using Base::split_value; + using Base::split_vector; + using Base::tri_indices; + using Base::type; + using Base::vertices; +}; +} // namespace internal + +template +void save(Archive &ar, const hpp::fcl::BVSplitter &splitter_, + const unsigned int /*version*/) { + using namespace hpp::fcl; + typedef internal::BVSplitterAccessor Accessor; + const Accessor &splitter = reinterpret_cast(splitter_); + + ar &make_nvp("split_axis", splitter.split_axis); + ar &make_nvp("split_vector", splitter.split_vector); + ar &make_nvp("split_value", splitter.split_value); + ar &make_nvp("type", splitter.type); + ar &make_nvp("split_method", splitter.split_method); +} + +template +void load(Archive &ar, hpp::fcl::BVSplitter &splitter_, + const unsigned int /*version*/) { + using namespace hpp::fcl; + typedef internal::BVSplitterAccessor Accessor; + Accessor &splitter = reinterpret_cast(splitter_); + + ar >> make_nvp("split_axis", splitter.split_axis); + ar >> make_nvp("split_vector", splitter.split_vector); + ar >> make_nvp("split_value", splitter.split_value); + ar >> make_nvp("type", splitter.type); + ar >> make_nvp("split_method", splitter.split_method); + + splitter.vertices = NULL; + splitter.tri_indices = NULL; } +} // namespace serialization +} // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H +#endif // ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H diff --git a/include/hpp/fcl/serialization/OBB.h b/include/hpp/fcl/serialization/OBB.h index 7b1ad2a7e..2c07a44e7 100644 --- a/include/hpp/fcl/serialization/OBB.h +++ b/include/hpp/fcl/serialization/OBB.h @@ -9,22 +9,17 @@ #include "hpp/fcl/serialization/fwd.h" -namespace boost -{ - namespace serialization - { - - template - void serialize(Archive & ar, - hpp::fcl::OBB & bv, - const unsigned int /*version*/) - { - ar & make_nvp("axes",bv.axes); - ar & make_nvp("To",bv.To); - ar & make_nvp("extent",bv.extent); - } +namespace boost { +namespace serialization { - } +template +void serialize(Archive& ar, hpp::fcl::OBB& bv, const unsigned int /*version*/) { + ar& make_nvp("axes", bv.axes); + ar& make_nvp("To", bv.To); + ar& make_nvp("extent", bv.extent); } -#endif // ifndef HPP_FCL_SERIALIZATION_OBB_H +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_OBB_H diff --git a/include/hpp/fcl/serialization/OBBRSS.h b/include/hpp/fcl/serialization/OBBRSS.h index b693c84a8..b19ce3a36 100644 --- a/include/hpp/fcl/serialization/OBBRSS.h +++ b/include/hpp/fcl/serialization/OBBRSS.h @@ -11,21 +11,17 @@ #include "hpp/fcl/serialization/OBB.h" #include "hpp/fcl/serialization/RSS.h" -namespace boost -{ - namespace serialization - { - - template - void serialize(Archive & ar, - hpp::fcl::OBBRSS & bv, - const unsigned int /*version*/) - { - ar & make_nvp("obb",bv.obb); - ar & make_nvp("rss",bv.rss); - } +namespace boost { +namespace serialization { - } +template +void serialize(Archive& ar, hpp::fcl::OBBRSS& bv, + const unsigned int /*version*/) { + ar& make_nvp("obb", bv.obb); + ar& make_nvp("rss", bv.rss); } -#endif // ifndef HPP_FCL_SERIALIZATION_OBBRSS_H +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_OBBRSS_H diff --git a/include/hpp/fcl/serialization/RSS.h b/include/hpp/fcl/serialization/RSS.h index ef110c00d..4f1f7ee75 100644 --- a/include/hpp/fcl/serialization/RSS.h +++ b/include/hpp/fcl/serialization/RSS.h @@ -9,23 +9,18 @@ #include "hpp/fcl/serialization/fwd.h" -namespace boost -{ - namespace serialization - { - - template - void serialize(Archive & ar, - hpp::fcl::RSS & bv, - const unsigned int /*version*/) - { - ar & make_nvp("axes",bv.axes); - ar & make_nvp("Tr",bv.Tr); - ar & make_nvp("length",make_array(bv.length,2)); - ar & make_nvp("radius",bv.radius); - } +namespace boost { +namespace serialization { - } +template +void serialize(Archive& ar, hpp::fcl::RSS& bv, const unsigned int /*version*/) { + ar& make_nvp("axes", bv.axes); + ar& make_nvp("Tr", bv.Tr); + ar& make_nvp("length", make_array(bv.length, 2)); + ar& make_nvp("radius", bv.radius); } -#endif // ifndef HPP_FCL_SERIALIZATION_RSS_H +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_RSS_H diff --git a/include/hpp/fcl/serialization/collision_data.h b/include/hpp/fcl/serialization/collision_data.h index e2f840918..83b66fa65 100644 --- a/include/hpp/fcl/serialization/collision_data.h +++ b/include/hpp/fcl/serialization/collision_data.h @@ -8,140 +8,133 @@ #include "hpp/fcl/collision_data.h" #include "hpp/fcl/serialization/fwd.h" -namespace boost -{ - namespace serialization - { - - template - void save(Archive & ar, - const hpp::fcl::Contact & contact, - const unsigned int /*version*/) - { - ar & make_nvp("b1",contact.b1); - ar & make_nvp("b2",contact.b2); - ar & make_nvp("normal",contact.normal); - ar & make_nvp("pos",contact.pos); - ar & make_nvp("penetration_depth",contact.penetration_depth); - } - - template - void load(Archive & ar, - hpp::fcl::Contact & contact, - const unsigned int /*version*/) - { - ar >> make_nvp("b1",contact.b1); - ar >> make_nvp("b2",contact.b2); - ar >> make_nvp("normal",contact.normal); - ar >> make_nvp("pos",contact.pos); - ar >> make_nvp("penetration_depth",contact.penetration_depth); - contact.o1 = NULL; - contact.o2 = NULL; - } - - HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::Contact) - - template - void serialize(Archive & ar, - hpp::fcl::QueryRequest & query_request, - const unsigned int /*version*/) - { - ar & make_nvp("enable_cached_gjk_guess",query_request.enable_cached_gjk_guess); - ar & make_nvp("cached_gjk_guess",query_request.cached_gjk_guess); - ar & make_nvp("cached_support_func_guess",query_request.cached_support_func_guess); - ar & make_nvp("enable_timings",query_request.enable_timings); - } - - template - void serialize(Archive & ar, - hpp::fcl::QueryResult & query_result, - const unsigned int /*version*/) - { - ar & make_nvp("cached_gjk_guess",query_result.cached_gjk_guess); - ar & make_nvp("cached_support_func_guess",query_result.cached_support_func_guess); - } - - template - void serialize(Archive & ar, - hpp::fcl::CollisionRequest & collision_request, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(collision_request)); - ar & make_nvp("num_max_contacts",collision_request.num_max_contacts); - ar & make_nvp("enable_contact",collision_request.enable_contact); - ar & make_nvp("enable_distance_lower_bound",collision_request.enable_distance_lower_bound); - ar & make_nvp("security_margin",collision_request.security_margin); - ar & make_nvp("break_distance",collision_request.break_distance); - } - - template - void save(Archive & ar, - const hpp::fcl::CollisionResult & collision_result, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(collision_result)); - ar & make_nvp("contacts",collision_result.getContacts()); - ar & make_nvp("distance_lower_bound",collision_result.distance_lower_bound); - } - - template - void load(Archive & ar, - hpp::fcl::CollisionResult & collision_result, - const unsigned int /*version*/) - { - ar >> make_nvp("base",boost::serialization::base_object(collision_result)); - std::vector contacts; - ar >> make_nvp("contacts",contacts); - collision_result.clear(); - for(size_t k = 0; k < contacts.size(); ++k) - collision_result.addContact(contacts[k]); - ar >> make_nvp("distance_lower_bound",collision_result.distance_lower_bound); - } - - HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionResult) - - template - void serialize(Archive & ar, - hpp::fcl::DistanceRequest & distance_request, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(distance_request)); - ar & make_nvp("enable_nearest_points",distance_request.enable_nearest_points); - ar & make_nvp("rel_err",distance_request.rel_err); - ar & make_nvp("abs_err",distance_request.abs_err); - } - - template - void save(Archive & ar, - const hpp::fcl::DistanceResult & distance_result, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(distance_result)); - ar & make_nvp("min_distance",distance_result.min_distance); - ar & make_nvp("nearest_points",make_array(distance_result.nearest_points,2)); - ar & make_nvp("normal",distance_result.normal); - ar & make_nvp("b1",distance_result.b1); - ar & make_nvp("b2",distance_result.b2); - } - - template - void load(Archive & ar, - hpp::fcl::DistanceResult & distance_result, - const unsigned int /*version*/) - { - ar >> make_nvp("base",boost::serialization::base_object(distance_result)); - ar >> make_nvp("min_distance",distance_result.min_distance); - ar >> make_nvp("nearest_points",make_array(distance_result.nearest_points,2)); - ar >> make_nvp("normal",distance_result.normal); - ar >> make_nvp("b1",distance_result.b1); - ar >> make_nvp("b2",distance_result.b2); - distance_result.o1 = NULL; - distance_result.o2 = NULL; - } - - HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::DistanceResult) - - } +namespace boost { +namespace serialization { + +template +void save(Archive& ar, const hpp::fcl::Contact& contact, + const unsigned int /*version*/) { + ar& make_nvp("b1", contact.b1); + ar& make_nvp("b2", contact.b2); + ar& make_nvp("normal", contact.normal); + ar& make_nvp("pos", contact.pos); + ar& make_nvp("penetration_depth", contact.penetration_depth); +} + +template +void load(Archive& ar, hpp::fcl::Contact& contact, + const unsigned int /*version*/) { + ar >> make_nvp("b1", contact.b1); + ar >> make_nvp("b2", contact.b2); + ar >> make_nvp("normal", contact.normal); + ar >> make_nvp("pos", contact.pos); + ar >> make_nvp("penetration_depth", contact.penetration_depth); + contact.o1 = NULL; + contact.o2 = NULL; +} + +HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::Contact) + +template +void serialize(Archive& ar, hpp::fcl::QueryRequest& query_request, + const unsigned int /*version*/) { + ar& make_nvp("enable_cached_gjk_guess", + query_request.enable_cached_gjk_guess); + ar& make_nvp("cached_gjk_guess", query_request.cached_gjk_guess); + ar& make_nvp("cached_support_func_guess", + query_request.cached_support_func_guess); + ar& make_nvp("enable_timings", query_request.enable_timings); +} + +template +void serialize(Archive& ar, hpp::fcl::QueryResult& query_result, + const unsigned int /*version*/) { + ar& make_nvp("cached_gjk_guess", query_result.cached_gjk_guess); + ar& make_nvp("cached_support_func_guess", + query_result.cached_support_func_guess); +} + +template +void serialize(Archive& ar, hpp::fcl::CollisionRequest& collision_request, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object( + collision_request)); + ar& make_nvp("num_max_contacts", collision_request.num_max_contacts); + ar& make_nvp("enable_contact", collision_request.enable_contact); + ar& make_nvp("enable_distance_lower_bound", + collision_request.enable_distance_lower_bound); + ar& make_nvp("security_margin", collision_request.security_margin); + ar& make_nvp("break_distance", collision_request.break_distance); +} + +template +void save(Archive& ar, const hpp::fcl::CollisionResult& collision_result, + const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object( + collision_result)); + ar& make_nvp("contacts", collision_result.getContacts()); + ar& make_nvp("distance_lower_bound", collision_result.distance_lower_bound); } -#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_DATA_H +template +void load(Archive& ar, hpp::fcl::CollisionResult& collision_result, + const unsigned int /*version*/) { + ar >> + make_nvp("base", boost::serialization::base_object( + collision_result)); + std::vector contacts; + ar >> make_nvp("contacts", contacts); + collision_result.clear(); + for (size_t k = 0; k < contacts.size(); ++k) + collision_result.addContact(contacts[k]); + ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound); +} + +HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionResult) + +template +void serialize(Archive& ar, hpp::fcl::DistanceRequest& distance_request, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object( + distance_request)); + ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points); + ar& make_nvp("rel_err", distance_request.rel_err); + ar& make_nvp("abs_err", distance_request.abs_err); +} + +template +void save(Archive& ar, const hpp::fcl::DistanceResult& distance_result, + const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object( + distance_result)); + ar& make_nvp("min_distance", distance_result.min_distance); + ar& make_nvp("nearest_points", make_array(distance_result.nearest_points, 2)); + ar& make_nvp("normal", distance_result.normal); + ar& make_nvp("b1", distance_result.b1); + ar& make_nvp("b2", distance_result.b2); +} + +template +void load(Archive& ar, hpp::fcl::DistanceResult& distance_result, + const unsigned int /*version*/) { + ar >> + make_nvp("base", boost::serialization::base_object( + distance_result)); + ar >> make_nvp("min_distance", distance_result.min_distance); + ar >> + make_nvp("nearest_points", make_array(distance_result.nearest_points, 2)); + ar >> make_nvp("normal", distance_result.normal); + ar >> make_nvp("b1", distance_result.b1); + ar >> make_nvp("b2", distance_result.b2); + distance_result.o1 = NULL; + distance_result.o2 = NULL; +} + +HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::DistanceResult) + +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_DATA_H diff --git a/include/hpp/fcl/serialization/collision_object.h b/include/hpp/fcl/serialization/collision_object.h index 17f2f179c..4264b38e3 100644 --- a/include/hpp/fcl/serialization/collision_object.h +++ b/include/hpp/fcl/serialization/collision_object.h @@ -10,41 +10,35 @@ #include "hpp/fcl/serialization/fwd.h" #include "hpp/fcl/serialization/AABB.h" -namespace boost -{ - namespace serialization - { - - template - void save(Archive & ar, - const hpp::fcl::CollisionGeometry & collision_geometry, - const unsigned int /*version*/) - { - ar & make_nvp("aabb_center",collision_geometry.aabb_center); - ar & make_nvp("aabb_radius",collision_geometry.aabb_radius); - ar & make_nvp("aabb_local",collision_geometry.aabb_local); - ar & make_nvp("cost_density",collision_geometry.cost_density); - ar & make_nvp("threshold_occupied",collision_geometry.threshold_occupied); - ar & make_nvp("threshold_free",collision_geometry.threshold_free); - } - - template - void load(Archive & ar, - hpp::fcl::CollisionGeometry & collision_geometry, - const unsigned int /*version*/) - { - ar >> make_nvp("aabb_center",collision_geometry.aabb_center); - ar >> make_nvp("aabb_radius",collision_geometry.aabb_radius); - ar >> make_nvp("aabb_local",collision_geometry.aabb_local); - ar >> make_nvp("cost_density",collision_geometry.cost_density); - ar >> make_nvp("threshold_occupied",collision_geometry.threshold_occupied); - ar >> make_nvp("threshold_free",collision_geometry.threshold_free); - collision_geometry.user_data = NULL; // no way to recover this - } - - HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionGeometry) - - } +namespace boost { +namespace serialization { + +template +void save(Archive& ar, const hpp::fcl::CollisionGeometry& collision_geometry, + const unsigned int /*version*/) { + ar& make_nvp("aabb_center", collision_geometry.aabb_center); + ar& make_nvp("aabb_radius", collision_geometry.aabb_radius); + ar& make_nvp("aabb_local", collision_geometry.aabb_local); + ar& make_nvp("cost_density", collision_geometry.cost_density); + ar& make_nvp("threshold_occupied", collision_geometry.threshold_occupied); + ar& make_nvp("threshold_free", collision_geometry.threshold_free); +} + +template +void load(Archive& ar, hpp::fcl::CollisionGeometry& collision_geometry, + const unsigned int /*version*/) { + ar >> make_nvp("aabb_center", collision_geometry.aabb_center); + ar >> make_nvp("aabb_radius", collision_geometry.aabb_radius); + ar >> make_nvp("aabb_local", collision_geometry.aabb_local); + ar >> make_nvp("cost_density", collision_geometry.cost_density); + ar >> make_nvp("threshold_occupied", collision_geometry.threshold_occupied); + ar >> make_nvp("threshold_free", collision_geometry.threshold_free); + collision_geometry.user_data = NULL; // no way to recover this } -#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H +HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionGeometry) + +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H diff --git a/include/hpp/fcl/serialization/eigen.h b/include/hpp/fcl/serialization/eigen.h index 4fac7b771..2aeb2c2e1 100644 --- a/include/hpp/fcl/serialization/eigen.h +++ b/include/hpp/fcl/serialization/eigen.h @@ -3,7 +3,8 @@ // /* - Code adapted from Pinocchio and https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp + Code adapted from Pinocchio and + https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp Copyright (c) 2015 Michael Tao */ @@ -20,81 +21,95 @@ // ref. https://gitlab.com/libeigen/eigen/-/issues/1676 #ifdef __GNUC__ #if __GNUC__ >= 7 && __cplusplus >= 201703L -namespace boost { namespace serialization { struct U; } } -namespace Eigen { namespace internal { -template<> struct traits {enum {Flags=0};}; -} } +namespace boost { +namespace serialization { +struct U; +} +} // namespace boost +namespace Eigen { +namespace internal { +template <> +struct traits { + enum { Flags = 0 }; +}; +} // namespace internal +} // namespace Eigen #endif #endif +namespace boost { +namespace serialization { -namespace boost -{ - namespace serialization - { - #ifndef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION - - template - void save(Archive & ar, const Eigen::Matrix & m, const unsigned int /*version*/) - { - Eigen::DenseIndex rows(m.rows()), cols(m.cols()); - if (Rows == Eigen::Dynamic) - ar & BOOST_SERIALIZATION_NVP(rows); - if (Cols == Eigen::Dynamic) - ar & BOOST_SERIALIZATION_NVP(cols); - ar & make_nvp("data",make_array(m.data(), (size_t)m.size())); - } - - template - void load(Archive & ar, Eigen::Matrix & m, const unsigned int /*version*/) - { - Eigen::DenseIndex rows = Rows, cols = Cols; - if (Rows == Eigen::Dynamic) - ar >> BOOST_SERIALIZATION_NVP(rows); - if (Cols == Eigen::Dynamic) - ar >> BOOST_SERIALIZATION_NVP(cols); - m.resize(rows,cols); - ar >> make_nvp("data",make_array(m.data(), (size_t)m.size())); - } - - template - void serialize(Archive & ar, Eigen::Matrix & m, const unsigned int version) - { - split_free(ar,m,version); - } - - template - void save(Archive & ar, const Eigen::Map & m, const unsigned int /*version*/) - { - Eigen::DenseIndex rows(m.rows()), cols(m.cols()); - if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) - ar & BOOST_SERIALIZATION_NVP(rows); - if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) - ar & BOOST_SERIALIZATION_NVP(cols); - ar & make_nvp("data",make_array(m.data(), (size_t)m.size())); - } - - template - void load(Archive & ar, Eigen::Map & m, const unsigned int /*version*/) - { - Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime, cols = PlainObjectBase::ColsAtCompileTime; - if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) - ar >> BOOST_SERIALIZATION_NVP(rows); - if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) - ar >> BOOST_SERIALIZATION_NVP(cols); - m.resize(rows,cols); - ar >> make_nvp("data",make_array(m.data(), (size_t)m.size())); - } - - template - void serialize(Archive & ar, Eigen::Map & m, const unsigned int version) - { - split_free(ar,m,version); - } - -#endif // ifned HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION - } + +template +void save(Archive& ar, + const Eigen::Matrix& m, + const unsigned int /*version*/) { + Eigen::DenseIndex rows(m.rows()), cols(m.cols()); + if (Rows == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows); + if (Cols == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(cols); + ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); +} + +template +void load(Archive& ar, + Eigen::Matrix& m, + const unsigned int /*version*/) { + Eigen::DenseIndex rows = Rows, cols = Cols; + if (Rows == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows); + if (Cols == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(cols); + m.resize(rows, cols); + ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); } -#endif // ifndef HPP_FCL_SERIALIZATION_EIGEN_H +template +void serialize(Archive& ar, + Eigen::Matrix& m, + const unsigned int version) { + split_free(ar, m, version); +} + +template +void save(Archive& ar, + const Eigen::Map& m, + const unsigned int /*version*/) { + Eigen::DenseIndex rows(m.rows()), cols(m.cols()); + if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) + ar& BOOST_SERIALIZATION_NVP(rows); + if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) + ar& BOOST_SERIALIZATION_NVP(cols); + ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); +} + +template +void load(Archive& ar, Eigen::Map& m, + const unsigned int /*version*/) { + Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime, + cols = PlainObjectBase::ColsAtCompileTime; + if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic) + ar >> BOOST_SERIALIZATION_NVP(rows); + if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic) + ar >> BOOST_SERIALIZATION_NVP(cols); + m.resize(rows, cols); + ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); +} + +template +void serialize(Archive& ar, + Eigen::Map& m, + const unsigned int version) { + split_free(ar, m, version); +} + +#endif // ifned HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_EIGEN_H diff --git a/include/hpp/fcl/serialization/fwd.h b/include/hpp/fcl/serialization/fwd.h index 66d762984..51ef14908 100644 --- a/include/hpp/fcl/serialization/fwd.h +++ b/include/hpp/fcl/serialization/fwd.h @@ -10,13 +10,10 @@ #include "hpp/fcl/serialization/eigen.h" -#define HPP_FCL_SERIALIZATION_SPLIT(Type) \ - template \ - void serialize(Archive & ar, \ - Type & value, \ - const unsigned int version) \ - { \ - split_free(ar,value,version); \ +#define HPP_FCL_SERIALIZATION_SPLIT(Type) \ + template \ + void serialize(Archive& ar, Type& value, const unsigned int version) { \ + split_free(ar, value, version); \ } -#endif // ifndef HPP_FCL_SERIALIZATION_FWD_H +#endif // ifndef HPP_FCL_SERIALIZATION_FWD_H diff --git a/include/hpp/fcl/serialization/geometric_shapes.h b/include/hpp/fcl/serialization/geometric_shapes.h index 03b0dc96e..0459c5f60 100644 --- a/include/hpp/fcl/serialization/geometric_shapes.h +++ b/include/hpp/fcl/serialization/geometric_shapes.h @@ -8,108 +8,97 @@ #include "hpp/fcl/shape/geometric_shapes.h" #include "hpp/fcl/serialization/fwd.h" -namespace boost -{ - namespace serialization - { - - template - void serialize(Archive & ar, - hpp::fcl::ShapeBase & shape_base, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(shape_base)); - } - - template - void serialize(Archive & ar, - hpp::fcl::TriangleP & triangle, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(triangle)); - ar & make_nvp("a",triangle.a); - ar & make_nvp("b",triangle.b); - ar & make_nvp("c",triangle.c); - } - - template - void serialize(Archive & ar, - hpp::fcl::Box & box, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(box)); - ar & make_nvp("halfSide",box.halfSide); - } - - template - void serialize(Archive & ar, - hpp::fcl::Sphere & sphere, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(sphere)); - ar & make_nvp("radius",sphere.radius); - } +namespace boost { +namespace serialization { - template - void serialize(Archive & ar, - hpp::fcl::Ellipsoid & ellipsoid, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(ellipsoid)); - ar & make_nvp("radii",ellipsoid.radii); - } - - template - void serialize(Archive & ar, - hpp::fcl::Capsule & capsule, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(capsule)); - ar & make_nvp("radius",capsule.radius); - ar & make_nvp("halfLength",capsule.halfLength); - } - - template - void serialize(Archive & ar, - hpp::fcl::Cone & cone, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(cone)); - ar & make_nvp("radius",cone.radius); - ar & make_nvp("halfLength",cone.halfLength); - } - - template - void serialize(Archive & ar, - hpp::fcl::Cylinder & cylinder, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(cylinder)); - ar & make_nvp("radius",cylinder.radius); - ar & make_nvp("halfLength",cylinder.halfLength); - } - - template - void serialize(Archive & ar, - hpp::fcl::Halfspace & half_space, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(half_space)); - ar & make_nvp("n",half_space.n); - ar & make_nvp("d",half_space.d); - } - - template - void serialize(Archive & ar, - hpp::fcl::Plane & plane, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(plane)); - ar & make_nvp("n",plane.n); - ar & make_nvp("d",plane.d); - } +template +void serialize(Archive& ar, hpp::fcl::ShapeBase& shape_base, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object( + shape_base)); +} + +template +void serialize(Archive& ar, hpp::fcl::TriangleP& triangle, + const unsigned int /*version*/) { + ar& make_nvp( + "base", boost::serialization::base_object(triangle)); + ar& make_nvp("a", triangle.a); + ar& make_nvp("b", triangle.b); + ar& make_nvp("c", triangle.c); +} + +template +void serialize(Archive& ar, hpp::fcl::Box& box, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(box)); + ar& make_nvp("halfSide", box.halfSide); +} + +template +void serialize(Archive& ar, hpp::fcl::Sphere& sphere, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(sphere)); + ar& make_nvp("radius", sphere.radius); +} + +template +void serialize(Archive& ar, hpp::fcl::Ellipsoid& ellipsoid, + const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object( + ellipsoid)); + ar& make_nvp("radii", ellipsoid.radii); +} - } +template +void serialize(Archive& ar, hpp::fcl::Capsule& capsule, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(capsule)); + ar& make_nvp("radius", capsule.radius); + ar& make_nvp("halfLength", capsule.halfLength); } -#endif // ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H +template +void serialize(Archive& ar, hpp::fcl::Cone& cone, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(cone)); + ar& make_nvp("radius", cone.radius); + ar& make_nvp("halfLength", cone.halfLength); +} + +template +void serialize(Archive& ar, hpp::fcl::Cylinder& cylinder, + const unsigned int /*version*/) { + ar& make_nvp( + "base", boost::serialization::base_object(cylinder)); + ar& make_nvp("radius", cylinder.radius); + ar& make_nvp("halfLength", cylinder.halfLength); +} + +template +void serialize(Archive& ar, hpp::fcl::Halfspace& half_space, + const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object( + half_space)); + ar& make_nvp("n", half_space.n); + ar& make_nvp("d", half_space.d); +} + +template +void serialize(Archive& ar, hpp::fcl::Plane& plane, + const unsigned int /*version*/) { + ar& make_nvp("base", + boost::serialization::base_object(plane)); + ar& make_nvp("n", plane.n); + ar& make_nvp("d", plane.d); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H diff --git a/include/hpp/fcl/serialization/hfield.h b/include/hpp/fcl/serialization/hfield.h index 83f833ad9..c88f63abd 100644 --- a/include/hpp/fcl/serialization/hfield.h +++ b/include/hpp/fcl/serialization/hfield.h @@ -10,73 +10,65 @@ #include "hpp/fcl/serialization/fwd.h" #include "hpp/fcl/serialization/OBBRSS.h" -namespace boost -{ - namespace serialization - { - - template - void serialize(Archive & ar, - hpp::fcl::HFNodeBase & node, - const unsigned int /*version*/) - { - ar & make_nvp("first_child",node.first_child); - ar & make_nvp("x_id",node.x_id); - ar & make_nvp("x_size",node.x_size); - ar & make_nvp("y_id",node.y_id); - ar & make_nvp("y_size",node.y_size); - } - - template - void serialize(Archive & ar, - hpp::fcl::HFNode & node, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(node)); - ar & make_nvp("bv",node.bv); - } - - namespace internal - { - template - struct HeightFieldAccessor : hpp::fcl::HeightField - { - typedef hpp::fcl::HeightField Base; - using Base::bvs; - using Base::num_bvs; - using Base::x_dim; - using Base::y_dim; - using Base::x_grid; - using Base::y_grid; - using Base::heights; - using Base::min_height; - using Base::max_height; - }; - } - - template - void serialize(Archive & ar, - hpp::fcl::HeightField & hf_model, - const unsigned int /*version*/) - { - ar & make_nvp("base",boost::serialization::base_object(hf_model)); - - - typedef internal::HeightFieldAccessor Accessor; - Accessor & access = reinterpret_cast(hf_model); - - ar & make_nvp("x_dim",access.x_dim); - ar & make_nvp("y_dim",access.y_dim); - ar & make_nvp("heights",access.heights); - ar & make_nvp("min_height",access.min_height); - ar & make_nvp("max_height",access.max_height); - ar & make_nvp("x_grid",access.x_grid); - ar & make_nvp("y_grid",access.y_grid); +namespace boost { +namespace serialization { - ar & make_nvp("bvs",access.bvs); - ar & make_nvp("num_bvs",access.num_bvs); - } - } +template +void serialize(Archive &ar, hpp::fcl::HFNodeBase &node, + const unsigned int /*version*/) { + ar &make_nvp("first_child", node.first_child); + ar &make_nvp("x_id", node.x_id); + ar &make_nvp("x_size", node.x_size); + ar &make_nvp("y_id", node.y_id); + ar &make_nvp("y_size", node.y_size); } -#endif // ifndef HPP_FCL_SERIALIZATION_HFIELD_H +template +void serialize(Archive &ar, hpp::fcl::HFNode &node, + const unsigned int /*version*/) { + ar &make_nvp("base", + boost::serialization::base_object(node)); + ar &make_nvp("bv", node.bv); +} + +namespace internal { +template +struct HeightFieldAccessor : hpp::fcl::HeightField { + typedef hpp::fcl::HeightField Base; + using Base::bvs; + using Base::heights; + using Base::max_height; + using Base::min_height; + using Base::num_bvs; + using Base::x_dim; + using Base::x_grid; + using Base::y_dim; + using Base::y_grid; +}; +} // namespace internal + +template +void serialize(Archive &ar, hpp::fcl::HeightField &hf_model, + const unsigned int /*version*/) { + ar &make_nvp( + "base", + boost::serialization::base_object(hf_model)); + + typedef internal::HeightFieldAccessor Accessor; + Accessor &access = reinterpret_cast(hf_model); + + ar &make_nvp("x_dim", access.x_dim); + ar &make_nvp("y_dim", access.y_dim); + ar &make_nvp("heights", access.heights); + ar &make_nvp("min_height", access.min_height); + ar &make_nvp("max_height", access.max_height); + ar &make_nvp("x_grid", access.x_grid); + ar &make_nvp("y_grid", access.y_grid); + + ar &make_nvp("bvs", access.bvs); + ar &make_nvp("num_bvs", access.num_bvs); +} +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_HFIELD_H diff --git a/include/hpp/fcl/serialization/memory.h b/include/hpp/fcl/serialization/memory.h index 6bde9e67a..e1e3d320f 100644 --- a/include/hpp/fcl/serialization/memory.h +++ b/include/hpp/fcl/serialization/memory.h @@ -8,28 +8,25 @@ namespace hpp { namespace fcl { -namespace internal -{ - template - struct memory_footprint_evaluator - { - static size_t run(const T &) { return sizeof(T); } - }; -} +namespace internal { +template +struct memory_footprint_evaluator { + static size_t run(const T &) { return sizeof(T); } +}; +} // namespace internal - /// \brief Returns the memory footpring of the input object. - /// For POD objects, this function returns the result of sizeof(T) - /// - /// \param[in] object whose memory footprint needs to be evaluated. - /// - /// \return the memory footprint of the input object. - template - size_t computeMemoryFootprint(const T & object) - { - return internal::memory_footprint_evaluator::run(object); - } +/// \brief Returns the memory footpring of the input object. +/// For POD objects, this function returns the result of sizeof(T) +/// +/// \param[in] object whose memory footprint needs to be evaluated. +/// +/// \return the memory footprint of the input object. +template +size_t computeMemoryFootprint(const T &object) { + return internal::memory_footprint_evaluator::run(object); +} -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp -#endif // ifndef HPP_FCL_SERIALIZATION_MEMORY_H +#endif // ifndef HPP_FCL_SERIALIZATION_MEMORY_H diff --git a/include/hpp/fcl/shape/convex.h b/include/hpp/fcl/shape/convex.h index 4d5ed1a16..e39415e48 100644 --- a/include/hpp/fcl/shape/convex.h +++ b/include/hpp/fcl/shape/convex.h @@ -35,40 +35,36 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_SHAPE_CONVEX_H #define HPP_FCL_SHAPE_CONVEX_H #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Convex polytope /// @tparam PolygonT the polygon class. It must have method \c size() and /// \c operator[](int i) template -class Convex : public ConvexBase -{ -public: +class Convex : public ConvexBase { + public: /// @brief Construct an uninitialized convex object - Convex () : ConvexBase(), polygons(NULL), num_polygons(0) {} + Convex() : ConvexBase(), polygons(NULL), num_polygons(0) {} - /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information - /// \param ownStorage whether this class owns the pointers of points and + /// @brief Constructing a convex, providing normal and offset of each polytype + /// surface, and the points and shape topology information \param ownStorage + /// whether this class owns the pointers of points and /// polygons. If owned, they are deleted upon destruction. /// \param points_ list of 3D points /// \param num_points_ number of 3D points /// \param polygons_ \copydoc Convex::polygons /// \param num_polygons_ the number of polygons. /// \note num_polygons_ is not the allocated size of polygons_. - Convex(bool ownStorage, - Vec3f* points_, unsigned int num_points_, + Convex(bool ownStorage, Vec3f* points_, unsigned int num_points_, PolygonT* polygons_, unsigned int num_polygons_); - /// @brief Copy constructor + /// @brief Copy constructor /// Only the list of neighbors is copied. Convex(const Convex& other); @@ -86,7 +82,7 @@ class Convex : public ConvexBase Vec3f computeCOM() const; FCL_REAL computeVolume() const; - + /// /// @brief Set the current Convex from a list of points and polygons. /// @@ -98,20 +94,19 @@ class Convex : public ConvexBase /// \param num_polygons the number of polygons. /// \note num_polygons is not the allocated size of polygons. /// - void set(bool ownStorage, - Vec3f* points, unsigned int num_points, + void set(bool ownStorage, Vec3f* points, unsigned int num_points, PolygonT* polygons, unsigned int num_polygons); - + /// @brief Clone (deep copy). - virtual Convex * clone() const; + virtual Convex* clone() const; -protected: + protected: void fillNeighbors(); }; -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #include diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx index 2ec3dd9e0..8edac251b 100644 --- a/include/hpp/fcl/shape/details/convex.hxx +++ b/include/hpp/fcl/shape/details/convex.hxx @@ -34,37 +34,29 @@ /** \author Joseph Mirabel */ - #ifndef HPP_FCL_SHAPE_CONVEX_HXX #define HPP_FCL_SHAPE_CONVEX_HXX #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { template Convex::Convex(bool own_storage, Vec3f* points_, - unsigned int num_points_, - PolygonT* polygons_, + unsigned int num_points_, PolygonT* polygons_, unsigned int num_polygons_) -: ConvexBase() -, polygons (polygons_) -, num_polygons (num_polygons_) -{ + : ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) { initialize(own_storage, points_, num_points_); fillNeighbors(); } template -Convex::Convex(const Convex& other) : - ConvexBase (other), - polygons (other.polygons), - num_polygons (other.num_polygons) -{ +Convex::Convex(const Convex& other) + : ConvexBase(other), + polygons(other.polygons), + num_polygons(other.num_polygons) { if (own_storage_) { polygons = new PolygonT[num_polygons]; std::copy(other.polygons, other.polygons + num_polygons, polygons); @@ -72,75 +64,70 @@ Convex::Convex(const Convex& other) : } template -Convex::~Convex() -{ - if (own_storage_) delete [] polygons; +Convex::~Convex() { + if (own_storage_) delete[] polygons; } template -void Convex::set(bool own_storage, - Vec3f* points_, - unsigned int num_points_, - PolygonT* polygons_, - unsigned int num_polygons_) -{ - if (own_storage_) - delete [] polygons; - ConvexBase::set(own_storage,points_,num_points_); +void Convex::set(bool own_storage, Vec3f* points_, + unsigned int num_points_, PolygonT* polygons_, + unsigned int num_polygons_) { + if (own_storage_) delete[] polygons; + ConvexBase::set(own_storage, points_, num_points_); num_polygons = num_polygons_; polygons = polygons_; - + fillNeighbors(); } template -Convex * Convex::clone() const -{ - Vec3f * cloned_points = new Vec3f[num_points]; +Convex* Convex::clone() const { + Vec3f* cloned_points = new Vec3f[num_points]; std::copy(points, points + num_points, cloned_points); - - PolygonT * cloned_polygons = new PolygonT[num_polygons]; + + PolygonT* cloned_polygons = new PolygonT[num_polygons]; std::copy(polygons, polygons + num_polygons, cloned_polygons); - - Convex * copy_ptr = new Convex(true, cloned_points, num_points, cloned_polygons, num_polygons); - + + Convex* copy_ptr = new Convex(true, cloned_points, num_points, + cloned_polygons, num_polygons); + copy_ptr->ShapeBase::operator=(*this); return copy_ptr; } template -Matrix3f Convex::computeMomentofInertia() const -{ +Matrix3f Convex::computeMomentofInertia() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::index_type index_type; - + Matrix3f C = Matrix3f::Zero(); Matrix3f C_canonical; - C_canonical << 1/60.0, 1/120.0, 1/120.0, - 1/120.0, 1/60.0, 1/120.0, - 1/120.0, 1/120.0, 1/60.0; + C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0, + 1 / 120.0, 1 / 120.0, 1 / 60.0; - for(unsigned int i = 0; i < num_polygons; ++i) - { - const PolygonT & polygon = polygons[i]; + for (unsigned int i = 0; i < num_polygons; ++i) { + const PolygonT& polygon = polygons[i]; // compute the center of the polygon - Vec3f plane_center(0,0,0); - for(size_type j = 0; j < polygon.size(); ++j) + Vec3f plane_center(0, 0, 0); + for (size_type j = 0; j < polygon.size(); ++j) plane_center += points[polygon[(index_type)j]]; plane_center /= polygon.size(); - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape + // compute the volume of tetrahedron making by neighboring two points, the + // plane center and the reference point (zero) of the convex shape const Vec3f& v3 = plane_center; - for(size_type j = 0; j < polygon.size(); ++j) - { + for (size_type j = 0; j < polygon.size(); ++j) { index_type e_first = polygon[static_cast(j)]; - index_type e_second = polygon[static_cast((j+1)%polygon.size())]; + index_type e_second = + polygon[static_cast((j + 1) % polygon.size())]; const Vec3f& v1 = points[e_first]; const Vec3f& v2 = points[e_second]; - Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document + Matrix3f A; + A << v1.transpose(), v2.transpose(), + v3.transpose(); // this is A' in the original document C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); } } @@ -149,28 +136,27 @@ Matrix3f Convex::computeMomentofInertia() const } template -Vec3f Convex::computeCOM() const -{ +Vec3f Convex::computeCOM() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::index_type index_type; - Vec3f com(0,0,0); + Vec3f com(0, 0, 0); FCL_REAL vol = 0; - for(unsigned int i = 0; i < num_polygons; ++i) - { - const PolygonT & polygon = polygons[i]; + for (unsigned int i = 0; i < num_polygons; ++i) { + const PolygonT& polygon = polygons[i]; // compute the center of the polygon - Vec3f plane_center(0,0,0); - for(size_type j = 0; j < polygon.size(); ++j) + Vec3f plane_center(0, 0, 0); + for (size_type j = 0; j < polygon.size(); ++j) plane_center += points[polygon[(index_type)j]]; plane_center /= polygon.size(); - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape + // compute the volume of tetrahedron making by neighboring two points, the + // plane center and the reference point (zero) of the convex shape const Vec3f& v3 = plane_center; - for(size_type j = 0; j < polygon.size(); ++j) - { + for (size_type j = 0; j < polygon.size(); ++j) { index_type e_first = polygon[static_cast(j)]; - index_type e_second = polygon[static_cast((j+1)%polygon.size())]; + index_type e_second = + polygon[static_cast((j + 1) % polygon.size())]; const Vec3f& v1 = points[e_first]; const Vec3f& v2 = points[e_second]; FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); @@ -179,32 +165,31 @@ Vec3f Convex::computeCOM() const } } - return com / (vol * 4); // here we choose zero as the reference + return com / (vol * 4); // here we choose zero as the reference } template -FCL_REAL Convex::computeVolume() const -{ +FCL_REAL Convex::computeVolume() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::index_type index_type; FCL_REAL vol = 0; - for(unsigned int i = 0; i < num_polygons; ++i) - { - const PolygonT & polygon = polygons[i]; + for (unsigned int i = 0; i < num_polygons; ++i) { + const PolygonT& polygon = polygons[i]; // compute the center of the polygon - Vec3f plane_center(0,0,0); - for(size_type j = 0; j < polygon.size(); ++j) + Vec3f plane_center(0, 0, 0); + for (size_type j = 0; j < polygon.size(); ++j) plane_center += points[polygon[(index_type)j]]; plane_center /= polygon.size(); - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape + // compute the volume of tetrahedron making by neighboring two points, the + // plane center and the reference point (zero point) of the convex shape const Vec3f& v3 = plane_center; - for(size_type j = 0; j < polygon.size(); ++j) - { + for (size_type j = 0; j < polygon.size(); ++j) { index_type e_first = polygon[static_cast(j)]; - index_type e_second = polygon[static_cast((j+1)%polygon.size())]; + index_type e_second = + polygon[static_cast((j + 1) % polygon.size())]; const Vec3f& v1 = points[e_first]; const Vec3f& v2 = points[e_second]; FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); @@ -216,26 +201,22 @@ FCL_REAL Convex::computeVolume() const } template -void Convex::fillNeighbors() -{ +void Convex::fillNeighbors() { neighbors = new Neighbors[num_points]; typedef typename PolygonT::size_type size_type; typedef typename PolygonT::index_type index_type; - std::vector > nneighbors (num_points); + std::vector > nneighbors(num_points); unsigned int c_nneighbors = 0; - for (unsigned int l = 0; l < num_polygons; ++l) - { - const PolygonT & polygon = polygons[l]; + for (unsigned int l = 0; l < num_polygons; ++l) { + const PolygonT& polygon = polygons[l]; const size_type n = polygon.size(); - for(size_type j = 0; j < polygon.size(); ++j) - { - size_type i = (j==0 ) ? n-1 : j-1; - size_type k = (j==n-1) ? 0 : j+1; - index_type pi = polygon[(index_type)i], - pj = polygon[(index_type)j], + for (size_type j = 0; j < polygon.size(); ++j) { + size_type i = (j == 0) ? n - 1 : j - 1; + size_type k = (j == n - 1) ? 0 : j + 1; + index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j], pk = polygon[(index_type)k]; // Update neighbors of pj; if (nneighbors[pj].count(pi) == 0) { @@ -254,16 +235,17 @@ void Convex::fillNeighbors() for (unsigned int i = 0; i < num_points; ++i) { Neighbors& n = neighbors[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) - throw std::logic_error ("Too many neighbors."); + throw std::logic_error("Too many neighbors."); n.count_ = (unsigned char)nneighbors[i].size(); - n.n_ = p_nneighbors; - p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors); + n.n_ = p_nneighbors; + p_nneighbors = + std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors); } - assert (p_nneighbors == nneighbors_ + c_nneighbors); + assert(p_nneighbors == nneighbors_ + c_nneighbors); } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h index fbe3088b5..dec758e54 100644 --- a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h +++ b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H #define GEOMETRIC_SHAPE_TO_BVH_MODEL_H @@ -43,44 +42,41 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Generate BVH model from box -template -void generateBVHModel(BVHModel& model, const Box& shape, const Transform3f& pose) -{ +template +void generateBVHModel(BVHModel& model, const Box& shape, + const Transform3f& pose) { FCL_REAL a = shape.halfSide[0]; FCL_REAL b = shape.halfSide[1]; FCL_REAL c = shape.halfSide[2]; std::vector points(8); std::vector tri_indices(12); - points[0] = Vec3f ( a, -b, c); - points[1] = Vec3f ( a, b, c); - points[2] = Vec3f (-a, b, c); - points[3] = Vec3f (-a, -b, c); - points[4] = Vec3f ( a, -b, -c); - points[5] = Vec3f ( a, b, -c); - points[6] = Vec3f (-a, b, -c); - points[7] = Vec3f (-a, -b, -c); - - tri_indices[ 0].set(0, 4, 1); - tri_indices[ 1].set(1, 4, 5); - tri_indices[ 2].set(2, 6, 3); - tri_indices[ 3].set(3, 6, 7); - tri_indices[ 4].set(3, 0, 2); - tri_indices[ 5].set(2, 0, 1); - tri_indices[ 6].set(6, 5, 7); - tri_indices[ 7].set(7, 5, 4); - tri_indices[ 8].set(1, 5, 2); - tri_indices[ 9].set(2, 5, 6); + points[0] = Vec3f(a, -b, c); + points[1] = Vec3f(a, b, c); + points[2] = Vec3f(-a, b, c); + points[3] = Vec3f(-a, -b, c); + points[4] = Vec3f(a, -b, -c); + points[5] = Vec3f(a, b, -c); + points[6] = Vec3f(-a, b, -c); + points[7] = Vec3f(-a, -b, -c); + + tri_indices[0].set(0, 4, 1); + tri_indices[1].set(1, 4, 5); + tri_indices[2].set(2, 6, 3); + tri_indices[3].set(3, 6, 7); + tri_indices[4].set(3, 0, 2); + tri_indices[5].set(2, 0, 1); + tri_indices[6].set(6, 5, 7); + tri_indices[7].set(7, 5, 4); + tri_indices[8].set(1, 5, 2); + tri_indices[9].set(2, 5, 6); tri_indices[10].set(3, 7, 0); tri_indices[11].set(0, 7, 4); - for(unsigned int i = 0; i < points.size(); ++i) - { + for (unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]).eval(); } @@ -90,10 +86,12 @@ void generateBVHModel(BVHModel& model, const Box& shape, const Transform3f& model.computeLocalAABB(); } -/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude. -template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3f& pose, unsigned int seg, unsigned int ring) -{ +/// @brief Generate BVH model from sphere, given the number of segments along +/// longitude and number of rings along latitude. +template +void generateBVHModel(BVHModel& model, const Sphere& shape, + const Transform3f& pose, unsigned int seg, + unsigned int ring) { std::vector points; std::vector tri_indices; @@ -107,33 +105,30 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3 thetad = pi / (ring + 1); theta = 0; - for(unsigned int i = 0; i < ring; ++i) - { + for (unsigned int i = 0; i < ring; ++i) { FCL_REAL theta_ = theta + thetad * (i + 1); - for(unsigned int j = 0; j < seg; ++j) - { - points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_))); + for (unsigned int j = 0; j < seg; ++j) { + points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), + r * sin(theta_) * sin(phi + j * phid), + r * cos(theta_))); } } points.push_back(Vec3f(0, 0, r)); points.push_back(Vec3f(0, 0, -r)); - for(unsigned int i = 0; i < ring - 1; ++i) - { - for(unsigned int j = 0; j < seg; ++j) - { - unsigned int a, b, c, d; - a = i * seg + j; - b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); - c = (i + 1) * seg + j; - d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); - tri_indices.push_back(Triangle(a, c, b)); - tri_indices.push_back(Triangle(b, c, d)); + for (unsigned int i = 0; i < ring - 1; ++i) { + for (unsigned int j = 0; j < seg; ++j) { + unsigned int a, b, c, d; + a = i * seg + j; + b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); + c = (i + 1) * seg + j; + d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); + tri_indices.push_back(Triangle(a, c, b)); + tri_indices.push_back(Triangle(b, c, d)); } } - for(unsigned int j = 0; j < seg; ++j) - { + for (unsigned int j = 0; j < seg; ++j) { unsigned int a, b; a = j; b = (j == seg - 1) ? 0 : (j + 1); @@ -144,8 +139,7 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3 tri_indices.push_back(Triangle(a, ring * seg + 1, b)); } - for(unsigned int i = 0; i < points.size(); ++i) - { + for (unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } @@ -156,24 +150,29 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3 } /// @brief Generate BVH model from sphere -/// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r, -/// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s -template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere) -{ +/// The difference between generateBVHModel is that it gives the number of +/// triangles faces N for a sphere with unit radius. For sphere of radius r, +/// then the number of triangles is r * r * N so that the area represented by a +/// single triangle is approximately the same.s +template +void generateBVHModel(BVHModel& model, const Sphere& shape, + const Transform3f& pose, + unsigned int n_faces_for_unit_sphere) { FCL_REAL r = shape.radius; - FCL_REAL n_low_bound = std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r; - unsigned int ring = (unsigned int) ceil(n_low_bound); - unsigned int seg = (unsigned int) ceil(n_low_bound); + FCL_REAL n_low_bound = + std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r; + unsigned int ring = (unsigned int)ceil(n_low_bound); + unsigned int seg = (unsigned int)ceil(n_low_bound); - generateBVHModel(model, shape, pose, seg, ring); + generateBVHModel(model, shape, pose, seg, ring); } - -/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. -template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num) -{ +/// @brief Generate BVH model from cylinder, given the number of segments along +/// circle and the number of segments along axis. +template +void generateBVHModel(BVHModel& model, const Cylinder& shape, + const Transform3f& pose, unsigned int tot, + unsigned int h_num) { std::vector points; std::vector tri_indices; @@ -186,39 +185,37 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transfor FCL_REAL hd = 2 * h / h_num; - for(unsigned int i = 0; i < tot; ++i) - points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h)); + for (unsigned int i = 0; i < tot; ++i) + points.push_back( + Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h)); - for(unsigned int i = 0; i < h_num - 1; ++i) - { - for(unsigned int j = 0; j < tot; ++j) - { - points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h - (i + 1) * hd)); + for (unsigned int i = 0; i < h_num - 1; ++i) { + for (unsigned int j = 0; j < tot; ++j) { + points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), + h - (i + 1) * hd)); } } - for(unsigned int i = 0; i < tot; ++i) - points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h)); + for (unsigned int i = 0; i < tot; ++i) + points.push_back( + Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); points.push_back(Vec3f(0, 0, h)); points.push_back(Vec3f(0, 0, -h)); - for(unsigned int i = 0; i < tot; ++i) - { + for (unsigned int i = 0; i < tot; ++i) { Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); tri_indices.push_back(tmp); } - for(unsigned int i = 0; i < tot; ++i) - { - Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); + for (unsigned int i = 0; i < tot; ++i) { + Triangle tmp((h_num + 1) * tot + 1, + h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); tri_indices.push_back(tmp); } - for(unsigned int i = 0; i < h_num; ++i) - { - for(unsigned int j = 0; j < tot; ++j) - { + for (unsigned int i = 0; i < h_num; ++i) { + for (unsigned int j = 0; j < tot; ++j) { unsigned int a, b, c, d; a = j; b = (j == tot - 1) ? 0 : (j + 1); @@ -231,8 +228,7 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transfor } } - for(unsigned int i = 0; i < points.size(); ++i) - { + for (unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } @@ -243,11 +239,13 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transfor } /// @brief Generate BVH model from cylinder -/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with -/// larger radius, the number of circle split number is r * tot. -template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder) -{ +/// Difference from generateBVHModel: is that it gives the circle split number +/// tot for a cylinder with unit radius. For cylinder with larger radius, the +/// number of circle split number is r * tot. +template +void generateBVHModel(BVHModel& model, const Cylinder& shape, + const Transform3f& pose, + unsigned int tot_for_unit_cylinder) { FCL_REAL r = shape.radius; FCL_REAL h = 2 * shape.halfLength; @@ -257,15 +255,16 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transfor FCL_REAL circle_edge = phid * r; unsigned int h_num = (unsigned int)ceil(h / circle_edge); - + generateBVHModel(model, shape, pose, tot, h_num); } - -/// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis. -template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num) -{ +/// @brief Generate BVH model from cone, given the number of segments along +/// circle and the number of segments along axis. +template +void generateBVHModel(BVHModel& model, const Cone& shape, + const Transform3f& pose, unsigned int tot, + unsigned int h_num) { std::vector points; std::vector tri_indices; @@ -279,38 +278,36 @@ void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& FCL_REAL hd = 2 * h / h_num; - for(unsigned int i = 0; i < h_num - 1; ++i) - { + for (unsigned int i = 0; i < h_num - 1; ++i) { FCL_REAL h_i = h - (i + 1) * hd; FCL_REAL rh = r * (0.5 - h_i / h / 2); - for(unsigned int j = 0; j < tot; ++j) - { - points.push_back(Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); + for (unsigned int j = 0; j < tot; ++j) { + points.push_back( + Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); } } - for(unsigned int i = 0; i < tot; ++i) - points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h)); + for (unsigned int i = 0; i < tot; ++i) + points.push_back( + Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); points.push_back(Vec3f(0, 0, h)); points.push_back(Vec3f(0, 0, -h)); - for(unsigned int i = 0; i < tot; ++i) - { + for (unsigned int i = 0; i < tot; ++i) { Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); tri_indices.push_back(tmp); } - for(unsigned int i = 0; i < tot; ++i) - { - Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i); + for (unsigned int i = 0; i < tot; ++i) { + Triangle tmp(h_num * tot + 1, + (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), + (h_num - 1) * tot + i); tri_indices.push_back(tmp); } - for(unsigned int i = 0; i < h_num - 1; ++i) - { - for(unsigned int j = 0; j < tot; ++j) - { + for (unsigned int i = 0; i < h_num - 1; ++i) { + for (unsigned int j = 0; j < tot; ++j) { unsigned int a, b, c, d; a = j; b = (j == tot - 1) ? 0 : (j + 1); @@ -323,8 +320,7 @@ void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& } } - for(unsigned int i = 0; i < points.size(); ++i) - { + for (unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } @@ -335,11 +331,12 @@ void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& } /// @brief Generate BVH model from cone -/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with -/// larger radius, the number of circle split number is r * tot. -template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone) -{ +/// Difference from generateBVHModel: is that it gives the circle split number +/// tot for a cylinder with unit radius. For cone with larger radius, the number +/// of circle split number is r * tot. +template +void generateBVHModel(BVHModel& model, const Cone& shape, + const Transform3f& pose, unsigned int tot_for_unit_cone) { FCL_REAL r = shape.radius; FCL_REAL h = 2 * shape.halfLength; @@ -353,8 +350,8 @@ void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& generateBVHModel(model, shape, pose, tot, h_num); } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index c488a0678..eccf00770 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_GEOMETRIC_SHAPES_H #define HPP_FCL_GEOMETRIC_SHAPES_H @@ -45,23 +44,18 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Base class for all basic geometric shapes -class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry -{ -public: +class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { + public: ShapeBase() {} - + /// \brief Copy constructor - ShapeBase(const ShapeBase & other) - : CollisionGeometry(other) - {} + ShapeBase(const ShapeBase& other) : CollisionGeometry(other) {} - virtual ~ShapeBase () {}; + virtual ~ShapeBase(){}; /// @brief Get object type: a geometric shape OBJECT_TYPE getObjectType() const { return OT_GEOM; } @@ -72,65 +66,48 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry /// @{ /// @brief Triangle stores the points instead of only indices of points -class HPP_FCL_DLLAPI TriangleP : public ShapeBase -{ -public: - TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_) - { - } - - - TriangleP(const TriangleP & other) - : ShapeBase(other) - , a(other.a), b(other.b), c(other.c) - {} - +class HPP_FCL_DLLAPI TriangleP : public ShapeBase { + public: + TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) + : ShapeBase(), a(a_), b(b_), c(c_) {} + + TriangleP(const TriangleP& other) + : ShapeBase(other), a(other.a), b(other.b), c(other.c) {} + /// @brief Clone *this into a new TriangleP virtual TriangleP* clone() const { return new TriangleP(*this); }; - + /// @brief virtual function of compute AABB in local coordinate void computeLocalAABB(); - + NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } Vec3f a, b, c; - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const TriangleP * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const TriangleP & other = *other_ptr; - + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const TriangleP* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const TriangleP& other = *other_ptr; + return a == other.a && b == other.b && c == other.c; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Center at zero point, axis aligned box -class HPP_FCL_DLLAPI Box : public ShapeBase -{ -public: - Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), halfSide(x/2, y/2, z/2) - { - } +class HPP_FCL_DLLAPI Box : public ShapeBase { + public: + Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) + : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {} - Box(const Vec3f& side_) : ShapeBase(), halfSide(side_/2) - { - } - - Box(const Box & other) - : ShapeBase(other) - , halfSide(other.halfSide) - { - } + Box(const Vec3f& side_) : ShapeBase(), halfSide(side_ / 2) {} + + Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {} - Box& operator=(const Box & other) - { + Box& operator=(const Box& other) { if (this == &other) return *this; this->halfSide = other.halfSide; @@ -152,142 +129,115 @@ class HPP_FCL_DLLAPI Box : public ShapeBase /// @brief Get node type: a box NODE_TYPE getNodeType() const { return GEOM_BOX; } - FCL_REAL computeVolume() const - { - return 8*halfSide.prod(); - } + FCL_REAL computeVolume() const { return 8 * halfSide.prod(); } - Matrix3f computeMomentofInertia() const - { + Matrix3f computeMomentofInertia() const { FCL_REAL V = computeVolume(); - Vec3f s (halfSide.cwiseAbs2() * V); - return (Vec3f (s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); - } - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const Box * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const Box & other = *other_ptr; - + Vec3f s(halfSide.cwiseAbs2() * V); + return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Box* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Box& other = *other_ptr; + return halfSide == other.halfSide; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Center at zero point sphere -class HPP_FCL_DLLAPI Sphere : public ShapeBase -{ -public: - Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) - { - } - - Sphere(const Sphere & other) - : ShapeBase(other) - , radius(other.radius) - { - } - +class HPP_FCL_DLLAPI Sphere : public ShapeBase { + public: + Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {} + + Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {} + /// @brief Clone *this into a new Sphere virtual Sphere* clone() const { return new Sphere(*this); }; - - /// @brief Radius of the sphere + + /// @brief Radius of the sphere FCL_REAL radius; - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB(); - /// @brief Get node type: a sphere + /// @brief Get node type: a sphere NODE_TYPE getNodeType() const { return GEOM_SPHERE; } - Matrix3f computeMomentofInertia() const - { + Matrix3f computeMomentofInertia() const { FCL_REAL I = 0.4 * radius * radius * computeVolume(); return I * Matrix3f::Identity(); } - FCL_REAL computeVolume() const - { - return 4 * boost::math::constants::pi() * radius * radius * radius / 3; + FCL_REAL computeVolume() const { + return 4 * boost::math::constants::pi() * radius * radius * + radius / 3; } - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const Sphere * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const Sphere & other = *other_ptr; - + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Sphere* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Sphere& other = *other_ptr; + return radius == other.radius; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Ellipsoid centered at point zero -class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase -{ -public: - Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz) : ShapeBase(), radii(rx, ry, rz) - { - } - - Ellipsoid(const Ellipsoid & other) - : ShapeBase(other) - , radii(other.radii) - { - } - +class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { + public: + Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz) + : ShapeBase(), radii(rx, ry, rz) {} + + Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {} + /// @brief Clone *this into a new Ellipsoid virtual Ellipsoid* clone() const { return new Ellipsoid(*this); }; - - /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2 + z^2/rz^2 = 1) + + /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2 + /// + z^2/rz^2 = 1) Vec3f radii; - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB(); - /// @brief Get node type: an ellipsoid + /// @brief Get node type: an ellipsoid NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; } - Matrix3f computeMomentofInertia() const - { + Matrix3f computeMomentofInertia() const { FCL_REAL V = computeVolume(); FCL_REAL a2 = V * radii[0] * radii[0]; FCL_REAL b2 = V * radii[1] * radii[1]; FCL_REAL c2 = V * radii[2] * radii[2]; - return (Matrix3f() << 0.2 * (b2 + c2), 0, 0, - 0, 0.2 * (a2 + c2), 0, - 0, 0, 0.2 * (a2 + b2)).finished(); - } - - FCL_REAL computeVolume() const - { - return 4 * boost::math::constants::pi() * radii[0] * radii[1] * radii[2] / 3; - } - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const Ellipsoid * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const Ellipsoid & other = *other_ptr; - + return (Matrix3f() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0, + 0.2 * (a2 + b2)) + .finished(); + } + + FCL_REAL computeVolume() const { + return 4 * boost::math::constants::pi() * radii[0] * radii[1] * + radii[2] / 3; + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Ellipsoid* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Ellipsoid& other = *other_ptr; + return radii == other.radii; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -295,162 +245,131 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase /// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$ /// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule /// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$. -class HPP_FCL_DLLAPI Capsule : public ShapeBase -{ -public: - Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) - { - halfLength = lz_/2; - } - - Capsule(const Capsule & other) - : ShapeBase(other) - , radius(other.radius) - , halfLength(other.halfLength) - { - } - +class HPP_FCL_DLLAPI Capsule : public ShapeBase { + public: + Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { + halfLength = lz_ / 2; + } + + Capsule(const Capsule& other) + : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} + /// @brief Clone *this into a new Capsule virtual Capsule* clone() const { return new Capsule(*this); }; - /// @brief Radius of capsule + /// @brief Radius of capsule FCL_REAL radius; - /// @brief Half Length along z axis + /// @brief Half Length along z axis FCL_REAL halfLength; - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB(); - /// @brief Get node type: a capsule + /// @brief Get node type: a capsule NODE_TYPE getNodeType() const { return GEOM_CAPSULE; } - FCL_REAL computeVolume() const - { - return boost::math::constants::pi() * radius * radius *((halfLength * 2) + radius * 4/3.0); + FCL_REAL computeVolume() const { + return boost::math::constants::pi() * radius * radius * + ((halfLength * 2) + radius * 4 / 3.0); } - Matrix3f computeMomentofInertia() const - { - FCL_REAL v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi(); - FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi() * 4 / 3.0; - + Matrix3f computeMomentofInertia() const { + FCL_REAL v_cyl = radius * radius * (halfLength * 2) * + boost::math::constants::pi(); + FCL_REAL v_sph = radius * radius * radius * + boost::math::constants::pi() * 4 / 3.0; + FCL_REAL h2 = halfLength * halfLength; FCL_REAL r2 = radius * radius; - FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength); + FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength); FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; - return (Matrix3f() << ix, 0, 0, - 0, ix, 0, - 0, 0, iz).finished(); - } - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const Capsule * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const Capsule & other = *other_ptr; - + return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Capsule* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Capsule& other = *other_ptr; + return radius == other.radius && halfLength == other.halfLength; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; /// @brief Cone /// The base of the cone is at \f$ z = - halfLength \f$ and the top is at /// \f$ z = halfLength \f$. -class HPP_FCL_DLLAPI Cone : public ShapeBase -{ -public: - Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) - { - halfLength = lz_/2; - } - - Cone(const Cone & other) - : ShapeBase(other) - , radius(other.radius) - , halfLength(other.halfLength) - { - } - +class HPP_FCL_DLLAPI Cone : public ShapeBase { + public: + Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { + halfLength = lz_ / 2; + } + + Cone(const Cone& other) + : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} + /// @brief Clone *this into a new Cone virtual Cone* clone() const { return new Cone(*this); }; - /// @brief Radius of the cone + /// @brief Radius of the cone FCL_REAL radius; - /// @brief Half Length along z axis + /// @brief Half Length along z axis FCL_REAL halfLength; - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB(); - /// @brief Get node type: a cone + /// @brief Get node type: a cone NODE_TYPE getNodeType() const { return GEOM_CONE; } - FCL_REAL computeVolume() const - { - return boost::math::constants::pi() * radius * radius * (halfLength * 2) / 3; + FCL_REAL computeVolume() const { + return boost::math::constants::pi() * radius * radius * + (halfLength * 2) / 3; } - Matrix3f computeMomentofInertia() const - { + Matrix3f computeMomentofInertia() const { FCL_REAL V = computeVolume(); - FCL_REAL ix = V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20); + FCL_REAL ix = + V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20); FCL_REAL iz = 0.3 * V * radius * radius; - return (Matrix3f() << ix, 0, 0, - 0, ix, 0, - 0, 0, iz).finished(); + return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } - Vec3f computeCOM() const - { - return Vec3f(0, 0, -0.5 * halfLength); - } - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const Cone * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const Cone & other = *other_ptr; - + Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Cone* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Cone& other = *other_ptr; + return radius == other.radius && halfLength == other.halfLength; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Cylinder along Z axis. /// The cylinder is defined at its centroid. -class HPP_FCL_DLLAPI Cylinder : public ShapeBase -{ -public: - Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) - { - halfLength = lz_/2; - } - - Cylinder(const Cylinder & other) - : ShapeBase(other) - , radius(other.radius) - , halfLength(other.halfLength) - { - } - - Cylinder& operator=(const Cylinder & other) - { +class HPP_FCL_DLLAPI Cylinder : public ShapeBase { + public: + Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { + halfLength = lz_ / 2; + } + + Cylinder(const Cylinder& other) + : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {} + + Cylinder& operator=(const Cylinder& other) { if (this == &other) return *this; this->radius = other.radius; @@ -460,55 +379,48 @@ class HPP_FCL_DLLAPI Cylinder : public ShapeBase /// @brief Clone *this into a new Cylinder virtual Cylinder* clone() const { return new Cylinder(*this); }; - - /// @brief Radius of the cylinder + + /// @brief Radius of the cylinder FCL_REAL radius; - /// @brief Half Length along z axis + /// @brief Half Length along z axis FCL_REAL halfLength; - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB(); - /// @brief Get node type: a cylinder + /// @brief Get node type: a cylinder NODE_TYPE getNodeType() const { return GEOM_CYLINDER; } - FCL_REAL computeVolume() const - { - return boost::math::constants::pi() * radius * radius * (halfLength * 2); + FCL_REAL computeVolume() const { + return boost::math::constants::pi() * radius * radius * + (halfLength * 2); } - Matrix3f computeMomentofInertia() const - { + Matrix3f computeMomentofInertia() const { FCL_REAL V = computeVolume(); FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3); FCL_REAL iz = V * radius * radius / 2; - return (Matrix3f() << ix, 0, 0, - 0, ix, 0, - 0, 0, iz).finished(); - } - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const Cylinder * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const Cylinder & other = *other_ptr; - + return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); + } + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Cylinder* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Cylinder& other = *other_ptr; + return radius == other.radius && halfLength == other.halfLength; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// @brief Base for convex polytope. /// @note Inherited classes are responsible for filling ConvexBase::neighbors; -class HPP_FCL_DLLAPI ConvexBase : public ShapeBase -{ -public: +class HPP_FCL_DLLAPI ConvexBase : public ShapeBase { + public: /// @brief Build a convex hull based on Qhull library /// and store the vertices and optionally the triangles /// \param points, num_points the points whose convex hull should be computed. @@ -521,79 +433,82 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase /// Qhull. /// \note hpp-fcl must have been compiled with option \c HPP_FCL_HAS_QHULL set /// to \c ON. - static ConvexBase* convexHull (const Vec3f* points, unsigned int num_points, - bool keepTriangles, const char* qhullCommand = NULL); + static ConvexBase* convexHull(const Vec3f* points, unsigned int num_points, + bool keepTriangles, + const char* qhullCommand = NULL); virtual ~ConvexBase(); - + /// @brief Clone (deep copy). - virtual ConvexBase * clone() const - { - ConvexBase * copy_ptr = new ConvexBase(*this); - ConvexBase & copy = *copy_ptr; + virtual ConvexBase* clone() const { + ConvexBase* copy_ptr = new ConvexBase(*this); + ConvexBase& copy = *copy_ptr; - if(!copy.own_storage_) - { + if (!copy.own_storage_) { copy.points = new Vec3f[copy.num_points]; std::copy(points, points + num_points, copy.points); } copy.own_storage_ = true; copy.ShapeBase::operator=(*this); - + return copy_ptr; } - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB(); - /// @brief Get node type: a conex polytope + /// @brief Get node type: a conex polytope NODE_TYPE getNodeType() const { return GEOM_CONVEX; } /// @brief An array of the points of the polygon. Vec3f* points; unsigned int num_points; - struct HPP_FCL_DLLAPI Neighbors - { + struct HPP_FCL_DLLAPI Neighbors { unsigned char count_; unsigned int* n_; - unsigned char const& count () const { return count_; } - unsigned int & operator[] (int i) { assert(i(&_other); - if(other_ptr == nullptr) return false; - const ConvexBase & other = *other_ptr; - - if(num_points != other.num_points) - return false; - - for(unsigned int i = 0; i < num_points; ++i) - { - if(points[i] != other.points[i]) - return false; + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const ConvexBase* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const ConvexBase& other = *other_ptr; + + if (num_points != other.num_points) return false; + + for (unsigned int i = 0; i < num_points; ++i) { + if (points[i] != other.points[i]) return false; } - - for(unsigned int i = 0; i < num_points; ++i) - { - if(neighbors[i] != other.neighbors[i]) - return false; + + for (unsigned int i = 0; i < num_points; ++i) { + if (neighbors[i] != other.neighbors[i]) return false; } - + return center == other.center; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -template class Convex; +template +class Convex; -/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; -/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points -/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space -class HPP_FCL_DLLAPI Halfspace : public ShapeBase -{ -public: +/// @brief Half Space: this is equivalent to the Plane in ODE. The separation +/// plane is defined as n * x = d; Points in the negative side of the separation +/// plane (i.e. {x | n * x < d}) are inside the half space and points in the +/// positive side of the separation plane (i.e. {x | n * x > d}) are outside the +/// half space +class HPP_FCL_DLLAPI Halfspace : public ShapeBase { + public: /// @brief Construct a half space with normal direction and offset - Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) - { + Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } /// @brief Construct a plane with normal direction and offset - Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_) - { + Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) + : ShapeBase(), n(a, b, c), d(d_) { unitNormalTest(); } - Halfspace() : ShapeBase(), n(1, 0, 0), d(0) - { - } - - Halfspace(const Halfspace & other) - : ShapeBase(other) - , n(other.n) - , d(other.d) - { - } + Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {} + + Halfspace(const Halfspace& other) + : ShapeBase(other), n(other.n), d(other.d) {} - /// @brief operator = - Halfspace& operator = (const Halfspace& other) - { + /// @brief operator = + Halfspace& operator=(const Halfspace& other) { n = other.n; d = other.d; return *this; @@ -694,78 +595,59 @@ class HPP_FCL_DLLAPI Halfspace : public ShapeBase /// @brief Clone *this into a new Halfspace virtual Halfspace* clone() const { return new Halfspace(*this); }; - FCL_REAL signedDistance(const Vec3f& p) const - { - return n.dot(p) - d; - } + FCL_REAL signedDistance(const Vec3f& p) const { return n.dot(p) - d; } - FCL_REAL distance(const Vec3f& p) const - { - return std::abs(n.dot(p) - d); - } + FCL_REAL distance(const Vec3f& p) const { return std::abs(n.dot(p) - d); } /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a half space NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; } - + /// @brief Plane normal Vec3f n; - + /// @brief Plane offset FCL_REAL d; -protected: - + protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const Halfspace * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const Halfspace & other = *other_ptr; - + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Halfspace* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Halfspace& other = *other_ptr; + return n == other.n && d == other.d; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -/// @brief Infinite plane -class HPP_FCL_DLLAPI Plane : public ShapeBase -{ -public: +/// @brief Infinite plane +class HPP_FCL_DLLAPI Plane : public ShapeBase { + public: /// @brief Construct a plane with normal direction and offset - Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) - { - unitNormalTest(); - } - - /// @brief Construct a plane with normal direction and offset - Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_) - { + Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } - Plane() : ShapeBase(), n(1, 0, 0), d(0) - {} - - Plane(const Plane & other) - : ShapeBase(other) - , n(other.n) - , d(other.d) - { + /// @brief Construct a plane with normal direction and offset + Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) + : ShapeBase(), n(a, b, c), d(d_) { + unitNormalTest(); } - /// @brief operator = - Plane& operator = (const Plane& other) - { + Plane() : ShapeBase(), n(1, 0, 0), d(0) {} + + Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {} + + /// @brief operator = + Plane& operator=(const Plane& other) { n = other.n; d = other.d; return *this; @@ -774,51 +656,41 @@ class HPP_FCL_DLLAPI Plane : public ShapeBase /// @brief Clone *this into a new Plane virtual Plane* clone() const { return new Plane(*this); }; - FCL_REAL signedDistance(const Vec3f& p) const - { - return n.dot(p) - d; - } + FCL_REAL signedDistance(const Vec3f& p) const { return n.dot(p) - d; } - FCL_REAL distance(const Vec3f& p) const - { - return std::abs(n.dot(p) - d); - } + FCL_REAL distance(const Vec3f& p) const { return std::abs(n.dot(p) - d); } - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB(); - /// @brief Get node type: a plane + /// @brief Get node type: a plane NODE_TYPE getNodeType() const { return GEOM_PLANE; } - /// @brief Plane normal + /// @brief Plane normal Vec3f n; - /// @brief Plane offset + /// @brief Plane offset FCL_REAL d; -protected: - - /// @brief Turn non-unit normal into unit + protected: + /// @brief Turn non-unit normal into unit void unitNormalTest(); - -private: - - virtual bool isEqual(const CollisionGeometry & _other) const - { - const Plane * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const Plane & other = *other_ptr; - + + private: + virtual bool isEqual(const CollisionGeometry& _other) const { + const Plane* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const Plane& other = *other_ptr; + return n == other.n && d == other.d; } - -public: - + + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/shape/geometric_shapes_utility.h b/include/hpp/fcl/shape/geometric_shapes_utility.h index 8e2ee7591..33d412aa2 100644 --- a/include/hpp/fcl/shape/geometric_shapes_utility.h +++ b/include/hpp/fcl/shape/geometric_shapes_utility.h @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #ifndef HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H #define HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H @@ -44,129 +43,170 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @cond IGNORE -namespace details -{ -/// @brief get the vertices of some convex shape which can bound the given shape in a specific configuration -HPP_FCL_DLLAPI std::vector getBoundVertices(const Box& box, const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Cone& cone, const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, const Transform3f& tf); -} +namespace details { +/// @brief get the vertices of some convex shape which can bound the given shape +/// in a specific configuration +HPP_FCL_DLLAPI std::vector getBoundVertices(const Box& box, + const Transform3f& tf); +HPP_FCL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, + const Transform3f& tf); +HPP_FCL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, + const Transform3f& tf); +HPP_FCL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, + const Transform3f& tf); +HPP_FCL_DLLAPI std::vector getBoundVertices(const Cone& cone, + const Transform3f& tf); +HPP_FCL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, + const Transform3f& tf); +HPP_FCL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, + const Transform3f& tf); +HPP_FCL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, + const Transform3f& tf); +} // namespace details /// @endcond - /// @brief calculate a bounding volume for a shape in a specific configuration -template -inline void computeBV(const S& s, const Transform3f& tf, BV& bv) -{ +template +inline void computeBV(const S& s, const Transform3f& tf, BV& bv) { std::vector convex_bound_vertices = details::getBoundVertices(s, tf); - fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(), bv); + fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(), + bv); } -template<> -HPP_FCL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Ellipsoid& e, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Capsule& s, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Cylinder& s, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const ConvexBase& s, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const TriangleP& s, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, const Transform3f& tf, AABB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, AABB& bv); - - - -template<> -HPP_FCL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, OBB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, OBB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Cylinder& s, const Transform3f& tf, OBB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const ConvexBase& s, const Transform3f& tf, OBB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, const Transform3f& tf, OBB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, const Transform3f& tf, RSS& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, const Transform3f& tf, OBBRSS& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, const Transform3f& tf, kIOS& bv); - -template<> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv); - -template<> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv); - -template<> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, OBB& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, RSS& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, OBBRSS& bv); - -template<> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv); - -template<> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv); - -template<> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv); - -template<> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv); - - -/// @brief construct a box shape (with a configuration) from a given bounding volume +template <> +HPP_FCL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, + AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Sphere& s, + const Transform3f& tf, AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Ellipsoid& e, + const Transform3f& tf, AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Capsule& s, + const Transform3f& tf, AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, + AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Cylinder& s, + const Transform3f& tf, AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const ConvexBase& s, + const Transform3f& tf, + AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const TriangleP& s, + const Transform3f& tf, AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Plane& s, + const Transform3f& tf, AABB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, + OBB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Sphere& s, + const Transform3f& tf, OBB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Capsule& s, + const Transform3f& tf, OBB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, + OBB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Cylinder& s, + const Transform3f& tf, OBB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const ConvexBase& s, + const Transform3f& tf, OBB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, OBB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, RSS& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, + OBBRSS& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, kIOS& bv); + +template <> +HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3f& tf, + KDOP<16>& bv); + +template <> +HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3f& tf, + KDOP<18>& bv); + +template <> +HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3f& tf, + KDOP<24>& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, + OBB& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, + RSS& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Plane& s, + const Transform3f& tf, OBBRSS& bv); + +template <> +HPP_FCL_DLLAPI void computeBV(const Plane& s, + const Transform3f& tf, kIOS& bv); + +template <> +HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3f& tf, + KDOP<16>& bv); + +template <> +HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3f& tf, + KDOP<18>& bv); + +template <> +HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3f& tf, + KDOP<24>& bv); + +/// @brief construct a box shape (with a configuration) from a given bounding +/// volume HPP_FCL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3f& tf); HPP_FCL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3f& tf); @@ -183,28 +223,36 @@ HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf); HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); +HPP_FCL_DLLAPI void constructBox(const AABB& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); +HPP_FCL_DLLAPI void constructBox(const OBB& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); +HPP_FCL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); +HPP_FCL_DLLAPI void constructBox(const kIOS& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); +HPP_FCL_DLLAPI void constructBox(const RSS& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); +HPP_FCL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); +HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); +HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); HPP_FCL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3f& tf); HPP_FCL_DLLAPI Plane transform(const Plane& a, const Transform3f& tf); -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif diff --git a/include/hpp/fcl/timings.h b/include/hpp/fcl/timings.h index 2e9f693c3..0a67e7035 100644 --- a/include/hpp/fcl/timings.h +++ b/include/hpp/fcl/timings.h @@ -8,105 +8,89 @@ #include "hpp/fcl/fwd.hh" #ifdef HPP_FCL_WITH_CXX11_SUPPORT - #include +#include #endif -namespace hpp { namespace fcl { - - struct CPUTimes - { - double wall; - double user; - double system; - - CPUTimes() - : wall(0) - , user(0) - , system(0) - {} - - void clear() - { - wall = user = system = 0; - } - - }; - - /// - /// @brief This class mimics the way "boost/timer/timer.hpp" operates while using the modern std::chrono library. - /// Importantly, this class will only have an effect for C++11 and more. - /// - struct HPP_FCL_DLLAPI Timer - { - - Timer() - : m_is_stopped(true) - { - start(); - } - - CPUTimes elapsed() const - { - if(m_is_stopped) - return m_times; +namespace hpp { +namespace fcl { + +struct CPUTimes { + double wall; + double user; + double system; + + CPUTimes() : wall(0), user(0), system(0) {} + + void clear() { wall = user = system = 0; } +}; - CPUTimes current(m_times); +/// +/// @brief This class mimics the way "boost/timer/timer.hpp" operates while +/// using the modern std::chrono library. +/// Importantly, this class will only have an effect for C++11 and more. +/// +struct HPP_FCL_DLLAPI Timer { + Timer() : m_is_stopped(true) { start(); } + + CPUTimes elapsed() const { + if (m_is_stopped) return m_times; + + CPUTimes current(m_times); #ifdef HPP_FCL_WITH_CXX11_SUPPORT - std::chrono::time_point current_clock = std::chrono::steady_clock::now(); - current.user += static_cast(std::chrono::duration_cast(current_clock - m_start).count())*1e-3; + std::chrono::time_point current_clock = + std::chrono::steady_clock::now(); + current.user += static_cast( + std::chrono::duration_cast( + current_clock - m_start) + .count()) * + 1e-3; #endif - return current; - } - - void start() - { - if(m_is_stopped) - { - m_is_stopped = false; - m_times.clear(); - + return current; + } + + void start() { + if (m_is_stopped) { + m_is_stopped = false; + m_times.clear(); + #ifdef HPP_FCL_WITH_CXX11_SUPPORT - m_start = std::chrono::steady_clock::now(); + m_start = std::chrono::steady_clock::now(); #endif - } } - - void stop() - { - if(m_is_stopped) - return; - m_is_stopped = true; - + } + + void stop() { + if (m_is_stopped) return; + m_is_stopped = true; + #ifdef HPP_FCL_WITH_CXX11_SUPPORT - m_end = std::chrono::steady_clock::now(); - m_times.user += static_cast(std::chrono::duration_cast(m_end - m_start).count())*1e-3; + m_end = std::chrono::steady_clock::now(); + m_times.user += static_cast( + std::chrono::duration_cast( + m_end - m_start) + .count()) * + 1e-3; #endif - - } - - void resume() - { + } + + void resume() { #ifdef HPP_FCL_WITH_CXX11_SUPPORT - if(m_is_stopped) - m_start = std::chrono::steady_clock::now(); + if (m_is_stopped) m_start = std::chrono::steady_clock::now(); #endif - } - - bool is_stopped() const - { - return m_is_stopped; - } - - protected: - - CPUTimes m_times; - bool m_is_stopped; - + } + + bool is_stopped() const { return m_is_stopped; } + + protected: + CPUTimes m_times; + bool m_is_stopped; + #ifdef HPP_FCL_WITH_CXX11_SUPPORT - std::chrono::time_point m_start, m_end; + std::chrono::time_point m_start, m_end; #endif - }; +}; -}} +} // namespace fcl +} // namespace hpp -#endif // ifndef HPP_FCL_TIMINGS_FWD_H +#endif // ifndef HPP_FCL_TIMINGS_FWD_H diff --git a/package.xml b/package.xml index 8d7bea21b..878f23b8f 100644 --- a/package.xml +++ b/package.xml @@ -3,7 +3,7 @@ hpp-fcl 1.8.1 An extension of the Flexible Collision Library. - Joseph Mirabel Justin Carpentier diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index 900d174e3..0ca124f61 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -47,7 +47,8 @@ #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/hpp/fcl/broadphase/default_broadphase_callbacks.h" //#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -//#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +//#include +//"doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" //#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_bruteforce.h" //#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_SaP.h" //#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_SSaP.h" @@ -60,62 +61,64 @@ using namespace hpp::fcl; -void exposeBroadPhase() -{ +void exposeBroadPhase() { CollisionCallBackBaseWrapper::expose(); DistanceCallBackBaseWrapper::expose(); // CollisionCallBackDefault - bp::class_ >("CollisionCallBackDefault",bp::no_init) - .def(dv::init()) - .DEF_RW_CLASS_ATTRIB(CollisionCallBackDefault,data) - ; - + bp::class_ >( + "CollisionCallBackDefault", bp::no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(CollisionCallBackDefault, data); + // DistanceCallBackDefault - bp::class_ >("DistanceCallBackDefault",bp::no_init) - .def(dv::init()) - .DEF_RW_CLASS_ATTRIB(DistanceCallBackDefault,data) - ; - + bp::class_ >( + "DistanceCallBackDefault", bp::no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(DistanceCallBackDefault, data); + // CollisionCallBackCollect - bp::class_ >("CollisionCallBackCollect",bp::no_init) - .def(dv::init()) - .DEF_CLASS_FUNC(CollisionCallBackCollect,numCollisionPairs) - .DEF_CLASS_FUNC2(CollisionCallBackCollect,getCollisionPairs, bp::return_value_policy()) - .DEF_CLASS_FUNC(CollisionCallBackCollect,exist) - ; - - bp::class_("CollisionData",bp::no_init) - .def(dv::init()) - .DEF_RW_CLASS_ATTRIB(CollisionData, request) - .DEF_RW_CLASS_ATTRIB(CollisionData, result) - .DEF_RW_CLASS_ATTRIB(CollisionData, done) - ; - - bp::class_("DistanceData",bp::no_init) - .def(dv::init()) - .DEF_RW_CLASS_ATTRIB(DistanceData, request) - .DEF_RW_CLASS_ATTRIB(DistanceData, result) - .DEF_RW_CLASS_ATTRIB(DistanceData, done) - ; - + bp::class_ >( + "CollisionCallBackCollect", bp::no_init) + .def(dv::init()) + .DEF_CLASS_FUNC(CollisionCallBackCollect, numCollisionPairs) + .DEF_CLASS_FUNC2(CollisionCallBackCollect, getCollisionPairs, + bp::return_value_policy()) + .DEF_CLASS_FUNC(CollisionCallBackCollect, exist); + + bp::class_("CollisionData", bp::no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(CollisionData, request) + .DEF_RW_CLASS_ATTRIB(CollisionData, result) + .DEF_RW_CLASS_ATTRIB(CollisionData, done); + + bp::class_("DistanceData", bp::no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(DistanceData, request) + .DEF_RW_CLASS_ATTRIB(DistanceData, result) + .DEF_RW_CLASS_ATTRIB(DistanceData, done); + BroadPhaseCollisionManagerWrapper::expose(); - - BroadPhaseCollisionManagerWrapper::exposeDerived(); - BroadPhaseCollisionManagerWrapper::exposeDerived(); - BroadPhaseCollisionManagerWrapper::exposeDerived(); + + BroadPhaseCollisionManagerWrapper::exposeDerived< + DynamicAABBTreeCollisionManager>(); + BroadPhaseCollisionManagerWrapper::exposeDerived< + DynamicAABBTreeArrayCollisionManager>(); + BroadPhaseCollisionManagerWrapper::exposeDerived< + IntervalTreeCollisionManager>(); BroadPhaseCollisionManagerWrapper::exposeDerived(); BroadPhaseCollisionManagerWrapper::exposeDerived(); BroadPhaseCollisionManagerWrapper::exposeDerived(); - + // Specific case of SpatialHashingCollisionManager { - typedef detail::SimpleHashTable HashTable; + typedef detail::SimpleHashTable + HashTable; typedef SpatialHashingCollisionManager Derived; - bp::class_ >("SpatialHashingCollisionManager",bp::no_init) - .def(dv::init >()) - ; + bp::class_ >( + "SpatialHashingCollisionManager", bp::no_init) + .def(dv::init >()); } - - } diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh index 3c643b99c..61f4fd346 100644 --- a/python/broadphase/broadphase_callbacks.hh +++ b/python/broadphase/broadphase_callbacks.hh @@ -47,56 +47,52 @@ #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_callbacks.h" #endif -namespace hpp { namespace fcl { +namespace hpp { +namespace fcl { -struct CollisionCallBackBaseWrapper -: CollisionCallBackBase, bp::wrapper -{ +struct CollisionCallBackBaseWrapper : CollisionCallBackBase, + bp::wrapper { typedef CollisionCallBackBase Base; - + void init() { this->get_override("init")(); } - bool collide(CollisionObject* o1, CollisionObject* o2) { return this->get_override("collide")(o1,o2); } - - static void expose() - { - bp::class_("CollisionCallBackBase",bp::no_init) - .def("init", - bp::pure_virtual(&Base::init), - doxygen::member_func_doc(&Base::init)) - .def("collide", - bp::pure_virtual(&Base::collide), - doxygen::member_func_doc(&Base::collide)) - .def("__call__", - &Base::operator(), - doxygen::member_func_doc(&Base::operator())) - ; + bool collide(CollisionObject* o1, CollisionObject* o2) { + return this->get_override("collide")(o1, o2); + } + + static void expose() { + bp::class_( + "CollisionCallBackBase", bp::no_init) + .def("init", bp::pure_virtual(&Base::init), + doxygen::member_func_doc(&Base::init)) + .def("collide", bp::pure_virtual(&Base::collide), + doxygen::member_func_doc(&Base::collide)) + .def("__call__", &Base::operator(), + doxygen::member_func_doc(&Base::operator())); } -}; // CollisionCallBackBaseWrapper +}; // CollisionCallBackBaseWrapper -struct DistanceCallBackBaseWrapper -: DistanceCallBackBase, bp::wrapper -{ +struct DistanceCallBackBaseWrapper : DistanceCallBackBase, + bp::wrapper { typedef DistanceCallBackBase Base; - + void init() { this->get_override("init")(); } - bool distance(CollisionObject* o1, CollisionObject* o2) { return this->get_override("distance")(o1,o2); } - - static void expose() - { - bp::class_("DistanceCallBackBase",bp::no_init) - .def("init", - bp::pure_virtual(&Base::init), - doxygen::member_func_doc(&Base::init)) - .def("distance", - bp::pure_virtual(&Base::distance), - doxygen::member_func_doc(&Base::distance)) - .def("__call__", - &Base::operator(), - doxygen::member_func_doc(&Base::operator())) - ; + bool distance(CollisionObject* o1, CollisionObject* o2) { + return this->get_override("distance")(o1, o2); + } + + static void expose() { + bp::class_( + "DistanceCallBackBase", bp::no_init) + .def("init", bp::pure_virtual(&Base::init), + doxygen::member_func_doc(&Base::init)) + .def("distance", bp::pure_virtual(&Base::distance), + doxygen::member_func_doc(&Base::distance)) + .def("__call__", &Base::operator(), + doxygen::member_func_doc(&Base::operator())); } -}; // DistanceCallBackBaseWrapper +}; // DistanceCallBackBaseWrapper -}} +} // namespace fcl +} // namespace hpp -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH +#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh index 561cb9a3c..e9c4900e9 100644 --- a/python/broadphase/broadphase_collision_manager.hh +++ b/python/broadphase/broadphase_collision_manager.hh @@ -50,125 +50,168 @@ #include -namespace hpp { namespace fcl { +namespace hpp { +namespace fcl { struct BroadPhaseCollisionManagerWrapper -: BroadPhaseCollisionManager, bp::wrapper -{ + : BroadPhaseCollisionManager, + bp::wrapper { typedef BroadPhaseCollisionManager Base; - - void registerObjects(const std::vector & other_objs) { this->get_override("registerObjects")(other_objs); } - void registerObject(CollisionObject* obj) { this->get_override("registerObjects")(obj); } - void unregisterObject(CollisionObject* obj) { this->get_override("unregisterObject")(obj); } - - void update(const std::vector & other_objs) { this->get_override("update")(other_objs); } - void update(CollisionObject* obj) { this->get_override("update")(obj); } + + void registerObjects(const std::vector &other_objs) { + this->get_override("registerObjects")(other_objs); + } + void registerObject(CollisionObject *obj) { + this->get_override("registerObjects")(obj); + } + void unregisterObject(CollisionObject *obj) { + this->get_override("unregisterObject")(obj); + } + + void update(const std::vector &other_objs) { + this->get_override("update")(other_objs); + } + void update(CollisionObject *obj) { this->get_override("update")(obj); } void update() { this->get_override("update")(); } - + void setup() { this->get_override("setup")(); } void clear() { this->get_override("clear")(); } - - std::vector getObjects() const { return this->get_override("getObjects")(); } - - void collide(CollisionCallBackBase * callback) const - { this->get_override("collide")(callback); } - void collide(CollisionObject * obj, CollisionCallBackBase * callback) const - { this->get_override("collide")(obj,callback); } - void collide(BroadPhaseCollisionManager * other_manager, CollisionCallBackBase * callback) const - { this->get_override("collide")(other_manager,callback); } - - void distance(DistanceCallBackBase * callback) const - { this->get_override("distance")(callback); } - void distance(CollisionObject * obj, DistanceCallBackBase * callback) const - { this->get_override("collide")(obj,callback); } - void distance(BroadPhaseCollisionManager * other_manager, DistanceCallBackBase * callback) const - { this->get_override("collide")(other_manager,callback); } + + std::vector getObjects() const { + return this->get_override("getObjects")(); + } + + void collide(CollisionCallBackBase *callback) const { + this->get_override("collide")(callback); + } + void collide(CollisionObject *obj, CollisionCallBackBase *callback) const { + this->get_override("collide")(obj, callback); + } + void collide(BroadPhaseCollisionManager *other_manager, + CollisionCallBackBase *callback) const { + this->get_override("collide")(other_manager, callback); + } + + void distance(DistanceCallBackBase *callback) const { + this->get_override("distance")(callback); + } + void distance(CollisionObject *obj, DistanceCallBackBase *callback) const { + this->get_override("collide")(obj, callback); + } + void distance(BroadPhaseCollisionManager *other_manager, + DistanceCallBackBase *callback) const { + this->get_override("collide")(other_manager, callback); + } bool empty() const { return this->get_override("empty")(); } size_t size() const { return this->get_override("size")(); } - - static void expose() - { - bp::class_("BroadPhaseCollisionManager",bp::no_init) - .def("registerObjects", - bp::pure_virtual(&Base::registerObjects), - doxygen::member_func_doc(&Base::registerObjects), - bp::with_custodian_and_ward_postcall<1,2>()) - .def("registerObject", - bp::pure_virtual(&Base::registerObject), - doxygen::member_func_doc(&Base::registerObject), - bp::with_custodian_and_ward_postcall<1,2>()) - .def("unregisterObject", - bp::pure_virtual(&Base::unregisterObject), - doxygen::member_func_doc(&Base::unregisterObject)) - - .def("update", - bp::pure_virtual((void (Base::*)())&Base::update), - doxygen::member_func_doc((void (Base::*)())(&Base::update))) - .def("update", - bp::pure_virtual((void (Base::*)(const std::vector &))&Base::update), - doxygen::member_func_doc((void (Base::*)(const std::vector&))(&Base::update)), - bp::with_custodian_and_ward_postcall<1,2>()) - .def("update", - bp::pure_virtual((void (Base::*)(CollisionObject* obj))&Base::update), - doxygen::member_func_doc((void (Base::*)(CollisionObject* obj))(&Base::update)), - bp::with_custodian_and_ward_postcall<1,2>()) - - .def("setup", - bp::pure_virtual(&Base::setup), - doxygen::member_func_doc(&Base::setup)) - .def("clear", - bp::pure_virtual(&Base::clear), - doxygen::member_func_doc(&Base::clear) - ) - .def("empty", - bp::pure_virtual(&Base::empty), - doxygen::member_func_doc(&Base::empty)) - .def("size", - bp::pure_virtual(&Base::size), - doxygen::member_func_doc(&Base::size)) - - .def("getObjects", - bp::pure_virtual((std::vector (Base::*)() const)&Base::getObjects), - doxygen::member_func_doc((std::vector (Base::*)() const)&Base::getObjects), - bp::with_custodian_and_ward_postcall<0,1>()) - - .def("collide", - bp::pure_virtual((void (Base::*)(CollisionCallBackBase *) const)&Base::collide), - doxygen::member_func_doc((void (Base::*)(CollisionCallBackBase *) const)&Base::collide)) - .def("collide", - bp::pure_virtual((void (Base::*)(CollisionObject *, CollisionCallBackBase *) const)&Base::collide), - doxygen::member_func_doc((void (Base::*)(CollisionObject *, CollisionCallBackBase *) const)&Base::collide)) - .def("collide", - bp::pure_virtual((void (Base::*)(BroadPhaseCollisionManager *, CollisionCallBackBase *) const)&Base::collide), - doxygen::member_func_doc((void (Base::*)(BroadPhaseCollisionManager *, CollisionCallBackBase *) const)&Base::collide)) - - .def("distance", - bp::pure_virtual((void (Base::*)(DistanceCallBackBase *) const)&Base::distance), - doxygen::member_func_doc((void (Base::*)(DistanceCallBackBase *) const)&Base::distance)) - .def("distance", - bp::pure_virtual((void (Base::*)(CollisionObject *, DistanceCallBackBase *) const)&Base::distance), - doxygen::member_func_doc((void (Base::*)(CollisionObject *, DistanceCallBackBase *) const)&Base::distance)) - .def("distance", - bp::pure_virtual((void (Base::*)(BroadPhaseCollisionManager *, DistanceCallBackBase *) const)&Base::distance), - doxygen::member_func_doc((void (Base::*)(BroadPhaseCollisionManager *, DistanceCallBackBase *) const)&Base::distance)) - ; + + static void expose() { + bp::class_( + "BroadPhaseCollisionManager", bp::no_init) + .def("registerObjects", bp::pure_virtual(&Base::registerObjects), + doxygen::member_func_doc(&Base::registerObjects), + bp::with_custodian_and_ward_postcall<1, 2>()) + .def("registerObject", bp::pure_virtual(&Base::registerObject), + doxygen::member_func_doc(&Base::registerObject), + bp::with_custodian_and_ward_postcall<1, 2>()) + .def("unregisterObject", bp::pure_virtual(&Base::unregisterObject), + doxygen::member_func_doc(&Base::unregisterObject)) + + .def("update", bp::pure_virtual((void(Base::*)()) & Base::update), + doxygen::member_func_doc((void(Base::*)())(&Base::update))) + .def("update", + bp::pure_virtual( + (void(Base::*)(const std::vector &)) & + Base::update), + doxygen::member_func_doc((void(Base::*)( + const std::vector &))(&Base::update)), + bp::with_custodian_and_ward_postcall<1, 2>()) + .def("update", + bp::pure_virtual((void(Base::*)(CollisionObject * obj)) & + Base::update), + doxygen::member_func_doc( + (void(Base::*)(CollisionObject * obj))(&Base::update)), + bp::with_custodian_and_ward_postcall<1, 2>()) + + .def("setup", bp::pure_virtual(&Base::setup), + doxygen::member_func_doc(&Base::setup)) + .def("clear", bp::pure_virtual(&Base::clear), + doxygen::member_func_doc(&Base::clear)) + .def("empty", bp::pure_virtual(&Base::empty), + doxygen::member_func_doc(&Base::empty)) + .def("size", bp::pure_virtual(&Base::size), + doxygen::member_func_doc(&Base::size)) + + .def( + "getObjects", + bp::pure_virtual((std::vector(Base::*)() const) & + Base::getObjects), + doxygen::member_func_doc( + (std::vector(Base::*)() const) & + Base::getObjects), + bp::with_custodian_and_ward_postcall<0, 1>()) + + .def( + "collide", + bp::pure_virtual((void(Base::*)(CollisionCallBackBase *) const) & + Base::collide), + doxygen::member_func_doc( + (void(Base::*)(CollisionCallBackBase *) const) & Base::collide)) + .def("collide", + bp::pure_virtual((void(Base::*)(CollisionObject *, + CollisionCallBackBase *) const) & + Base::collide), + doxygen::member_func_doc( + (void(Base::*)(CollisionObject *, CollisionCallBackBase *) + const) & + Base::collide)) + .def("collide", + bp::pure_virtual((void(Base::*)(BroadPhaseCollisionManager *, + CollisionCallBackBase *) const) & + Base::collide), + doxygen::member_func_doc( + (void(Base::*)(BroadPhaseCollisionManager *, + CollisionCallBackBase *) const) & + Base::collide)) + + .def( + "distance", + bp::pure_virtual((void(Base::*)(DistanceCallBackBase *) const) & + Base::distance), + doxygen::member_func_doc( + (void(Base::*)(DistanceCallBackBase *) const) & Base::distance)) + .def("distance", + bp::pure_virtual((void(Base::*)(CollisionObject *, + DistanceCallBackBase *) const) & + Base::distance), + doxygen::member_func_doc( + (void(Base::*)(CollisionObject *, DistanceCallBackBase *) + const) & + Base::distance)) + .def("distance", + bp::pure_virtual((void(Base::*)(BroadPhaseCollisionManager *, + DistanceCallBackBase *) const) & + Base::distance), + doxygen::member_func_doc( + (void(Base::*)(BroadPhaseCollisionManager *, + DistanceCallBackBase *) const) & + Base::distance)); } - - template - static void exposeDerived() - { + + template + static void exposeDerived() { std::string class_name = boost::typeindex::type_id().pretty_name(); boost::algorithm::replace_all(class_name, "hpp::fcl::", ""); - - bp::class_ >(class_name.c_str(),bp::no_init) - .def(dv::init()) - ; + + bp::class_ >( + class_name.c_str(), bp::no_init) + .def(dv::init()); } - - -}; // BroadPhaseCollisionManagerWrapper -}} +}; // BroadPhaseCollisionManagerWrapper + +} // namespace fcl +} // namespace hpp -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH +#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH diff --git a/python/broadphase/fwd.hh b/python/broadphase/fwd.hh index 44d75aa87..2f8e6bc57 100644 --- a/python/broadphase/fwd.hh +++ b/python/broadphase/fwd.hh @@ -14,7 +14,7 @@ namespace python { void exposeBroadPhase(); } -} -} +} // namespace fcl +} // namespace hpp -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_FWD_HH +#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_FWD_HH diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index e4f479cc7..598356d9a 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -69,153 +69,140 @@ using boost::noncopyable; typedef std::vector Vec3fs; typedef std::vector Triangles; -struct BVHModelBaseWrapper -{ - typedef Eigen::Matrix RowMatrixX3; +struct BVHModelBaseWrapper { + typedef Eigen::Matrix RowMatrixX3; typedef Eigen::Map MapRowMatrixX3; typedef Eigen::Ref RefRowMatrixX3; - - static Vec3f & vertex (BVHModelBase & bvh, unsigned int i) - { + + static Vec3f& vertex(BVHModelBase& bvh, unsigned int i) { if (i >= bvh.num_vertices) throw std::out_of_range("index is out of range"); return bvh.vertices[i]; } - static RefRowMatrixX3 vertices(BVHModelBase & bvh) - { - return MapRowMatrixX3(bvh.vertices[0].data(),bvh.num_vertices,3); + static RefRowMatrixX3 vertices(BVHModelBase& bvh) { + return MapRowMatrixX3(bvh.vertices[0].data(), bvh.num_vertices, 3); } - - static Triangle tri_indices (const BVHModelBase& bvh, unsigned int i) - { + + static Triangle tri_indices(const BVHModelBase& bvh, unsigned int i) { if (i >= bvh.num_tris) throw std::out_of_range("index is out of range"); return bvh.tri_indices[i]; } }; template -void exposeBVHModel (const std::string& bvname) -{ +void exposeBVHModel(const std::string& bvname) { typedef BVHModel BVH; const std::string type_name = "BVHModel" + bvname; - class_ , shared_ptr > - (type_name.c_str(), doxygen::class_doc(), no_init) - .def (dv::init()) - .def (dv::init()) - .DEF_CLASS_FUNC(BVH,getNumBVs) - .DEF_CLASS_FUNC(BVH,makeParentRelative) - .DEF_CLASS_FUNC(BVHModelBase,memUsage) - .def ("clone", &BVH::clone, - doxygen::member_func_doc(&BVH::clone), - return_value_policy()) - ; + class_, shared_ptr >( + type_name.c_str(), doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) + .DEF_CLASS_FUNC(BVH, getNumBVs) + .DEF_CLASS_FUNC(BVH, makeParentRelative) + .DEF_CLASS_FUNC(BVHModelBase, memUsage) + .def("clone", &BVH::clone, doxygen::member_func_doc(&BVH::clone), + return_value_policy()); } template -void exposeHeightField (const std::string & bvname) -{ +void exposeHeightField(const std::string& bvname) { typedef HeightField Geometry; typedef typename Geometry::Base Base; typedef typename Geometry::Node Node; const std::string type_name = "HeightField" + bvname; - class_ , shared_ptr > - (type_name.c_str(), doxygen::class_doc(), no_init) - .def (dv::init< HeightField >()) - .def (dv::init< HeightField , const HeightField &>()) - .def (dv::init< HeightField , FCL_REAL, FCL_REAL, const MatrixXf &, bp::optional >()) - - .DEF_CLASS_FUNC(Geometry,getXDim) - .DEF_CLASS_FUNC(Geometry,getYDim) - .DEF_CLASS_FUNC(Geometry,getMinHeight) - .DEF_CLASS_FUNC(Geometry,getMaxHeight) - .DEF_CLASS_FUNC(Geometry,getNodeType) - .DEF_CLASS_FUNC(Geometry,updateHeights) - - .def("clone", &Geometry::clone, - doxygen::member_func_doc(&Geometry::clone), - return_value_policy()) - .def("getXGrid", &Geometry::getXGrid, - doxygen::member_func_doc(&Geometry::getXGrid), - bp::return_value_policy()) - .def("getYGrid", &Geometry::getYGrid, - doxygen::member_func_doc(&Geometry::getYGrid), - bp::return_value_policy()) - .def("getHeights", &Geometry::getHeights, - doxygen::member_func_doc(&Geometry::getHeights), - bp::return_value_policy()) - .def("getBV", (Node & (Geometry::*)(unsigned int))&Geometry::getBV, - doxygen::member_func_doc((Node & (Geometry::*)(unsigned int))&Geometry::getBV), - bp::return_internal_reference<>()) - ; + class_, shared_ptr >( + type_name.c_str(), doxygen::class_doc(), no_init) + .def(dv::init >()) + .def(dv::init, const HeightField&>()) + .def(dv::init, FCL_REAL, FCL_REAL, const MatrixXf&, + bp::optional >()) + + .DEF_CLASS_FUNC(Geometry, getXDim) + .DEF_CLASS_FUNC(Geometry, getYDim) + .DEF_CLASS_FUNC(Geometry, getMinHeight) + .DEF_CLASS_FUNC(Geometry, getMaxHeight) + .DEF_CLASS_FUNC(Geometry, getNodeType) + .DEF_CLASS_FUNC(Geometry, updateHeights) + + .def("clone", &Geometry::clone, + doxygen::member_func_doc(&Geometry::clone), + return_value_policy()) + .def("getXGrid", &Geometry::getXGrid, + doxygen::member_func_doc(&Geometry::getXGrid), + bp::return_value_policy()) + .def("getYGrid", &Geometry::getYGrid, + doxygen::member_func_doc(&Geometry::getYGrid), + bp::return_value_policy()) + .def("getHeights", &Geometry::getHeights, + doxygen::member_func_doc(&Geometry::getHeights), + bp::return_value_policy()) + .def("getBV", (Node & (Geometry::*)(unsigned int)) & Geometry::getBV, + doxygen::member_func_doc((Node & (Geometry::*)(unsigned int)) & + Geometry::getBV), + bp::return_internal_reference<>()); } -struct ConvexBaseWrapper -{ - typedef Eigen::Matrix RowMatrixX3; +struct ConvexBaseWrapper { + typedef Eigen::Matrix RowMatrixX3; typedef Eigen::Map MapRowMatrixX3; typedef Eigen::Ref RefRowMatrixX3; - - static Vec3f & point (const ConvexBase& convex, unsigned int i) - { - if (i >= convex.num_points) throw std::out_of_range("index is out of range"); + + static Vec3f& point(const ConvexBase& convex, unsigned int i) { + if (i >= convex.num_points) + throw std::out_of_range("index is out of range"); return convex.points[i]; } - - static RefRowMatrixX3 points (const ConvexBase& convex) - { - return MapRowMatrixX3(convex.points[0].data(),convex.num_points,3); + + static RefRowMatrixX3 points(const ConvexBase& convex) { + return MapRowMatrixX3(convex.points[0].data(), convex.num_points, 3); } - static list neighbors (const ConvexBase& convex, unsigned int i) - { - if (i >= convex.num_points) throw std::out_of_range("index is out of range"); + static list neighbors(const ConvexBase& convex, unsigned int i) { + if (i >= convex.num_points) + throw std::out_of_range("index is out of range"); list n; for (unsigned char j = 0; j < convex.neighbors[i].count(); ++j) - n.append (convex.neighbors[i][j]); + n.append(convex.neighbors[i][j]); return n; } - static ConvexBase* convexHull (const Vec3fs& points, bool keepTri, - const char* qhullCommand) - { - return ConvexBase::convexHull (points.data(), (unsigned int)points.size(), keepTri, - qhullCommand); + static ConvexBase* convexHull(const Vec3fs& points, bool keepTri, + const char* qhullCommand) { + return ConvexBase::convexHull(points.data(), (unsigned int)points.size(), + keepTri, qhullCommand); } }; template -struct ConvexWrapper -{ +struct ConvexWrapper { typedef Convex Convex_t; - static PolygonT polygons (const Convex_t& convex, unsigned int i) - { - if (i >= convex.num_polygons) throw std::out_of_range("index is out of range"); + static PolygonT polygons(const Convex_t& convex, unsigned int i) { + if (i >= convex.num_polygons) + throw std::out_of_range("index is out of range"); return convex.polygons[i]; } - static shared_ptr constructor (const Vec3fs& _points, const Triangles& _tris) - { + static shared_ptr constructor(const Vec3fs& _points, + const Triangles& _tris) { Vec3f* points = new Vec3f[_points.size()]; for (std::size_t i = 0; i < _points.size(); ++i) points[i] = _points[i]; Triangle* tris = new Triangle[_tris.size()]; for (std::size_t i = 0; i < _tris.size(); ++i) tris[i] = _tris[i]; - return shared_ptr(new Convex_t(true, points, (unsigned int)_points.size(), - tris, (unsigned int)_tris.size())); + return shared_ptr(new Convex_t(true, points, + (unsigned int)_points.size(), tris, + (unsigned int)_tris.size())); } }; - -template -void defComputeMemoryFootprint() -{ - doxygen::def("computeMemoryFootprint",&computeMemoryFootprint); +template +void defComputeMemoryFootprint() { + doxygen::def("computeMemoryFootprint", &computeMemoryFootprint); } -void exposeComputeMemoryFootprint() -{ +void exposeComputeMemoryFootprint() { defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); @@ -225,418 +212,426 @@ void exposeComputeMemoryFootprint() defComputeMemoryFootprint(); defComputeMemoryFootprint(); defComputeMemoryFootprint(); - - defComputeMemoryFootprint< BVHModel >(); - defComputeMemoryFootprint< BVHModel >(); - defComputeMemoryFootprint< BVHModel >(); + + defComputeMemoryFootprint >(); + defComputeMemoryFootprint >(); + defComputeMemoryFootprint >(); } -void exposeShapes () -{ - class_ , shared_ptr, noncopyable> - ("ShapeBase", doxygen::class_doc(), no_init) - //.def ("getObjectType", &CollisionGeometry::getObjectType) - ; - - class_ , shared_ptr > - ("Box", doxygen::class_doc(), no_init) - .def (dv::init()) - .def (dv::init()) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB(Box,halfSide) - .def ("clone", &Box::clone, - doxygen::member_func_doc(&Box::clone), - return_value_policy()) - ; - - class_ , shared_ptr > - ("Capsule", doxygen::class_doc(), no_init) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (Capsule, radius) - .DEF_RW_CLASS_ATTRIB (Capsule, halfLength) - .def ("clone", &Capsule::clone, - doxygen::member_func_doc(&Capsule::clone), - return_value_policy()) - ; - - class_ , shared_ptr > - ("Cone", doxygen::class_doc(), no_init) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (Cone, radius) - .DEF_RW_CLASS_ATTRIB (Cone, halfLength) - .def ("clone", &Cone::clone, - doxygen::member_func_doc(&Cone::clone), - return_value_policy()) - ; - - class_ , shared_ptr, noncopyable> - ("ConvexBase", doxygen::class_doc(), no_init) - .DEF_RO_CLASS_ATTRIB (ConvexBase, center) - .DEF_RO_CLASS_ATTRIB (ConvexBase, num_points) - .def ("point", &ConvexBaseWrapper::point, - bp::args("self","index"),"Retrieve the point given by its index.", - bp::return_internal_reference<>()) - .def ("points", &ConvexBaseWrapper::point, - bp::args("self","index"),"Retrieve the point given by its index.", - ::hpp::fcl::python::deprecated_member< bp::return_internal_reference<> >()) - .def ("points", &ConvexBaseWrapper::points, - bp::args("self"),"Retrieve all the points.", - bp::with_custodian_and_ward_postcall<0,1>()) - // .add_property ("points", - // bp::make_function(&ConvexBaseWrapper::points,bp::with_custodian_and_ward_postcall<0,1>()), - // "Points of the convex.") - .def ("neighbors", &ConvexBaseWrapper::neighbors) - .def ("convexHull", &ConvexBaseWrapper::convexHull, - doxygen::member_func_doc(&ConvexBase::convexHull), - return_value_policy()) - .staticmethod("convexHull") - .def ("clone", &ConvexBase::clone, - doxygen::member_func_doc(&ConvexBase::clone), - return_value_policy()) - ; - - class_ , bases, shared_ptr >, noncopyable> - ("Convex", doxygen::class_doc< Convex >(), no_init) - .def("__init__", make_constructor(&ConvexWrapper::constructor)) - .DEF_RO_CLASS_ATTRIB (Convex, num_polygons) - .def ("polygons", &ConvexWrapper::polygons) - ; - - class_ , shared_ptr > - ("Cylinder", doxygen::class_doc(), no_init) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (Cylinder, radius) - .DEF_RW_CLASS_ATTRIB (Cylinder, halfLength) - .def ("clone", &Cylinder::clone, - doxygen::member_func_doc(&Cylinder::clone), - return_value_policy()) - ; - - class_ , shared_ptr > - ("Halfspace", doxygen::class_doc(), no_init) - .def (dv::init()) - .def (dv::init()) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (Halfspace, n) - .DEF_RW_CLASS_ATTRIB (Halfspace, d) - .def ("clone", &Halfspace::clone, - doxygen::member_func_doc(&Halfspace::clone), - return_value_policy()) - ; - - class_ , shared_ptr > - ("Plane", doxygen::class_doc(), no_init) - .def (dv::init()) - .def (dv::init()) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (Plane, n) - .DEF_RW_CLASS_ATTRIB (Plane, d) - .def ("clone", &Plane::clone, - doxygen::member_func_doc(&Plane::clone), - return_value_policy()) - ; - - class_ , shared_ptr > - ("Sphere", doxygen::class_doc(), no_init) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (Sphere, radius) - .def ("clone", &Sphere::clone, - doxygen::member_func_doc(&Sphere::clone), - return_value_policy()) - ; - - class_ , shared_ptr > - ("Ellipsoid", doxygen::class_doc(), no_init) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (Ellipsoid, radii) - .def ("clone", &Ellipsoid::clone, - doxygen::member_func_doc(&Ellipsoid::clone), - return_value_policy()) - ; - - class_ , shared_ptr > - ("TriangleP", doxygen::class_doc(), no_init) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (TriangleP, a) - .DEF_RW_CLASS_ATTRIB (TriangleP, b) - .DEF_RW_CLASS_ATTRIB (TriangleP, c) - .def ("clone", &TriangleP::clone, - doxygen::member_func_doc(&TriangleP::clone), - return_value_policy()) - ; +void exposeShapes() { + class_, shared_ptr, + noncopyable>("ShapeBase", doxygen::class_doc(), no_init) + //.def ("getObjectType", &CollisionGeometry::getObjectType) + ; + class_, shared_ptr >( + "Box", doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(Box, halfSide) + .def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone), + return_value_policy()); + + class_, shared_ptr >( + "Capsule", doxygen::class_doc(), no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(Capsule, radius) + .DEF_RW_CLASS_ATTRIB(Capsule, halfLength) + .def("clone", &Capsule::clone, doxygen::member_func_doc(&Capsule::clone), + return_value_policy()); + + class_, shared_ptr >( + "Cone", doxygen::class_doc(), no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(Cone, radius) + .DEF_RW_CLASS_ATTRIB(Cone, halfLength) + .def("clone", &Cone::clone, doxygen::member_func_doc(&Cone::clone), + return_value_policy()); + + class_, shared_ptr, noncopyable>( + "ConvexBase", doxygen::class_doc(), no_init) + .DEF_RO_CLASS_ATTRIB(ConvexBase, center) + .DEF_RO_CLASS_ATTRIB(ConvexBase, num_points) + .def("point", &ConvexBaseWrapper::point, bp::args("self", "index"), + "Retrieve the point given by its index.", + bp::return_internal_reference<>()) + .def("points", &ConvexBaseWrapper::point, bp::args("self", "index"), + "Retrieve the point given by its index.", + ::hpp::fcl::python::deprecated_member< + bp::return_internal_reference<> >()) + .def("points", &ConvexBaseWrapper::points, bp::args("self"), + "Retrieve all the points.", + bp::with_custodian_and_ward_postcall<0, 1>()) + // .add_property ("points", + // bp::make_function(&ConvexBaseWrapper::points,bp::with_custodian_and_ward_postcall<0,1>()), + // "Points of the convex.") + .def("neighbors", &ConvexBaseWrapper::neighbors) + .def("convexHull", &ConvexBaseWrapper::convexHull, + doxygen::member_func_doc(&ConvexBase::convexHull), + return_value_policy()) + .staticmethod("convexHull") + .def("clone", &ConvexBase::clone, + doxygen::member_func_doc(&ConvexBase::clone), + return_value_policy()); + + class_, bases, shared_ptr >, + noncopyable>("Convex", doxygen::class_doc >(), + no_init) + .def("__init__", make_constructor(&ConvexWrapper::constructor)) + .DEF_RO_CLASS_ATTRIB(Convex, num_polygons) + .def("polygons", &ConvexWrapper::polygons); + + class_, shared_ptr >( + "Cylinder", doxygen::class_doc(), no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(Cylinder, radius) + .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength) + .def("clone", &Cylinder::clone, + doxygen::member_func_doc(&Cylinder::clone), + return_value_policy()); + + class_, shared_ptr >( + "Halfspace", doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(Halfspace, n) + .DEF_RW_CLASS_ATTRIB(Halfspace, d) + .def("clone", &Halfspace::clone, + doxygen::member_func_doc(&Halfspace::clone), + return_value_policy()); + + class_, shared_ptr >( + "Plane", doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(Plane, n) + .DEF_RW_CLASS_ATTRIB(Plane, d) + .def("clone", &Plane::clone, doxygen::member_func_doc(&Plane::clone), + return_value_policy()); + + class_, shared_ptr >( + "Sphere", doxygen::class_doc(), no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(Sphere, radius) + .def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone), + return_value_policy()); + + class_, shared_ptr >( + "Ellipsoid", doxygen::class_doc(), no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) + .def("clone", &Ellipsoid::clone, + doxygen::member_func_doc(&Ellipsoid::clone), + return_value_policy()); + + class_, shared_ptr >( + "TriangleP", doxygen::class_doc(), no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(TriangleP, a) + .DEF_RW_CLASS_ATTRIB(TriangleP, b) + .DEF_RW_CLASS_ATTRIB(TriangleP, c) + .def("clone", &TriangleP::clone, + doxygen::member_func_doc(&TriangleP::clone), + return_value_policy()); } -boost::python::tuple AABB_distance_proxy(const AABB & self, const AABB & other) -{ - Vec3f P,Q; - FCL_REAL distance = self.distance(other,&P,&Q); - return boost::python::make_tuple(distance,P,Q); +boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) { + Vec3f P, Q; + FCL_REAL distance = self.distance(other, &P, &Q); + return boost::python::make_tuple(distance, P, Q); } -void exposeCollisionGeometries () -{ - +void exposeCollisionGeometries() { enum_("BVHModelType") - .value ("BVH_MODEL_UNKNOWN" , BVH_MODEL_UNKNOWN) - .value ("BVH_MODEL_TRIANGLES" , BVH_MODEL_TRIANGLES) - .value ("BVH_MODEL_POINTCLOUD" , BVH_MODEL_POINTCLOUD) - .export_values() - ; - + .value("BVH_MODEL_UNKNOWN", BVH_MODEL_UNKNOWN) + .value("BVH_MODEL_TRIANGLES", BVH_MODEL_TRIANGLES) + .value("BVH_MODEL_POINTCLOUD", BVH_MODEL_POINTCLOUD) + .export_values(); + enum_("BVHBuildState") - .value("BVH_BUILD_STATE_EMPTY",BVH_BUILD_STATE_EMPTY) - .value("BVH_BUILD_STATE_BEGUN",BVH_BUILD_STATE_BEGUN) - .value("BVH_BUILD_STATE_PROCESSED",BVH_BUILD_STATE_PROCESSED) - .value("BVH_BUILD_STATE_UPDATE_BEGUN",BVH_BUILD_STATE_UPDATE_BEGUN) - .value("BVH_BUILD_STATE_UPDATED",BVH_BUILD_STATE_UPDATED) - .value("BVH_BUILD_STATE_REPLACE_BEGUN",BVH_BUILD_STATE_REPLACE_BEGUN) - .export_values() - ; - - if(!eigenpy::register_symbolic_link_to_registered_type()) - { + .value("BVH_BUILD_STATE_EMPTY", BVH_BUILD_STATE_EMPTY) + .value("BVH_BUILD_STATE_BEGUN", BVH_BUILD_STATE_BEGUN) + .value("BVH_BUILD_STATE_PROCESSED", BVH_BUILD_STATE_PROCESSED) + .value("BVH_BUILD_STATE_UPDATE_BEGUN", BVH_BUILD_STATE_UPDATE_BEGUN) + .value("BVH_BUILD_STATE_UPDATED", BVH_BUILD_STATE_UPDATED) + .value("BVH_BUILD_STATE_REPLACE_BEGUN", BVH_BUILD_STATE_REPLACE_BEGUN) + .export_values(); + + if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("OBJECT_TYPE") - .value ("OT_UNKNOWN", OT_UNKNOWN) - .value ("OT_BVH" , OT_BVH) - .value ("OT_GEOM" , OT_GEOM) - .value ("OT_OCTREE" , OT_OCTREE) - .value ("OT_HFIELD" , OT_HFIELD) - .export_values() - ; + .value("OT_UNKNOWN", OT_UNKNOWN) + .value("OT_BVH", OT_BVH) + .value("OT_GEOM", OT_GEOM) + .value("OT_OCTREE", OT_OCTREE) + .value("OT_HFIELD", OT_HFIELD) + .export_values(); } - - if(!eigenpy::register_symbolic_link_to_registered_type()) - { + + if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("NODE_TYPE") - .value ("BV_UNKNOWN", BV_UNKNOWN) - .value ("BV_AABB" , BV_AABB) - .value ("BV_OBB" , BV_OBB) - .value ("BV_RSS" , BV_RSS) - .value ("BV_kIOS" , BV_kIOS) - .value ("BV_OBBRSS", BV_OBBRSS) - .value ("BV_KDOP16", BV_KDOP16) - .value ("BV_KDOP18", BV_KDOP18) - .value ("BV_KDOP24", BV_KDOP24) - .value ("GEOM_BOX" , GEOM_BOX) - .value ("GEOM_SPHERE" , GEOM_SPHERE) - .value ("GEOM_ELLIPSOID" , GEOM_ELLIPSOID) - .value ("GEOM_CAPSULE" , GEOM_CAPSULE) - .value ("GEOM_CONE" , GEOM_CONE) - .value ("GEOM_CYLINDER" , GEOM_CYLINDER) - .value ("GEOM_CONVEX" , GEOM_CONVEX) - .value ("GEOM_PLANE" , GEOM_PLANE) - .value ("GEOM_HALFSPACE", GEOM_HALFSPACE) - .value ("GEOM_TRIANGLE" , GEOM_TRIANGLE) - .value ("GEOM_OCTREE" , GEOM_OCTREE) - .value ("HF_AABB" , HF_AABB) - .value ("HF_OBBRSS" , HF_OBBRSS) - .export_values() - ; + .value("BV_UNKNOWN", BV_UNKNOWN) + .value("BV_AABB", BV_AABB) + .value("BV_OBB", BV_OBB) + .value("BV_RSS", BV_RSS) + .value("BV_kIOS", BV_kIOS) + .value("BV_OBBRSS", BV_OBBRSS) + .value("BV_KDOP16", BV_KDOP16) + .value("BV_KDOP18", BV_KDOP18) + .value("BV_KDOP24", BV_KDOP24) + .value("GEOM_BOX", GEOM_BOX) + .value("GEOM_SPHERE", GEOM_SPHERE) + .value("GEOM_ELLIPSOID", GEOM_ELLIPSOID) + .value("GEOM_CAPSULE", GEOM_CAPSULE) + .value("GEOM_CONE", GEOM_CONE) + .value("GEOM_CYLINDER", GEOM_CYLINDER) + .value("GEOM_CONVEX", GEOM_CONVEX) + .value("GEOM_PLANE", GEOM_PLANE) + .value("GEOM_HALFSPACE", GEOM_HALFSPACE) + .value("GEOM_TRIANGLE", GEOM_TRIANGLE) + .value("GEOM_OCTREE", GEOM_OCTREE) + .value("HF_AABB", HF_AABB) + .value("HF_OBBRSS", HF_OBBRSS) + .export_values(); } - + class_("AABB", - "A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points", + "A class describing the AABB collision structure, which is a " + "box in 3D space determined by two diagonal points", no_init) - .def(init<>(bp::arg("self"), "Default constructor")) - .def(init(bp::args("self","other"), "Copy constructor")) - .def(init(bp::args("self","v"),"Creating an AABB at position v with zero size.")) - .def(init(bp::args("self","a","b"),"Creating an AABB with two endpoints a and b.")) - .def(init(bp::args("self","core","delta"),"Creating an AABB centered as core and is of half-dimension delta.")) - .def(init(bp::args("self","a","b","c"),"Creating an AABB contains three points.")) - - .def("contain", - (bool (AABB::*)(const Vec3f &) const)&AABB::contain, - bp::args("self","p"), - "Check whether the AABB contains a point p.") - .def("contain", - (bool (AABB::*)(const AABB &) const)&AABB::contain, - bp::args("self","other"), - "Check whether the AABB contains another AABB.") - - .def("overlap", - (bool (AABB::*)(const AABB &) const)&AABB::overlap, - bp::args("self","other"), - "Check whether two AABB are overlap.") - .def("overlap", - (bool (AABB::*)(const AABB&, AABB&) const)&AABB::overlap, - bp::args("self","other","overlapping_part"), - "Check whether two AABB are overlaping and return the overloaping part if true.") - - .def("distance", - (FCL_REAL (AABB::*)(const AABB &) const)&AABB::distance, - bp::args("self","other"), - "Distance between two AABBs.") -// .def("distance", -// AABB_distance_proxy, -// bp::args("self","other"), -// "Distance between two AABBs.") - - .add_property("min_", - bp::make_function(+[](AABB & self) -> Vec3f & { return self.min_; }, bp::return_internal_reference<>()), - bp::make_function(+[](AABB & self, const Vec3f & min_) -> void { self.min_ = min_; }), - "The min point in the AABB.") - .add_property("max_", - bp::make_function(+[](AABB & self) -> Vec3f & { return self.max_; }, bp::return_internal_reference<>()), - bp::make_function(+[](AABB & self, const Vec3f & max_) -> void { self.max_ = max_; }), - "The max point in the AABB.") - - .def(bp::self == bp::self) - .def(bp::self != bp::self) - - .def(bp::self + bp::self) - .def(bp::self += bp::self) - .def(bp::self += Vec3f()) - - .def("size",&AABB::volume,bp::arg("self"),"Size of the AABB.") - .def("center",&AABB::center,bp::arg("self"),"Center of the AABB.") - .def("width",&AABB::width,bp::arg("self"),"Width of the AABB.") - .def("height",&AABB::height,bp::arg("self"),"Height of the AABB.") - .def("depth",&AABB::depth,bp::arg("self"),"Depth of the AABB.") - .def("volume",&AABB::volume,bp::arg("self"),"Volume of the AABB.") - - .def("expand", - static_cast(&AABB::expand), -// doxygen::member_func_doc(static_cast(&AABB::expand)), -// doxygen::member_func_args(static_cast(&AABB::expand)), - bp::return_internal_reference<>()) - .def("expand", - static_cast(&AABB::expand), -// doxygen::member_func_doc(static_cast(&AABB::expand)), -// doxygen::member_func_args(static_cast(&AABB::expand)), - bp::return_internal_reference<>()) - .def("expand", - static_cast(&AABB::expand), -// doxygen::member_func_doc(static_cast(&AABB::expand)), -// doxygen::member_func_args(static_cast(&AABB::expand)), - bp::return_internal_reference<>()) - ; - - def("translate", - (AABB (*)(const AABB&, const Vec3f&))&translate, - bp::args("aabb","t"), - "Translate the center of AABB by t"); - - def("rotate", - (AABB (*)(const AABB&, const Matrix3f&))&rotate, - bp::args("aabb","R"), - "Rotate the AABB object by R"); - - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ - ("CollisionGeometry", no_init) - .def ("getObjectType", &CollisionGeometry::getObjectType) - .def ("getNodeType", &CollisionGeometry::getNodeType) - - .def ("computeLocalAABB", &CollisionGeometry::computeLocalAABB) - - .def ("computeCOM", &CollisionGeometry::computeCOM) - .def ("computeMomentofInertia", &CollisionGeometry::computeMomentofInertia) - .def ("computeVolume", &CollisionGeometry::computeVolume) - .def ("computeMomentofInertiaRelatedToCOM", &CollisionGeometry::computeMomentofInertiaRelatedToCOM) - - .def_readwrite("aabb_radius",&CollisionGeometry::aabb_radius,"AABB radius.") - .def_readwrite("aabb_center",&CollisionGeometry::aabb_center,"AABB center in local coordinate.") - .def_readwrite("aabb_local",&CollisionGeometry::aabb_local,"AABB in local coordinate, used for tight AABB when only translation transform.") - - .def("isOccupied",&CollisionGeometry::isOccupied,bp::arg("self"),"Whether the object is completely occupied.") - .def("isFree",&CollisionGeometry::isFree,bp::arg("self"),"Whether the object is completely free.") - .def("isUncertain",&CollisionGeometry::isUncertain,bp::arg("self"),"Whether the object has some uncertainty.") - - .def_readwrite("cost_density",&CollisionGeometry::cost_density,"Collision cost for unit volume.") - .def_readwrite("threshold_occupied",&CollisionGeometry::threshold_occupied,"Threshold for occupied ( >= is occupied).") - .def_readwrite("threshold_free",&CollisionGeometry::threshold_free,"Threshold for free (<= is free).") - - .def(self == self) - .def(self != self) - ; + .def(init<>(bp::arg("self"), "Default constructor")) + .def(init(bp::args("self", "other"), "Copy constructor")) + .def(init(bp::args("self", "v"), + "Creating an AABB at position v with zero size.")) + .def(init(bp::args("self", "a", "b"), + "Creating an AABB with two endpoints a and b.")) + .def(init( + bp::args("self", "core", "delta"), + "Creating an AABB centered as core and is of half-dimension delta.")) + .def(init(bp::args("self", "a", "b", "c"), + "Creating an AABB contains three points.")) + + .def("contain", (bool(AABB::*)(const Vec3f&) const) & AABB::contain, + bp::args("self", "p"), "Check whether the AABB contains a point p.") + .def("contain", (bool(AABB::*)(const AABB&) const) & AABB::contain, + bp::args("self", "other"), + "Check whether the AABB contains another AABB.") + + .def("overlap", (bool(AABB::*)(const AABB&) const) & AABB::overlap, + bp::args("self", "other"), "Check whether two AABB are overlap.") + .def("overlap", (bool(AABB::*)(const AABB&, AABB&) const) & AABB::overlap, + bp::args("self", "other", "overlapping_part"), + "Check whether two AABB are overlaping and return the overloaping " + "part if true.") + + .def("distance", (FCL_REAL(AABB::*)(const AABB&) const) & AABB::distance, + bp::args("self", "other"), "Distance between two AABBs.") + // .def("distance", + // AABB_distance_proxy, + // bp::args("self","other"), + // "Distance between two AABBs.") + + .add_property( + "min_", + bp::make_function( + +[](AABB& self) -> Vec3f& { return self.min_; }, + bp::return_internal_reference<>()), + bp::make_function( + +[](AABB& self, const Vec3f& min_) -> void { self.min_ = min_; }), + "The min point in the AABB.") + .add_property( + "max_", + bp::make_function( + +[](AABB& self) -> Vec3f& { return self.max_; }, + bp::return_internal_reference<>()), + bp::make_function( + +[](AABB& self, const Vec3f& max_) -> void { self.max_ = max_; }), + "The max point in the AABB.") + + .def(bp::self == bp::self) + .def(bp::self != bp::self) + + .def(bp::self + bp::self) + .def(bp::self += bp::self) + .def(bp::self += Vec3f()) + + .def("size", &AABB::volume, bp::arg("self"), "Size of the AABB.") + .def("center", &AABB::center, bp::arg("self"), "Center of the AABB.") + .def("width", &AABB::width, bp::arg("self"), "Width of the AABB.") + .def("height", &AABB::height, bp::arg("self"), "Height of the AABB.") + .def("depth", &AABB::depth, bp::arg("self"), "Depth of the AABB.") + .def("volume", &AABB::volume, bp::arg("self"), "Volume of the AABB.") + + .def("expand", + static_cast(&AABB::expand), + // doxygen::member_func_doc(static_cast(&AABB::expand)), + // doxygen::member_func_args(static_cast(&AABB::expand)), + bp::return_internal_reference<>()) + .def("expand", + static_cast(&AABB::expand), + // doxygen::member_func_doc(static_cast(&AABB::expand)), + // doxygen::member_func_args(static_cast(&AABB::expand)), + bp::return_internal_reference<>()) + .def("expand", static_cast(&AABB::expand), + // doxygen::member_func_doc(static_cast(&AABB::expand)), + // doxygen::member_func_args(static_cast(&AABB::expand)), + bp::return_internal_reference<>()); + + def("translate", (AABB(*)(const AABB&, const Vec3f&)) & translate, + bp::args("aabb", "t"), "Translate the center of AABB by t"); + + def("rotate", (AABB(*)(const AABB&, const Matrix3f&)) & rotate, + bp::args("aabb", "R"), "Rotate the AABB object by R"); + + if (!eigenpy::register_symbolic_link_to_registered_type< + CollisionGeometry>()) { + class_( + "CollisionGeometry", no_init) + .def("getObjectType", &CollisionGeometry::getObjectType) + .def("getNodeType", &CollisionGeometry::getNodeType) + + .def("computeLocalAABB", &CollisionGeometry::computeLocalAABB) + + .def("computeCOM", &CollisionGeometry::computeCOM) + .def("computeMomentofInertia", + &CollisionGeometry::computeMomentofInertia) + .def("computeVolume", &CollisionGeometry::computeVolume) + .def("computeMomentofInertiaRelatedToCOM", + &CollisionGeometry::computeMomentofInertiaRelatedToCOM) + + .def_readwrite("aabb_radius", &CollisionGeometry::aabb_radius, + "AABB radius.") + .def_readwrite("aabb_center", &CollisionGeometry::aabb_center, + "AABB center in local coordinate.") + .def_readwrite("aabb_local", &CollisionGeometry::aabb_local, + "AABB in local coordinate, used for tight AABB when " + "only translation transform.") + + .def("isOccupied", &CollisionGeometry::isOccupied, bp::arg("self"), + "Whether the object is completely occupied.") + .def("isFree", &CollisionGeometry::isFree, bp::arg("self"), + "Whether the object is completely free.") + .def("isUncertain", &CollisionGeometry::isUncertain, bp::arg("self"), + "Whether the object has some uncertainty.") + + .def_readwrite("cost_density", &CollisionGeometry::cost_density, + "Collision cost for unit volume.") + .def_readwrite("threshold_occupied", + &CollisionGeometry::threshold_occupied, + "Threshold for occupied ( >= is occupied).") + .def_readwrite("threshold_free", &CollisionGeometry::threshold_free, + "Threshold for free (<= is free).") + + .def(self == self) + .def(self != self); } exposeShapes(); - class_ , BVHModelPtr_t, noncopyable> - ("BVHModelBase", no_init) - .def ("vertex", &BVHModelBaseWrapper::vertex, - bp::args("self","index"),"Retrieve the vertex given by its index.", - bp::return_internal_reference<>()) - .def ("vertices", &BVHModelBaseWrapper::vertex, - bp::args("self","index"),"Retrieve the vertex given by its index.", - ::hpp::fcl::python::deprecated_member< bp::return_internal_reference<> >()) - .def ("vertices", &BVHModelBaseWrapper::vertices, - bp::args("self"),"Retrieve all the vertices.", - bp::with_custodian_and_ward_postcall<0,1>()) -// .add_property ("vertices", -// bp::make_function(&BVHModelBaseWrapper::vertices,bp::with_custodian_and_ward_postcall<0,1>()), -// "Vertices of the BVH.") - .def ("tri_indices", &BVHModelBaseWrapper::tri_indices, - bp::args("self","index"),"Retrieve the triangle given by its index.") - .def_readonly ("num_vertices", &BVHModelBase::num_vertices) - .def_readonly ("num_tris", &BVHModelBase::num_tris) - .def_readonly ("build_state", &BVHModelBase::build_state) - - .def_readonly ("convex", &BVHModelBase::convex) - - .DEF_CLASS_FUNC(BVHModelBase, buildConvexRepresentation) - .DEF_CLASS_FUNC(BVHModelBase, buildConvexHull) - - // Expose function to build a BVH - .def(dv::member_func("beginModel", &BVHModelBase::beginModel)) - .def(dv::member_func("addVertex", &BVHModelBase::addVertex)) - .def(dv::member_func("addVertices", &BVHModelBase::addVertices)) - .def(dv::member_func("addTriangle", &BVHModelBase::addTriangle)) - .def(dv::member_func("addTriangles", &BVHModelBase::addTriangles)) - .def(dv::member_func("addSubModel", &BVHModelBase::addSubModel)) - .def(dv::member_func("addSubModel", &BVHModelBase::addSubModel)) - .def(dv::member_func("endModel", &BVHModelBase::endModel)) - // Expose function to replace a BVH - .def(dv::member_func("beginReplaceModel", &BVHModelBase::beginReplaceModel)) - .def(dv::member_func("replaceVertex", &BVHModelBase::replaceVertex)) - .def(dv::member_func("replaceTriangle", &BVHModelBase::replaceTriangle)) - .def(dv::member_func("replaceSubModel", &BVHModelBase::replaceSubModel)) - .def(dv::member_func("endReplaceModel", &BVHModelBase::endReplaceModel)) - .def(dv::member_func("getModelType", &BVHModelBase::getModelType)) - ; - exposeBVHModel("OBB" ); - exposeBVHModel("OBBRSS" ); + class_, BVHModelPtr_t, noncopyable>( + "BVHModelBase", no_init) + .def("vertex", &BVHModelBaseWrapper::vertex, bp::args("self", "index"), + "Retrieve the vertex given by its index.", + bp::return_internal_reference<>()) + .def("vertices", &BVHModelBaseWrapper::vertex, bp::args("self", "index"), + "Retrieve the vertex given by its index.", + ::hpp::fcl::python::deprecated_member< + bp::return_internal_reference<> >()) + .def("vertices", &BVHModelBaseWrapper::vertices, bp::args("self"), + "Retrieve all the vertices.", + bp::with_custodian_and_ward_postcall<0, 1>()) + // .add_property ("vertices", + // bp::make_function(&BVHModelBaseWrapper::vertices,bp::with_custodian_and_ward_postcall<0,1>()), + // "Vertices of the BVH.") + .def("tri_indices", &BVHModelBaseWrapper::tri_indices, + bp::args("self", "index"), + "Retrieve the triangle given by its index.") + .def_readonly("num_vertices", &BVHModelBase::num_vertices) + .def_readonly("num_tris", &BVHModelBase::num_tris) + .def_readonly("build_state", &BVHModelBase::build_state) + + .def_readonly("convex", &BVHModelBase::convex) + + .DEF_CLASS_FUNC(BVHModelBase, buildConvexRepresentation) + .DEF_CLASS_FUNC(BVHModelBase, buildConvexHull) + + // Expose function to build a BVH + .def(dv::member_func("beginModel", &BVHModelBase::beginModel)) + .def(dv::member_func("addVertex", &BVHModelBase::addVertex)) + .def(dv::member_func("addVertices", &BVHModelBase::addVertices)) + .def(dv::member_func("addTriangle", &BVHModelBase::addTriangle)) + .def(dv::member_func("addTriangles", &BVHModelBase::addTriangles)) + .def(dv::member_func("addSubModel", + &BVHModelBase::addSubModel)) + .def(dv::member_func( + "addSubModel", &BVHModelBase::addSubModel)) + .def(dv::member_func("endModel", &BVHModelBase::endModel)) + // Expose function to replace a BVH + .def(dv::member_func("beginReplaceModel", + &BVHModelBase::beginReplaceModel)) + .def(dv::member_func("replaceVertex", &BVHModelBase::replaceVertex)) + .def(dv::member_func("replaceTriangle", &BVHModelBase::replaceTriangle)) + .def(dv::member_func("replaceSubModel", &BVHModelBase::replaceSubModel)) + .def(dv::member_func("endReplaceModel", &BVHModelBase::endReplaceModel)) + .def(dv::member_func("getModelType", &BVHModelBase::getModelType)); + exposeBVHModel("OBB"); + exposeBVHModel("OBBRSS"); exposeHeightField("OBBRSS"); exposeHeightField("AABB"); exposeComputeMemoryFootprint(); } -void exposeCollisionObject () -{ +void exposeCollisionObject() { namespace bp = boost::python; - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ - ("CollisionObject", no_init) - .def (dv::init >()) - .def (dv::init >()) - .def (dv::init >()) - - .DEF_CLASS_FUNC(CollisionObject, getObjectType) - .DEF_CLASS_FUNC(CollisionObject, getNodeType) - .DEF_CLASS_FUNC(CollisionObject, computeAABB) - .def(dv::member_func("getAABB", - static_cast(&CollisionObject::getAABB), bp::return_internal_reference<>())) - - .DEF_CLASS_FUNC2(CollisionObject, getTranslation, bp::return_value_policy()) - .DEF_CLASS_FUNC(CollisionObject, setTranslation) - .DEF_CLASS_FUNC2(CollisionObject, getRotation, bp::return_value_policy()) - .DEF_CLASS_FUNC(CollisionObject, setRotation) - .DEF_CLASS_FUNC2(CollisionObject, getTransform, bp::return_value_policy()) - .def(dv::member_func("setTransform", - static_cast(&CollisionObject::setTransform))) - - .DEF_CLASS_FUNC(CollisionObject, isIdentityTransform) - .DEF_CLASS_FUNC(CollisionObject, setIdentityTransform) - .DEF_CLASS_FUNC2(CollisionObject, setCollisionGeometry, (bp::with_custodian_and_ward_postcall<1,2>())) - - .def(dv::member_func("collisionGeometry", - static_cast(&CollisionObject::collisionGeometry), - bp::return_value_policy())) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_("CollisionObject", no_init) + .def(dv::init >()) + .def(dv::init >()) + .def(dv::init >()) + + .DEF_CLASS_FUNC(CollisionObject, getObjectType) + .DEF_CLASS_FUNC(CollisionObject, getNodeType) + .DEF_CLASS_FUNC(CollisionObject, computeAABB) + .def(dv::member_func("getAABB", + static_cast( + &CollisionObject::getAABB), + bp::return_internal_reference<>())) + + .DEF_CLASS_FUNC2(CollisionObject, getTranslation, + bp::return_value_policy()) + .DEF_CLASS_FUNC(CollisionObject, setTranslation) + .DEF_CLASS_FUNC2(CollisionObject, getRotation, + bp::return_value_policy()) + .DEF_CLASS_FUNC(CollisionObject, setRotation) + .DEF_CLASS_FUNC2(CollisionObject, getTransform, + bp::return_value_policy()) + .def(dv::member_func( + "setTransform", + static_cast( + &CollisionObject::setTransform))) + + .DEF_CLASS_FUNC(CollisionObject, isIdentityTransform) + .DEF_CLASS_FUNC(CollisionObject, setIdentityTransform) + .DEF_CLASS_FUNC2(CollisionObject, setCollisionGeometry, + (bp::with_custodian_and_ward_postcall<1, 2>())) + + .def(dv::member_func( + "collisionGeometry", + static_cast( + &CollisionObject::collisionGeometry), + bp::return_value_policy())); } } diff --git a/python/collision.cc b/python/collision.cc index c8e502bfe..de2490709 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -52,142 +52,150 @@ using namespace hpp::fcl; namespace dv = doxygen::visitor; -template const CollisionGeometry* geto(const Contact& c) -{ return index == 1 ? c.o1 : c.o2; } - -void exposeCollisionAPI () -{ - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - enum_ ("CollisionRequestFlag") - .value ("CONTACT", CONTACT) - .value ("DISTANCE_LOWER_BOUND", DISTANCE_LOWER_BOUND) - .value ("NO_REQUEST", NO_REQUEST) - .export_values() - ; - } - - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_("CPUTimes",no_init) - .def_readonly("wall",&CPUTimes::wall,"wall time in micro seconds (us)") - .def_readonly("user",&CPUTimes::user,"user time in micro seconds (us)") - .def_readonly("system",&CPUTimes::system,"system time in micro seconds (us)") - .def("clear",&CPUTimes::clear,arg("self"),"Reset the time values.") - ; - } +template +const CollisionGeometry* geto(const Contact& c) { + return index == 1 ? c.o1 : c.o2; +} - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ ("QueryRequest", - doxygen::class_doc(), no_init) - .DEF_RW_CLASS_ATTRIB (QueryRequest, enable_cached_gjk_guess ) - .DEF_RW_CLASS_ATTRIB (QueryRequest, cached_gjk_guess ) - .DEF_RW_CLASS_ATTRIB (QueryRequest, cached_support_func_guess ) - .DEF_RW_CLASS_ATTRIB (QueryRequest, enable_timings ) - .DEF_CLASS_FUNC (QueryRequest, updateGuess) - ; +void exposeCollisionAPI() { + if (!eigenpy::register_symbolic_link_to_registered_type< + CollisionRequestFlag>()) { + enum_("CollisionRequestFlag") + .value("CONTACT", CONTACT) + .value("DISTANCE_LOWER_BOUND", DISTANCE_LOWER_BOUND) + .value("NO_REQUEST", NO_REQUEST) + .export_values(); } - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ > ("CollisionRequest", - doxygen::class_doc(), no_init) - .def (dv::init()) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (CollisionRequest, num_max_contacts ) - .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_contact ) - .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_distance_lower_bound) - .DEF_RW_CLASS_ATTRIB (CollisionRequest, security_margin ) - .DEF_RW_CLASS_ATTRIB (CollisionRequest, break_distance ) - .DEF_RW_CLASS_ATTRIB (CollisionRequest, distance_upper_bound ) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_("CPUTimes", no_init) + .def_readonly("wall", &CPUTimes::wall, + "wall time in micro seconds (us)") + .def_readonly("user", &CPUTimes::user, + "user time in micro seconds (us)") + .def_readonly("system", &CPUTimes::system, + "system time in micro seconds (us)") + .def("clear", &CPUTimes::clear, arg("self"), "Reset the time values."); } - if(!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) - { - class_< std::vector >("StdVec_CollisionRequest") - .def(vector_indexing_suite< std::vector >()) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_("QueryRequest", doxygen::class_doc(), + no_init) + .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_cached_gjk_guess) + .DEF_RW_CLASS_ATTRIB(QueryRequest, cached_gjk_guess) + .DEF_RW_CLASS_ATTRIB(QueryRequest, cached_support_func_guess) + .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_timings) + .DEF_CLASS_FUNC(QueryRequest, updateGuess); } - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ ("Contact", - doxygen::class_doc(), init<>(arg("self"),"Default constructor")) - .def(dv::init()) - .def(dv::init()) - .add_property ("o1", - make_function(&geto<1>, return_value_policy()), - doxygen::class_attrib_doc("o1")) - .add_property ("o2", - make_function(&geto<2>, return_value_policy()), - doxygen::class_attrib_doc("o2")) - .DEF_RW_CLASS_ATTRIB (Contact, b1) - .DEF_RW_CLASS_ATTRIB (Contact, b2) - .DEF_RW_CLASS_ATTRIB (Contact, normal) - .DEF_RW_CLASS_ATTRIB (Contact, pos) - .DEF_RW_CLASS_ATTRIB (Contact, penetration_depth) - .def (self == self) - .def (self != self) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_ >( + "CollisionRequest", doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(CollisionRequest, num_max_contacts) + .DEF_RW_CLASS_ATTRIB(CollisionRequest, enable_contact) + .DEF_RW_CLASS_ATTRIB(CollisionRequest, enable_distance_lower_bound) + .DEF_RW_CLASS_ATTRIB(CollisionRequest, security_margin) + .DEF_RW_CLASS_ATTRIB(CollisionRequest, break_distance) + .DEF_RW_CLASS_ATTRIB(CollisionRequest, distance_upper_bound); } - if(!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) - { - class_< std::vector >("StdVec_Contact") - .def(vector_indexing_suite< std::vector >()) - ; + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector >()) { + class_ >("StdVec_CollisionRequest") + .def(vector_indexing_suite >()); } - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ ("QueryResult", - doxygen::class_doc(), no_init) - .DEF_RW_CLASS_ATTRIB (QueryResult, cached_gjk_guess ) - .DEF_RW_CLASS_ATTRIB (QueryResult, cached_support_func_guess) - .DEF_RW_CLASS_ATTRIB (QueryResult, timings) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_("Contact", doxygen::class_doc(), + init<>(arg("self"), "Default constructor")) + .def(dv::init()) + .def(dv::init()) + .add_property( + "o1", + make_function(&geto<1>, + return_value_policy()), + doxygen::class_attrib_doc("o1")) + .add_property( + "o2", + make_function(&geto<2>, + return_value_policy()), + doxygen::class_attrib_doc("o2")) + .DEF_RW_CLASS_ATTRIB(Contact, b1) + .DEF_RW_CLASS_ATTRIB(Contact, b2) + .DEF_RW_CLASS_ATTRIB(Contact, normal) + .DEF_RW_CLASS_ATTRIB(Contact, pos) + .DEF_RW_CLASS_ATTRIB(Contact, penetration_depth) + .def(self == self) + .def(self != self); } - if(!eigenpy::register_symbolic_link_to_registered_type< CollisionResult >()) - { - class_ > ("CollisionResult", - doxygen::class_doc(), no_init) - .def (dv::init()) - .DEF_CLASS_FUNC (CollisionResult, isCollision) - .DEF_CLASS_FUNC (CollisionResult, numContacts) - .DEF_CLASS_FUNC (CollisionResult, addContact) - .DEF_CLASS_FUNC (CollisionResult, clear) - .DEF_CLASS_FUNC2 (CollisionResult, getContact , return_value_policy()) - .def(dv::member_func("getContacts",static_cast &) const>(&CollisionResult::getContacts))) - .def("getContacts",static_cast & (CollisionResult::*)() const>(&CollisionResult::getContacts), - doxygen::member_func_doc(static_cast & (CollisionResult::*)() const>(&CollisionResult::getContacts)), - return_internal_reference<>()) - - .DEF_RW_CLASS_ATTRIB(CollisionResult, distance_lower_bound) - ; + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector >()) { + class_ >("StdVec_Contact") + .def(vector_indexing_suite >()); } - if(!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) - { - class_< std::vector >("StdVec_CollisionResult") - .def(vector_indexing_suite< std::vector >()) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_("QueryResult", doxygen::class_doc(), + no_init) + .DEF_RW_CLASS_ATTRIB(QueryResult, cached_gjk_guess) + .DEF_RW_CLASS_ATTRIB(QueryResult, cached_support_func_guess) + .DEF_RW_CLASS_ATTRIB(QueryResult, timings); } - doxygen::def ("collide", static_cast< std::size_t (*)(const CollisionObject*, const CollisionObject*, - const CollisionRequest&, CollisionResult&) > (&collide)); - doxygen::def ("collide", static_cast< std::size_t (*)( - const CollisionGeometry*, const Transform3f&, - const CollisionGeometry*, const Transform3f&, - CollisionRequest&, CollisionResult&) > (&collide)); + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_ >( + "CollisionResult", doxygen::class_doc(), no_init) + .def(dv::init()) + .DEF_CLASS_FUNC(CollisionResult, isCollision) + .DEF_CLASS_FUNC(CollisionResult, numContacts) + .DEF_CLASS_FUNC(CollisionResult, addContact) + .DEF_CLASS_FUNC(CollisionResult, clear) + .DEF_CLASS_FUNC2(CollisionResult, getContact, + return_value_policy()) + .def(dv::member_func( + "getContacts", + static_cast&) const>( + &CollisionResult::getContacts))) + .def("getContacts", + static_cast& (CollisionResult::*)() + const>(&CollisionResult::getContacts), + doxygen::member_func_doc( + static_cast& (CollisionResult::*)() + const>(&CollisionResult::getContacts)), + return_internal_reference<>()) + + .DEF_RW_CLASS_ATTRIB(CollisionResult, distance_lower_bound); + } - class_ ("ComputeCollision", - doxygen::class_doc(), no_init) - .def (dv::init()) - .def ("__call__", static_cast< std::size_t (ComputeCollision::*)( - const Transform3f&, const Transform3f&, - CollisionRequest&, CollisionResult&) const> (&ComputeCollision::operator())); + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector >()) { + class_ >("StdVec_CollisionResult") + .def(vector_indexing_suite >()); + } + doxygen::def("collide", + static_cast(&collide)); + doxygen::def( + "collide", + static_cast( + &collide)); + + class_("ComputeCollision", + doxygen::class_doc(), no_init) + .def(dv::init()) + .def("__call__", + static_cast(&ComputeCollision::operator())); } diff --git a/python/deprecation.hh b/python/deprecation.hh index b5d90dfed..3df1bda52 100644 --- a/python/deprecation.hh +++ b/python/deprecation.hh @@ -13,49 +13,42 @@ namespace hpp { namespace fcl { namespace python { - template - struct deprecated_warning_policy : Policy - { - deprecated_warning_policy(const std::string & warning_message = "") - : Policy() - , m_warning_message(warning_message) - {} - - template - bool precall(ArgumentPackage const & args) const - { - PyErr_WarnEx(PyExc_UserWarning,m_warning_message.c_str(),1); - return static_cast(this)->precall(args); - } - - typedef typename Policy::result_converter result_converter; - typedef typename Policy::argument_package argument_package; - - protected: - - const std::string m_warning_message; - }; - - template - struct deprecated_member : deprecated_warning_policy - { - deprecated_member(const std::string & warning_message - = "This class member has been marked as deprecated and will be removed in a future release.") - : deprecated_warning_policy(warning_message) - {} - }; - - template - struct deprecated_function : deprecated_warning_policy - { - deprecated_function(const std::string & warning_message - = "This function has been marked as deprecated and will be removed in a future release.") - : deprecated_warning_policy(warning_message) - {} - }; - -} -} -} - -#endif // ifndef HPP_FCL_PYTHON_UTILS_DEPRECATION_H +template +struct deprecated_warning_policy : Policy { + deprecated_warning_policy(const std::string& warning_message = "") + : Policy(), m_warning_message(warning_message) {} + + template + bool precall(ArgumentPackage const& args) const { + PyErr_WarnEx(PyExc_UserWarning, m_warning_message.c_str(), 1); + return static_cast(this)->precall(args); + } + + typedef typename Policy::result_converter result_converter; + typedef typename Policy::argument_package argument_package; + + protected: + const std::string m_warning_message; +}; + +template +struct deprecated_member : deprecated_warning_policy { + deprecated_member(const std::string& warning_message = + "This class member has been marked as deprecated and " + "will be removed in a future release.") + : deprecated_warning_policy(warning_message) {} +}; + +template +struct deprecated_function : deprecated_warning_policy { + deprecated_function(const std::string& warning_message = + "This function has been marked as deprecated and " + "will be removed in a future release.") + : deprecated_warning_policy(warning_message) {} +}; + +} // namespace python +} // namespace fcl +} // namespace hpp + +#endif // ifndef HPP_FCL_PYTHON_UTILS_DEPRECATION_H diff --git a/python/distance.cc b/python/distance.cc index cb5c7cbe0..d69427f23 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -49,77 +49,77 @@ using namespace hpp::fcl; namespace dv = doxygen::visitor; -struct DistanceRequestWrapper -{ - static Vec3f getNearestPoint1(const DistanceResult & res) { return res.nearest_points[0]; } - static Vec3f getNearestPoint2(const DistanceResult & res) { return res.nearest_points[1]; } +struct DistanceRequestWrapper { + static Vec3f getNearestPoint1(const DistanceResult& res) { + return res.nearest_points[0]; + } + static Vec3f getNearestPoint2(const DistanceResult& res) { + return res.nearest_points[1]; + } }; -void exposeDistanceAPI () -{ - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ > ("DistanceRequest", - doxygen::class_doc(), - init >((arg("self"), - arg("enable_nearest_points"), - arg("rel_err"), - arg("abs_err")),"Constructor")) - .DEF_RW_CLASS_ATTRIB (DistanceRequest, enable_nearest_points) - .DEF_RW_CLASS_ATTRIB (DistanceRequest, rel_err) - .DEF_RW_CLASS_ATTRIB (DistanceRequest, abs_err) - ; +void exposeDistanceAPI() { + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_ >( + "DistanceRequest", doxygen::class_doc(), + init >( + (arg("self"), arg("enable_nearest_points"), arg("rel_err"), + arg("abs_err")), + "Constructor")) + .DEF_RW_CLASS_ATTRIB(DistanceRequest, enable_nearest_points) + .DEF_RW_CLASS_ATTRIB(DistanceRequest, rel_err) + .DEF_RW_CLASS_ATTRIB(DistanceRequest, abs_err); } - if(!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) - { - class_< std::vector >("StdVec_DistanceRequest") - .def(vector_indexing_suite< std::vector >()) - ; + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector >()) { + class_ >("StdVec_DistanceRequest") + .def(vector_indexing_suite >()); } - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ > ("DistanceResult", - doxygen::class_doc(), - no_init) - .def (dv::init()) - .DEF_RW_CLASS_ATTRIB (DistanceResult, min_distance) - .DEF_RW_CLASS_ATTRIB(DistanceResult, normal) - //.def_readwrite ("nearest_points", &DistanceResult::nearest_points) - .def("getNearestPoint1",&DistanceRequestWrapper::getNearestPoint1, - doxygen::class_attrib_doc("nearest_points")) - .def("getNearestPoint2",&DistanceRequestWrapper::getNearestPoint2, - doxygen::class_attrib_doc("nearest_points")) - .DEF_RO_CLASS_ATTRIB (DistanceResult, o1) - .DEF_RO_CLASS_ATTRIB (DistanceResult, o2) - .DEF_RW_CLASS_ATTRIB (DistanceResult, b1) - .DEF_RW_CLASS_ATTRIB (DistanceResult, b2) + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_ >( + "DistanceResult", doxygen::class_doc(), no_init) + .def(dv::init()) + .DEF_RW_CLASS_ATTRIB(DistanceResult, min_distance) + .DEF_RW_CLASS_ATTRIB(DistanceResult, normal) + //.def_readwrite ("nearest_points", &DistanceResult::nearest_points) + .def("getNearestPoint1", &DistanceRequestWrapper::getNearestPoint1, + doxygen::class_attrib_doc("nearest_points")) + .def("getNearestPoint2", &DistanceRequestWrapper::getNearestPoint2, + doxygen::class_attrib_doc("nearest_points")) + .DEF_RO_CLASS_ATTRIB(DistanceResult, o1) + .DEF_RO_CLASS_ATTRIB(DistanceResult, o2) + .DEF_RW_CLASS_ATTRIB(DistanceResult, b1) + .DEF_RW_CLASS_ATTRIB(DistanceResult, b2) - .def ("clear", &DistanceResult::clear, - doxygen::member_func_doc(&DistanceResult::clear)) - ; + .def("clear", &DistanceResult::clear, + doxygen::member_func_doc(&DistanceResult::clear)); } - if(!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) - { - class_< std::vector >("StdVec_DistanceResult") - .def(vector_indexing_suite< std::vector >()) - ; + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector >()) { + class_ >("StdVec_DistanceResult") + .def(vector_indexing_suite >()); } - doxygen::def ("distance", static_cast< FCL_REAL (*)(const CollisionObject*, const CollisionObject*, - const DistanceRequest&, DistanceResult&) > (&distance)); - doxygen::def ("distance", static_cast< FCL_REAL (*)( - const CollisionGeometry*, const Transform3f&, - const CollisionGeometry*, const Transform3f&, - DistanceRequest&, DistanceResult&) > (&distance)); - - class_ ("ComputeDistance", - doxygen::class_doc(), no_init) - .def (dv::init()) - .def ("__call__", static_cast< FCL_REAL (ComputeDistance::*)( - const Transform3f&, const Transform3f&, - DistanceRequest&, DistanceResult&) const> (&ComputeDistance::operator())); + doxygen::def( + "distance", + static_cast( + &distance)); + doxygen::def( + "distance", + static_cast(&distance)); + class_("ComputeDistance", + doxygen::class_doc(), no_init) + .def(dv::init()) + .def("__call__", + static_cast(&ComputeDistance::operator())); } diff --git a/python/fcl.cc b/python/fcl.cc index f732c6e98..59aec9b95 100644 --- a/python/fcl.cc +++ b/python/fcl.cc @@ -56,43 +56,38 @@ namespace dv = doxygen::visitor; #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(load_overloads,MeshLoader::load,1,2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(load_overloads, MeshLoader::load, 1, 2) #pragma GCC diagnostic pop -void exposeMeshLoader () -{ +void exposeMeshLoader() { using namespace boost::python; - - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ > ("MeshLoader", - doxygen::class_doc(), - init< optional< NODE_TYPE> >((arg("self"),arg("node_type")), - doxygen::constructor_doc())) - .def ("load",&MeshLoader::load, - load_overloads((arg("self"),arg("filename"),arg("scale")), - doxygen::member_func_doc(&MeshLoader::load))) - .def (dv::member_func("loadOctree",&MeshLoader::loadOctree)) - ; + + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_ >( + "MeshLoader", doxygen::class_doc(), + init >( + (arg("self"), arg("node_type")), + doxygen::constructor_doc())) + .def("load", &MeshLoader::load, + load_overloads((arg("self"), arg("filename"), arg("scale")), + doxygen::member_func_doc(&MeshLoader::load))) + .def(dv::member_func("loadOctree", &MeshLoader::loadOctree)); } - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ , shared_ptr > ( - "CachedMeshLoader", - doxygen::class_doc(), - init< optional< NODE_TYPE> >((arg("self"),arg("node_type")), - doxygen::constructor_doc())) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_, shared_ptr >( + "CachedMeshLoader", doxygen::class_doc(), + init >( + (arg("self"), arg("node_type")), + doxygen::constructor_doc())); } } -BOOST_PYTHON_MODULE(hppfcl) -{ +BOOST_PYTHON_MODULE(hppfcl) { namespace bp = boost::python; - + PyImport_ImportModule("warnings"); - + exposeVersion(); exposeMaths(); exposeCollisionGeometries(); diff --git a/python/fwd.hh b/python/fwd.hh index e45968ca6..45e24bcbf 100644 --- a/python/fwd.hh +++ b/python/fwd.hh @@ -8,7 +8,7 @@ #include #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC namespace doxygen { - using hpp::fcl::shared_ptr; +using hpp::fcl::shared_ptr; } #endif @@ -26,15 +26,15 @@ namespace doxygen { namespace bp = boost::python; namespace dv = doxygen::visitor; -#define DEF_RW_CLASS_ATTRIB(CLASS, ATTRIB) \ - def_readwrite (#ATTRIB, &CLASS::ATTRIB, \ - doxygen::class_attrib_doc(#ATTRIB)) -#define DEF_RO_CLASS_ATTRIB(CLASS, ATTRIB) \ - def_readonly (#ATTRIB, &CLASS::ATTRIB, \ - doxygen::class_attrib_doc(#ATTRIB)) -#define DEF_CLASS_FUNC(CLASS, ATTRIB) \ - def (dv::member_func(#ATTRIB, &CLASS::ATTRIB)) -#define DEF_CLASS_FUNC2(CLASS, ATTRIB,policy) \ - def (#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB),policy) +#define DEF_RW_CLASS_ATTRIB(CLASS, ATTRIB) \ + def_readwrite(#ATTRIB, &CLASS::ATTRIB, \ + doxygen::class_attrib_doc(#ATTRIB)) +#define DEF_RO_CLASS_ATTRIB(CLASS, ATTRIB) \ + def_readonly(#ATTRIB, &CLASS::ATTRIB, \ + doxygen::class_attrib_doc(#ATTRIB)) +#define DEF_CLASS_FUNC(CLASS, ATTRIB) \ + def(dv::member_func(#ATTRIB, &CLASS::ATTRIB)) +#define DEF_CLASS_FUNC2(CLASS, ATTRIB, policy) \ + def(#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB), policy) -#endif // ifndef HPP_FCL_PYTHON_FWD_HH +#endif // ifndef HPP_FCL_PYTHON_FWD_HH diff --git a/python/gjk.cc b/python/gjk.cc index 70f4ec683..521dd7d2f 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -46,58 +46,54 @@ using namespace boost::python; using namespace hpp::fcl; -using hpp::fcl::details::MinkowskiDiff; -using hpp::fcl::details::GJK; using hpp::fcl::details::EPA; +using hpp::fcl::details::GJK; +using hpp::fcl::details::MinkowskiDiff; -void exposeGJK() -{ - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - enum_ ("GJKStatus") - .value ("Valid", GJK::Valid) - .value ("Inside", GJK::Inside) - .value ("Failed", GJK::Failed) - .export_values() - ; +void exposeGJK() { + if (!eigenpy::register_symbolic_link_to_registered_type()) { + enum_("GJKStatus") + .value("Valid", GJK::Valid) + .value("Inside", GJK::Inside) + .value("Failed", GJK::Failed) + .export_values(); } - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ ("MinkowskiDiff", doxygen::class_doc(), no_init) - .def (doxygen::visitor::init()) - .def ("set", static_cast < void (MinkowskiDiff::*)( - const ShapeBase*, const ShapeBase*)> (&MinkowskiDiff::set), - doxygen::member_func_doc( - static_cast < void (MinkowskiDiff::*)( - const ShapeBase*, const ShapeBase*)> (&MinkowskiDiff::set))) - .def ("set", static_cast < void (MinkowskiDiff::*)( - const ShapeBase*, const ShapeBase*, - const Transform3f& tf0, const Transform3f& tf1)> (&MinkowskiDiff::set), - doxygen::member_func_doc( - static_cast < void (MinkowskiDiff::*)( - const ShapeBase*, const ShapeBase*, - const Transform3f& tf0, const Transform3f& tf1)> (&MinkowskiDiff::set))) - .DEF_CLASS_FUNC(MinkowskiDiff, support0) - .DEF_CLASS_FUNC(MinkowskiDiff, support1) - .DEF_CLASS_FUNC(MinkowskiDiff, support) - .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, inflation) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_("MinkowskiDiff", doxygen::class_doc(), + no_init) + .def(doxygen::visitor::init()) + .def("set", + static_cast(&MinkowskiDiff::set), + doxygen::member_func_doc( + static_cast(&MinkowskiDiff::set))) + .def("set", + static_cast(&MinkowskiDiff::set), + doxygen::member_func_doc( + static_cast(&MinkowskiDiff::set))) + .DEF_CLASS_FUNC(MinkowskiDiff, support0) + .DEF_CLASS_FUNC(MinkowskiDiff, support1) + .DEF_CLASS_FUNC(MinkowskiDiff, support) + .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, inflation); } - if(!eigenpy::register_symbolic_link_to_registered_type()) - { - class_ ("GJK", doxygen::class_doc(), no_init) - .def (doxygen::visitor::init()) - .DEF_RW_CLASS_ATTRIB (GJK, distance) - .DEF_RW_CLASS_ATTRIB (GJK, ray) - .DEF_RW_CLASS_ATTRIB (GJK, support_hint) - .DEF_CLASS_FUNC(GJK, evaluate) - .DEF_CLASS_FUNC(GJK, hasClosestPoints) - .DEF_CLASS_FUNC(GJK, hasPenetrationInformation) - .DEF_CLASS_FUNC(GJK, getClosestPoints) - .DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak) - .DEF_CLASS_FUNC(GJK, getGuessFromSimplex) - ; + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_("GJK", doxygen::class_doc(), no_init) + .def(doxygen::visitor::init()) + .DEF_RW_CLASS_ATTRIB(GJK, distance) + .DEF_RW_CLASS_ATTRIB(GJK, ray) + .DEF_RW_CLASS_ATTRIB(GJK, support_hint) + .DEF_CLASS_FUNC(GJK, evaluate) + .DEF_CLASS_FUNC(GJK, hasClosestPoints) + .DEF_CLASS_FUNC(GJK, hasPenetrationInformation) + .DEF_CLASS_FUNC(GJK, getClosestPoints) + .DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak) + .DEF_CLASS_FUNC(GJK, getGuessFromSimplex); } } diff --git a/python/hppfcl/viewer.py b/python/hppfcl/viewer.py index 0e326929e..29cdb2ac8 100644 --- a/python/hppfcl/viewer.py +++ b/python/hppfcl/viewer.py @@ -6,55 +6,79 @@ import hppfcl, numpy as np from gepetto import Color + def applyConfiguration(gui, name, tf): - gui.applyConfiguration(name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist()) + gui.applyConfiguration( + name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist() + ) + -def displayShape(gui, name, geom, color = (.9, .9, .9, 1.)): +def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)): if isinstance(geom, hppfcl.Capsule): - return gui.addCapsule(name, geom.radius, 2. * geom.halfLength, color) + return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color) elif isinstance(geom, hppfcl.Cylinder): - return gui.addCylinder(name, geom.radius, 2. * geom.halfLength, color) + return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color) elif isinstance(geom, hppfcl.Box): - w, h, d = (2. * geom.halfSide).tolist() + w, h, d = (2.0 * geom.halfSide).tolist() return gui.addBox(name, w, h, d, color) elif isinstance(geom, hppfcl.Sphere): return gui.addSphere(name, geom.radius, color) elif isinstance(geom, hppfcl.Cone): - return gui.addCone(name, geom.radius, 2. * geom.halfLength, color) + return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color) elif isinstance(geom, hppfcl.Convex): - pts = [ geom.points(geom.polygons(f)[i]).tolist() for f in range(geom.num_polygons) for i in range(3) ] + pts = [ + geom.points(geom.polygons(f)[i]).tolist() + for f in range(geom.num_polygons) + for i in range(3) + ] gui.addCurve(name, pts, color) gui.setCurveMode(name, "TRIANGLES") gui.setLightingMode(name, "ON") gui.setBoolProperty(name, "BackfaceDrawing", True) return True elif isinstance(geom, hppfcl.ConvexBase): - pts = [ geom.points(i).tolist() for i in range(geom.num_points) ] + pts = [geom.points(i).tolist() for i in range(geom.num_points)] gui.addCurve(name, pts, color) gui.setCurveMode(name, "POINTS") gui.setLightingMode(name, "OFF") return True else: - msg = "Unsupported geometry type for %s (%s)" % (geometry_object.name, type(geom) ) + msg = "Unsupported geometry type for %s (%s)" % ( + geometry_object.name, + type(geom), + ) warnings.warn(msg, category=UserWarning, stacklevel=2) return False -def displayDistanceResult(gui, group_name, res, closest_points = True, normal = True): + +def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True): gui.createGroup(group_name) r = 0.01 if closest_points: - p = [ group_name+"/p1", group_name+"/p2" ] + p = [group_name + "/p1", group_name + "/p2"] gui.addSphere(p[0], r, Color.red) gui.addSphere(p[1], r, Color.blue) - qid = [0,0,0,1] - gui.applyConfigurations(p, [ res.getNearestPoint1().tolist() + qid, res.getNearestPoint2().tolist() + qid, ]) + qid = [0, 0, 0, 1] + gui.applyConfigurations( + p, + [ + res.getNearestPoint1().tolist() + qid, + res.getNearestPoint2().tolist() + qid, + ], + ) if normal: - n = group_name+"/normal" + n = group_name + "/normal" gui.addArrow(n, r, 0.1, Color.green) - gui.applyConfiguration(n, - res.getNearestPoint1().tolist() + hppfcl.Quaternion.FromTwoVectors(np.array([1,0,0]), res.normal).coeffs().tolist()) + gui.applyConfiguration( + n, + res.getNearestPoint1().tolist() + + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal) + .coeffs() + .tolist(), + ) gui.refresh() + def displayCollisionResult(gui, group_name, res, color=Color.green): if res.isCollision(): if gui.nodeExists(group_name): @@ -63,18 +87,23 @@ def displayCollisionResult(gui, group_name, res, color=Color.green): gui.createGroup(group_name) for i in range(res.numContacts()): contact = res.getContact(i) - n = group_name+"/contact"+str(i) + n = group_name + "/contact" + str(i) depth = contact.penetration_depth if gui.nodeExists(n): - gui.setFloatProperty(n, 'Size', depth) - gui.setFloatProperty(n, 'Radius', 0.1*depth) + gui.setFloatProperty(n, "Size", depth) + gui.setFloatProperty(n, "Radius", 0.1 * depth) gui.setColor(n, color) else: - gui.addArrow(n, depth*0.1, depth, color) + gui.addArrow(n, depth * 0.1, depth, color) N = contact.normal P = contact.pos - gui.applyConfiguration(n, (P-depth*N/2).tolist() + - hppfcl.Quaternion.FromTwoVectors(np.array([1,0,0]), N).coeffs().tolist()) + gui.applyConfiguration( + n, + (P - depth * N / 2).tolist() + + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N) + .coeffs() + .tolist(), + ) gui.refresh() elif gui.nodeExists(group_name): gui.setVisibility(group_name, "OFF") diff --git a/python/math.cc b/python/math.cc index dee7d1ab4..5308c8c3f 100644 --- a/python/math.cc +++ b/python/math.cc @@ -49,88 +49,93 @@ using namespace hpp::fcl; namespace dv = doxygen::visitor; -struct TriangleWrapper -{ - static Triangle::index_type getitem (const Triangle& t, int i) - { - if(i >= 3 || i <= -3) - PyErr_SetString(PyExc_IndexError,"Index out of range"); - return t[(i%3)]; +struct TriangleWrapper { + static Triangle::index_type getitem(const Triangle& t, int i) { + if (i >= 3 || i <= -3) + PyErr_SetString(PyExc_IndexError, "Index out of range"); + return t[(i % 3)]; } - static void setitem (Triangle& t, int i, Triangle::index_type v) - { - if(i >= 3 || i <= -3) - PyErr_SetString(PyExc_IndexError,"Index out of range"); - t[(i%3)] = v; + static void setitem(Triangle& t, int i, Triangle::index_type v) { + if (i >= 3 || i <= -3) + PyErr_SetString(PyExc_IndexError, "Index out of range"); + t[(i % 3)] = v; } }; -void exposeMaths () -{ +void exposeMaths() { eigenpy::enableEigenPy(); - if(!eigenpy::register_symbolic_link_to_registered_type()) + if (!eigenpy::register_symbolic_link_to_registered_type()) eigenpy::exposeQuaternion(); - if(!eigenpy::register_symbolic_link_to_registered_type()) + if (!eigenpy::register_symbolic_link_to_registered_type()) eigenpy::exposeAngleAxis(); eigenpy::enableEigenPySpecific(); - eigenpy::enableEigenPySpecific(); - - class_ ("Transform3f", doxygen::class_doc(), no_init) - .def (dv::init()) - .def (dv::init()) - .def (dv::init()) - .def (dv::init()) - .def (dv::init()) - .def (dv::init()) - .def (dv::init()) - - .def (dv::member_func("getQuatRotation", &Transform3f::getQuatRotation)) - .def ("getTranslation", &Transform3f::getTranslation, doxygen::member_func_doc(&Transform3f::getTranslation), return_value_policy()) - .def ("getRotation", &Transform3f::getRotation, return_value_policy()) - .def (dv::member_func("isIdentity", &Transform3f::isIdentity)) - - .def (dv::member_func("setQuatRotation", &Transform3f::setQuatRotation)) - .def ("setTranslation", &Transform3f::setTranslation) - .def ("setRotation", &Transform3f::setRotation) - .def (dv::member_func("setTransform", &Transform3f::setTransform)) - .def (dv::member_func("setTransform", static_cast(&Transform3f::setTransform))) - .def (dv::member_func("setIdentity", &Transform3f::setIdentity)) - .def (dv::member_func("Identity", &Transform3f::Identity)).staticmethod("Identity") - - .def (dv::member_func("transform", &Transform3f::transform)) - .def ("inverseInPlace", &Transform3f::inverseInPlace, return_internal_reference<>(), doxygen::member_func_doc(&Transform3f::inverseInPlace)) - .def (dv::member_func("inverse", &Transform3f::inverse)) - .def (dv::member_func("inverseTimes", &Transform3f::inverseTimes)) - - .def (self * self) - .def (self *= self) - .def (self == self) - .def (self != self) - ; - - class_ ("Triangle", no_init) - .def(dv::init()) - .def (dv::init ()) - .def ("__getitem__", &TriangleWrapper::getitem) - .def ("__setitem__", &TriangleWrapper::setitem) - .def(dv::member_func("set", &Triangle::set)) - .def(dv::member_func("size", &Triangle::size)) - .staticmethod("size") - .def(self == self) - ; - - if(!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) - { - class_< std::vector >("StdVec_Vec3f") - .def(vector_indexing_suite< std::vector >()) - ; + eigenpy::enableEigenPySpecific(); + + class_("Transform3f", doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + + .def(dv::member_func("getQuatRotation", &Transform3f::getQuatRotation)) + .def("getTranslation", &Transform3f::getTranslation, + doxygen::member_func_doc(&Transform3f::getTranslation), + return_value_policy()) + .def("getRotation", &Transform3f::getRotation, + return_value_policy()) + .def(dv::member_func("isIdentity", &Transform3f::isIdentity)) + + .def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation)) + .def("setTranslation", &Transform3f::setTranslation) + .def("setRotation", &Transform3f::setRotation) + .def(dv::member_func("setTransform", + &Transform3f::setTransform)) + .def(dv::member_func( + "setTransform", + static_cast( + &Transform3f::setTransform))) + .def(dv::member_func("setIdentity", &Transform3f::setIdentity)) + .def(dv::member_func("Identity", &Transform3f::Identity)) + .staticmethod("Identity") + + .def(dv::member_func("transform", &Transform3f::transform)) + .def("inverseInPlace", &Transform3f::inverseInPlace, + return_internal_reference<>(), + doxygen::member_func_doc(&Transform3f::inverseInPlace)) + .def(dv::member_func("inverse", &Transform3f::inverse)) + .def(dv::member_func("inverseTimes", &Transform3f::inverseTimes)) + + .def(self * self) + .def(self *= self) + .def(self == self) + .def(self != self); + + class_("Triangle", no_init) + .def(dv::init()) + .def(dv::init()) + .def("__getitem__", &TriangleWrapper::getitem) + .def("__setitem__", &TriangleWrapper::setitem) + .def(dv::member_func("set", &Triangle::set)) + .def(dv::member_func("size", &Triangle::size)) + .staticmethod("size") + .def(self == self); + + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector >()) { + class_ >("StdVec_Vec3f") + .def(vector_indexing_suite >()); } - if(!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) - { - class_< std::vector >("StdVec_Triangle") - .def(vector_indexing_suite< std::vector >()) - ; + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector >()) { + class_ >("StdVec_Triangle") + .def(vector_indexing_suite >()); } } diff --git a/python/octree.cc b/python/octree.cc index caee08971..073e711f1 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -8,23 +8,23 @@ #include "doxygen_autodoc/functions.h" #endif -void exposeOctree() -{ +void exposeOctree() { using namespace hpp::fcl; namespace bp = boost::python; namespace dv = doxygen::visitor; - - bp::class_ >("OcTree",doxygen::class_doc(),bp::no_init) - .def(dv::init()) - .def(dv::member_func("getTreeDepth",&OcTree::getTreeDepth)) - .def(dv::member_func("getOccupancyThres",&OcTree::getOccupancyThres)) - .def(dv::member_func("getFreeThres",&OcTree::getFreeThres)) - .def(dv::member_func("getDefaultOccupancy",&OcTree::getDefaultOccupancy)) - .def(dv::member_func("setCellDefaultOccupancy",&OcTree::setCellDefaultOccupancy)) - .def(dv::member_func("setOccupancyThres",&OcTree::setOccupancyThres)) - .def(dv::member_func("setFreeThres",&OcTree::setFreeThres)) - .def(dv::member_func("getRootBV",&OcTree::getRootBV)) - ; - - doxygen::def("makeOctree",&makeOctree); + + bp::class_ >( + "OcTree", doxygen::class_doc(), bp::no_init) + .def(dv::init()) + .def(dv::member_func("getTreeDepth", &OcTree::getTreeDepth)) + .def(dv::member_func("getOccupancyThres", &OcTree::getOccupancyThres)) + .def(dv::member_func("getFreeThres", &OcTree::getFreeThres)) + .def(dv::member_func("getDefaultOccupancy", &OcTree::getDefaultOccupancy)) + .def(dv::member_func("setCellDefaultOccupancy", + &OcTree::setCellDefaultOccupancy)) + .def(dv::member_func("setOccupancyThres", &OcTree::setOccupancyThres)) + .def(dv::member_func("setFreeThres", &OcTree::setFreeThres)) + .def(dv::member_func("getRootBV", &OcTree::getRootBV)); + + doxygen::def("makeOctree", &makeOctree); } diff --git a/python/version.cc b/python/version.cc index b106ee841..f83798629 100644 --- a/python/version.cc +++ b/python/version.cc @@ -8,38 +8,30 @@ namespace bp = boost::python; -inline bool checkVersionAtLeast(int major, - int minor, - int patch) -{ +inline bool checkVersionAtLeast(int major, int minor, int patch) { return HPP_FCL_VERSION_AT_LEAST(major, minor, patch); } -inline bool checkVersionAtMost(int major, - int minor, - int patch) -{ +inline bool checkVersionAtMost(int major, int minor, int patch) { return HPP_FCL_VERSION_AT_MOST(major, minor, patch); } -void exposeVersion() -{ +void exposeVersion() { // Define release numbers of the current hpp-fcl version. - bp::scope().attr("__version__") = BOOST_PP_STRINGIZE(HPP_FCL_MAJOR_VERSION) "." - BOOST_PP_STRINGIZE(HPP_FCL_MINOR_VERSION) "." - BOOST_PP_STRINGIZE(HPP_FCL_PATCH_VERSION); + bp::scope().attr("__version__") = + BOOST_PP_STRINGIZE(HPP_FCL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(HPP_FCL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(HPP_FCL_PATCH_VERSION); bp::scope().attr("__raw_version__") = HPP_FCL_VERSION; bp::scope().attr("HPP_FCL_MAJOR_VERSION") = HPP_FCL_MAJOR_VERSION; bp::scope().attr("HPP_FCL_MINOR_VERSION") = HPP_FCL_MINOR_VERSION; bp::scope().attr("HPP_FCL_PATCH_VERSION") = HPP_FCL_PATCH_VERSION; - bp::def("checkVersionAtLeast",&checkVersionAtLeast, - bp::args("major","minor","patch"), + bp::def("checkVersionAtLeast", &checkVersionAtLeast, + bp::args("major", "minor", "patch"), "Checks if the current version of hpp-fcl is at least" " the version provided by the input arguments."); - bp::def("checkVersionAtMost",&checkVersionAtMost, - bp::args("major","minor","patch"), + bp::def("checkVersionAtMost", &checkVersionAtMost, + bp::args("major", "minor", "patch"), "Checks if the current version of hpp-fcl is at most" " the version provided by the input arguments."); } diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 14ce08999..b6ed66cbc 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -40,73 +40,57 @@ #include #include -namespace hpp -{ -namespace fcl -{ - -AABB::AABB() : min_(Vec3f::Constant((std::numeric_limits::max)())), - max_(Vec3f::Constant(-(std::numeric_limits::max)())) -{ -} +namespace hpp { +namespace fcl { + +AABB::AABB() + : min_(Vec3f::Constant((std::numeric_limits::max)())), + max_(Vec3f::Constant(-(std::numeric_limits::max)())) {} bool AABB::overlap(const AABB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const -{ - const FCL_REAL breakDistance (request.break_distance + request.security_margin); + FCL_REAL& sqrDistLowerBound) const { + const FCL_REAL breakDistance(request.break_distance + + request.security_margin); const FCL_REAL breakDistance2 = breakDistance * breakDistance; sqrDistLowerBound = (min_ - other.max_).array().max(0).matrix().squaredNorm(); - if(sqrDistLowerBound > breakDistance2) return false; + if (sqrDistLowerBound > breakDistance2) return false; sqrDistLowerBound = (other.min_ - max_).array().max(0).matrix().squaredNorm(); - if(sqrDistLowerBound > breakDistance2) return false; + if (sqrDistLowerBound > breakDistance2) return false; return true; } -FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const -{ +FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { FCL_REAL result = 0; - for(Eigen::DenseIndex i = 0; i < 3; ++i) - { + for (Eigen::DenseIndex i = 0; i < 3; ++i) { const FCL_REAL& amin = min_[i]; const FCL_REAL& amax = max_[i]; const FCL_REAL& bmin = other.min_[i]; const FCL_REAL& bmax = other.max_[i]; - - if(amin > bmax) - { + + if (amin > bmax) { FCL_REAL delta = bmax - amin; result += delta * delta; - if(P && Q) - { + if (P && Q) { (*P)[i] = amin; (*Q)[i] = bmax; } - } - else if(bmin > amax) - { + } else if (bmin > amax) { FCL_REAL delta = amax - bmin; result += delta * delta; - if(P && Q) - { + if (P && Q) { (*P)[i] = amax; (*Q)[i] = bmin; } - } - else - { - if(P && Q) - { - if(bmin >= amin) - { + } else { + if (P && Q) { + if (bmin >= amin) { FCL_REAL t = 0.5 * (amax + bmin); (*P)[i] = t; (*Q)[i] = t; - } - else - { + } else { FCL_REAL t = 0.5 * (amin + bmax); (*P)[i] = t; (*Q)[i] = t; @@ -118,23 +102,18 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const return std::sqrt(result); } -FCL_REAL AABB::distance(const AABB& other) const -{ +FCL_REAL AABB::distance(const AABB& other) const { FCL_REAL result = 0; - for(Eigen::DenseIndex i = 0; i < 3; ++i) - { + for (Eigen::DenseIndex i = 0; i < 3; ++i) { const FCL_REAL& amin = min_[i]; const FCL_REAL& amax = max_[i]; const FCL_REAL& bmin = other.min_[i]; const FCL_REAL& bmax = other.max_[i]; - - if(amin > bmax) - { + + if (amin > bmax) { FCL_REAL delta = bmax - amin; result += delta * delta; - } - else if(bmin > amax) - { + } else if (bmin > amax) { FCL_REAL delta = amax - bmin; result += delta * delta; } @@ -143,20 +122,19 @@ FCL_REAL AABB::distance(const AABB& other) const return std::sqrt(result); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2) -{ - AABB bb1 (translate (rotate (b1, R0), T0)); - return bb1.overlap (b2); +bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, + const AABB& b2) { + AABB bb1(translate(rotate(b1, R0), T0)); + return bb1.overlap(b2); } bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, - const AABB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) -{ - AABB bb1 (translate (rotate (b1, R0), T0)); - return bb1.overlap (b2, request, sqrDistLowerBound); + const AABB& b2, const CollisionRequest& request, + FCL_REAL& sqrDistLowerBound) { + AABB bb1(translate(rotate(b1, R0), T0)); + return bb1.overlap(b2, request, sqrDistLowerBound); } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 6f00859d6..4c3d159aa 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -44,28 +44,24 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Compute the 8 vertices of a OBB -inline void computeVertices(const OBB& b, Vec3f vertices[8]) -{ - Matrix3f extAxes (b.axes * b.extent.asDiagonal()); - vertices[0].noalias() = b.To + extAxes * Vec3f(-1,-1,-1); - vertices[1].noalias() = b.To + extAxes * Vec3f( 1,-1,-1); - vertices[2].noalias() = b.To + extAxes * Vec3f( 1, 1,-1); - vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1,-1); - vertices[4].noalias() = b.To + extAxes * Vec3f(-1,-1, 1); - vertices[5].noalias() = b.To + extAxes * Vec3f( 1,-1, 1); - vertices[6].noalias() = b.To + extAxes * Vec3f( 1, 1, 1); +inline void computeVertices(const OBB& b, Vec3f vertices[8]) { + Matrix3f extAxes(b.axes * b.extent.asDiagonal()); + vertices[0].noalias() = b.To + extAxes * Vec3f(-1, -1, -1); + vertices[1].noalias() = b.To + extAxes * Vec3f(1, -1, -1); + vertices[2].noalias() = b.To + extAxes * Vec3f(1, 1, -1); + vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1, -1); + vertices[4].noalias() = b.To + extAxes * Vec3f(-1, -1, 1); + vertices[5].noalias() = b.To + extAxes * Vec3f(1, -1, 1); + vertices[6].noalias() = b.To + extAxes * Vec3f(1, 1, 1); vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1); } /// @brief OBB merge method when the centers of two smaller OBB are far away -inline OBB merge_largedist(const OBB& b1, const OBB& b2) -{ +inline OBB merge_largedist(const OBB& b1, const OBB& b2) { OBB b; Vec3f vertex[16]; computeVertices(b1, vertex); @@ -78,19 +74,30 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) b.axes.col(0).noalias() = (b1.To - b2.To).normalized(); Vec3f vertex_proj[16]; - for(int i = 0; i < 16; ++i) - vertex_proj[i].noalias() = vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0)); + for (int i = 0; i < 16; ++i) + vertex_proj[i].noalias() = + vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0)); getCovariance(vertex_proj, NULL, NULL, NULL, 16, M); eigen(M, s, E); int min, mid, max; - if (s[0] > s[1]) { max = 0; min = 1; } - else { min = 0; max = 1; } - if (s[2] < s[min]) { mid = min; min = 2; } - else if (s[2] > s[max]) { mid = max; max = 2; } - else { mid = 2; } - + if (s[0] > s[1]) { + max = 0; + min = 1; + } else { + min = 0; + max = 1; + } + if (s[2] < s[min]) { + mid = min; + min = 2; + } else if (s[2] > s[max]) { + mid = max; + max = 2; + } else { + mid = 2; + } b.axes.col(1) << E[0][max], E[1][max], E[2][max]; b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid]; @@ -105,55 +112,46 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) return b; } - /// @brief OBB merge method when the centers of two smaller OBB are close -inline OBB merge_smalldist(const OBB& b1, const OBB& b2) -{ +inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { OBB b; b.To = (b1.To + b2.To) * 0.5; - Quaternion3f q0 (b1.axes), q1 (b2.axes); - if(q0.dot(q1) < 0) - q1.coeffs() *= -1; + Quaternion3f q0(b1.axes), q1(b2.axes); + if (q0.dot(q1) < 0) q1.coeffs() *= -1; - Quaternion3f q ((q0.coeffs() + q1.coeffs()).normalized()); + Quaternion3f q((q0.coeffs() + q1.coeffs()).normalized()); b.axes = q.toRotationMatrix(); - Vec3f vertex[8], diff; FCL_REAL real_max = (std::numeric_limits::max)(); Vec3f pmin(real_max, real_max, real_max); Vec3f pmax(-real_max, -real_max, -real_max); computeVertices(b1, vertex); - for(int i = 0; i < 8; ++i) - { + for (int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; - for(int j = 0; j < 3; ++j) - { + for (int j = 0; j < 3; ++j) { FCL_REAL dot = diff.dot(b.axes.col(j)); - if(dot > pmax[j]) + if (dot > pmax[j]) pmax[j] = dot; - else if(dot < pmin[j]) + else if (dot < pmin[j]) pmin[j] = dot; } } computeVertices(b2, vertex); - for(int i = 0; i < 8; ++i) - { + for (int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; - for(int j = 0; j < 3; ++j) - { + for (int j = 0; j < 3; ++j) { FCL_REAL dot = diff.dot(b.axes.col(j)); - if(dot > pmax[j]) + if (dot > pmax[j]) pmax[j] = dot; - else if(dot < pmin[j]) + else if (dot < pmin[j]) pmin[j] = dot; } } - for(int j = 0; j < 3; ++j) - { + for (int j = 0; j < 3; ++j) { b.To.noalias() += (b.axes.col(j) * (0.5 * (pmax[j] + pmin[j]))); b.extent[j] = 0.5 * (pmax[j] - pmin[j]); } @@ -161,12 +159,12 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) return b; } -bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b) -{ +bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, + const Vec3f& b) { FCL_REAL t, s; const FCL_REAL reps = 1e-6; - Matrix3f Bf (B.array().abs() + reps); + Matrix3f Bf(B.array().abs() + reps); // Bf += reps; // if any of these tests are one-sided, then the polyhedra are disjoint @@ -175,31 +173,27 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& t = ((T[0] < 0.0) ? -T[0] : T[0]); // if(t > (a[0] + Bf.dotX(b))) - if(t > (a[0] + Bf.row(0).dot(b))) - return true; + if (t > (a[0] + Bf.row(0).dot(b))) return true; // B1 x B2 = B0 // s = B.transposeDotX(T); - s = B.col(0).dot(T); + s = B.col(0).dot(T); t = ((s < 0.0) ? -s : s); // if(t > (b[0] + Bf.transposeDotX(a))) - if(t > (b[0] + Bf.col(0).dot(a))) - return true; + if (t > (b[0] + Bf.col(0).dot(a))) return true; // A2 x A0 = A1 t = ((T[1] < 0.0) ? -T[1] : T[1]); // if(t > (a[1] + Bf.dotY(b))) - if(t > (a[1] + Bf.row(1).dot(b))) - return true; + if (t > (a[1] + Bf.row(1).dot(b))) return true; // A0 x A1 = A2 - t =((T[2] < 0.0) ? -T[2] : T[2]); + t = ((T[2] < 0.0) ? -T[2] : T[2]); // if(t > (a[2] + Bf.dotZ(b))) - if(t > (a[2] + Bf.row(2).dot(b))) - return true; + if (t > (a[2] + Bf.row(2).dot(b))) return true; // B2 x B0 = B1 // s = B.transposeDotY(T); @@ -207,8 +201,7 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& t = ((s < 0.0) ? -s : s); // if(t > (b[1] + Bf.transposeDotY(a))) - if(t > (b[1] + Bf.col(1).dot(a))) - return true; + if (t > (b[1] + Bf.col(1).dot(a))) return true; // B0 x B1 = B2 // s = B.transposeDotZ(T); @@ -216,139 +209,132 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& t = ((s < 0.0) ? -s : s); // if(t > (b[2] + Bf.transposeDotZ(a))) - if(t > (b[2] + Bf.col(2).dot(a))) - return true; + if (t > (b[2] + Bf.col(2).dot(a))) return true; // A0 x B0 s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); - if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + - b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) + if (t > + (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) return true; // A0 x B1 s = T[2] * B(1, 1) - T[1] * B(2, 1); t = ((s < 0.0) ? -s : s); - if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + - b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) + if (t > + (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) return true; // A0 x B2 s = T[2] * B(1, 2) - T[1] * B(2, 2); t = ((s < 0.0) ? -s : s); - if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + - b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) + if (t > + (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) return true; // A1 x B0 s = T[0] * B(2, 0) - T[2] * B(0, 0); t = ((s < 0.0) ? -s : s); - if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + - b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) + if (t > + (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) return true; // A1 x B1 s = T[0] * B(2, 1) - T[2] * B(0, 1); t = ((s < 0.0) ? -s : s); - if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + - b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) + if (t > + (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) return true; // A1 x B2 s = T[0] * B(2, 2) - T[2] * B(0, 2); t = ((s < 0.0) ? -s : s); - if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + - b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) + if (t > + (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) return true; // A2 x B0 s = T[1] * B(0, 0) - T[0] * B(1, 0); t = ((s < 0.0) ? -s : s); - if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + - b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) + if (t > + (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) return true; // A2 x B1 s = T[1] * B(0, 1) - T[0] * B(1, 1); t = ((s < 0.0) ? -s : s); - if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + - b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) + if (t > + (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) return true; // A2 x B2 s = T[1] * B(0, 2) - T[0] * B(1, 2); t = ((s < 0.0) ? -s : s); - if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + - b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) + if (t > + (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) return true; return false; - } -namespace internal -{ - inline FCL_REAL obbDisjoint_check_A_axis ( - const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf) - { - // |T| - |B| * b - a - Vec3f AABB_corner (T.cwiseAbs () - a); - AABB_corner.noalias() -= Bf * b; - return AABB_corner.array().max(0).matrix().squaredNorm (); - } +namespace internal { +inline FCL_REAL obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a, + const Vec3f& b, const Matrix3f& Bf) { + // |T| - |B| * b - a + Vec3f AABB_corner(T.cwiseAbs() - a); + AABB_corner.noalias() -= Bf * b; + return AABB_corner.array().max(0).matrix().squaredNorm(); +} - inline FCL_REAL obbDisjoint_check_B_axis ( - const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf) - { - // Bf = |B| - // | B^T T| - Bf^T * a - b - FCL_REAL s, t = 0; - s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; - if (s > 0) t += s*s; - s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; - if (s > 0) t += s*s; - s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2]; - if (s > 0) t += s*s; - return t; - } +inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const Matrix3f& Bf) { + // Bf = |B| + // | B^T T| - Bf^T * a - b + FCL_REAL s, t = 0; + s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; + if (s > 0) t += s * s; + s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; + if (s > 0) t += s * s; + s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2]; + if (s > 0) t += s * s; + return t; +} - template - struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi - { - static inline bool run (int ia, int ja, int ka, - const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf, - const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - if (sinus2 < 1e-6) return false; - - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); - // We need to divide by the norm || Aia x Bib || - // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 - if (diff > 0) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } +template +struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi { + static inline bool run(int ia, int ja, int ka, const Matrix3f& B, + const Vec3f& T, const Vec3f& a, const Vec3f& b, + const Matrix3f& Bf, const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + if (sinus2 < 1e-6) return false; + + const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + + const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + // We need to divide by the norm || Aia x Bib || + // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 + if (diff > 0) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } - return false; } - }; -} - + return false; + } +}; +} // namespace internal // B, T orientation and position of 2nd OBB in frame of 1st OBB, // a extent of 1st OBB, @@ -356,93 +342,91 @@ namespace internal // // This function tests whether bounding boxes should be broken down. // -bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const CollisionRequest& request, - FCL_REAL& squaredLowerBoundDistance) -{ - const FCL_REAL breakDistance (request.break_distance + request.security_margin); +bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const CollisionRequest& request, + FCL_REAL& squaredLowerBoundDistance) { + const FCL_REAL breakDistance(request.break_distance + + request.security_margin); const FCL_REAL breakDistance2 = breakDistance * breakDistance; - Matrix3f Bf (B.cwiseAbs()); + Matrix3f Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin - squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis (T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return true; + squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; - squaredLowerBoundDistance = internal::obbDisjoint_check_B_axis (B, T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return true; + squaredLowerBoundDistance = + internal::obbDisjoint_check_B_axis(B, T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; // Ai x Bj int ja = 1, ka = 2; for (int ia = 0; ia < 3; ++ia) { - if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run (ia, ja, ka, - B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run (ia, ja, ka, - B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run (ia, ja, ka, - B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - ja = ka; ka = ia; + if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run( + ia, ja, ka, B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run( + ia, ja, ka, B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run( + ia, ja, ka, B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + ja = ka; + ka = ia; } return false; } -bool OBB::overlap(const OBB& other) const -{ +bool OBB::overlap(const OBB& other) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part - Vec3f T (axes.transpose() * (other.To - To)); - Matrix3f R (axes.transpose() * other.axes); + Vec3f T(axes.transpose() * (other.To - To)); + Matrix3f R(axes.transpose() * other.axes); return !obbDisjoint(R, T, extent, other.extent); } - bool OBB::overlap(const OBB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const - { - /// compute what transform [R,T] that takes us from cs1 to cs2. - /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] - /// First compute the rotation part, then translation part - // Vec3f t = other.To - To; // T2 - T1 - // Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) - // Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), - // axis[0].dot(other.axis[2]), - // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), - // axis[1].dot(other.axis[2]), - // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), - // axis[2].dot(other.axis[2])); - Vec3f T (axes.transpose() * (other.To - To)); - Matrix3f R (axes.transpose() * other.axes); - - return !obbDisjointAndLowerBoundDistance - (R, T, extent, other.extent, request, sqrDistLowerBound); +bool OBB::overlap(const OBB& other, const CollisionRequest& request, + FCL_REAL& sqrDistLowerBound) const { + /// compute what transform [R,T] that takes us from cs1 to cs2. + /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] + /// First compute the rotation part, then translation part + // Vec3f t = other.To - To; // T2 - T1 + // Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) + // Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), + // axis[0].dot(other.axis[2]), + // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), + // axis[1].dot(other.axis[2]), + // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), + // axis[2].dot(other.axis[2])); + Vec3f T(axes.transpose() * (other.To - To)); + Matrix3f R(axes.transpose() * other.axes); + + return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request, + sqrDistLowerBound); } - -bool OBB::contain(const Vec3f& p) const -{ - Vec3f local_p (p - To); +bool OBB::contain(const Vec3f& p) const { + Vec3f local_p(p - To); FCL_REAL proj = local_p.dot(axes.col(0)); - if((proj > extent[0]) || (proj < -extent[0])) - return false; + if ((proj > extent[0]) || (proj < -extent[0])) return false; proj = local_p.dot(axes.col(1)); - if((proj > extent[1]) || (proj < -extent[1])) - return false; + if ((proj > extent[1]) || (proj < -extent[1])) return false; proj = local_p.dot(axes.col(2)); - if((proj > extent[2]) || (proj < -extent[2])) - return false; + if ((proj > extent[2]) || (proj < -extent[2])) return false; return true; } -OBB& OBB::operator += (const Vec3f& p) -{ +OBB& OBB::operator+=(const Vec3f& p) { OBB bvp; bvp.To = p; bvp.axes.noalias() = axes; @@ -452,56 +436,48 @@ OBB& OBB::operator += (const Vec3f& p) return *this; } -OBB OBB::operator + (const OBB& other) const -{ +OBB OBB::operator+(const OBB& other) const { Vec3f center_diff = To - other.To; FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); - FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); - if(center_diff.norm() > 2 * (max_extent + max_extent2)) - { + FCL_REAL max_extent2 = + std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); + if (center_diff.norm() > 2 * (max_extent + max_extent2)) { return merge_largedist(*this, other); - } - else - { + } else { return merge_smalldist(*this, other); } } - -FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const -{ +FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const { std::cerr << "OBB distance not implemented!" << std::endl; return 0.0; } - -bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2) -{ - Vec3f Ttemp (R0 * b2.To + T0 - b1.To); - Vec3f T (b1.axes.transpose() * Ttemp); - Matrix3f R (b1.axes.transpose() * R0 * b2.axes); +bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, + const OBB& b2) { + Vec3f Ttemp(R0 * b2.To + T0 - b1.To); + Vec3f T(b1.axes.transpose() * Ttemp); + Matrix3f R(b1.axes.transpose() * R0 * b2.axes); return !obbDisjoint(R, T, b1.extent, b2.extent); } bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2, - const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) -{ - Vec3f Ttemp (R0 * b2.To + T0 - b1.To); - Vec3f T (b1.axes.transpose() * Ttemp); - Matrix3f R (b1.axes.transpose() * R0 * b2.axes); - - return !obbDisjointAndLowerBoundDistance (R, T, b1.extent, b2.extent, - request, sqrDistLowerBound); + const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { + Vec3f Ttemp(R0 * b2.To + T0 - b1.To); + Vec3f T(b1.axes.transpose() * Ttemp); + Matrix3f R(b1.axes.transpose() * R0 * b2.axes); + + return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request, + sqrDistLowerBound); } -OBB translate(const OBB& bv, const Vec3f& t) -{ +OBB translate(const OBB& bv, const Vec3f& t) { OBB res(bv); res.To += t; return res; } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/BV/OBB.h b/src/BV/OBB.h index 479714d8e..24d6ef877 100644 --- a/src/BV/OBB.h +++ b/src/BV/OBB.h @@ -34,22 +34,20 @@ */ #ifndef HPP_FCL_SRC_OBB_H -# define HPP_FCL_SRC_OBB_H +#define HPP_FCL_SRC_OBB_H -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { - bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const CollisionRequest& request, - FCL_REAL& squaredLowerBoundDistance); +bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const CollisionRequest& request, + FCL_REAL& squaredLowerBoundDistance); - bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b); -} // namespace fcl +bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, + const Vec3f& b); +} // namespace fcl -} // namespace hpp +} // namespace hpp -#endif // HPP_FCL_SRC_OBB_H +#endif // HPP_FCL_SRC_OBB_H diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp index 648c1c054..87a8494e8 100644 --- a/src/BV/OBBRSS.cpp +++ b/src/BV/OBBRSS.cpp @@ -37,20 +37,16 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -OBBRSS translate(const OBBRSS& bv, const Vec3f& t) -{ +OBBRSS translate(const OBBRSS& bv, const Vec3f& t) { OBBRSS res(bv); res.obb.To += t; res.rss.Tr += t; return res; } +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index 220e6b449..b6d91cf77 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -39,46 +39,45 @@ #include #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Clip value between a and b -void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) -{ - if(val < a) val = a; - else if(val > b) val = b; +void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) { + if (val < a) + val = a; + else if (val > b) + val = b; } -/// @brief Finds the parameters t & u corresponding to the two closest points on a pair of line segments. -/// The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector -/// pointing to the other endpoint, and t is a scalar that produces all the points between the two endpoints. Since "A" is a unit -/// vector, "a" is the segment's length. -/// The second segment is defined as Pb + B*u, 0 <= u <= b. -/// Many of the terms needed by the algorithm are already computed for other purposes,so we just pass these terms into the function -/// instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb. -/// Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. -void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) -{ +/// @brief Finds the parameters t & u corresponding to the two closest points on +/// a pair of line segments. The first segment is defined as Pa + A*t, 0 <= t <= +/// a, where "Pa" is one endpoint of the segment, "A" is a unit vector pointing +/// to the other endpoint, and t is a scalar that produces all the points +/// between the two endpoints. Since "A" is a unit vector, "a" is the segment's +/// length. The second segment is defined as Pb + B*u, 0 <= u <= b. Many of the +/// terms needed by the algorithm are already computed for other purposes,so we +/// just pass these terms into the function instead of complete specifications +/// of each segment. "T" in the dot products is the vector betweeen Pa and Pb. +/// Reference: "On fast computation of distance between line segments." Vladimir +/// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. +void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, + FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) { FCL_REAL denom = 1 - A_dot_B * A_dot_B; - if(denom == 0) t = 0; - else - { + if (denom == 0) + t = 0; + else { t = (A_dot_T - B_dot_T * A_dot_B) / denom; clipToRange(t, 0, a); } u = t * A_dot_B - B_dot_T; - if(u < 0) - { + if (u < 0) { u = 0; t = A_dot_T; clipToRange(t, 0, a); - } - else if(u > b) - { + } else if (u > b) { u = b; t = u * A_dot_B + A_dot_T; clipToRange(t, 0, a); @@ -90,9 +89,10 @@ void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_ /// Pa + A*t, 0 <= t <= a, is within the half space /// determined by the point Pa and the direction Anorm. /// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. -bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) -{ - if(fabs(Anorm_dot_B) < 1e-7) return false; +bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, + FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, + FCL_REAL B_dot_T) { + if (fabs(Anorm_dot_B) < 1e-7) return false; FCL_REAL t, u, v; @@ -104,21 +104,20 @@ bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_ v = t * A_dot_B - B_dot_T; - if(Anorm_dot_B > 0) - { - if(v > (u + 1e-7)) return true; - } - else - { - if(v < (u - 1e-7)) return true; + if (Anorm_dot_B > 0) { + if (v > (u + 1e-7)) return true; + } else { + if (v < (u - 1e-7)) return true; } return false; } - -/// @brief Distance between two oriented rectangles; P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle. -FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL) -{ +/// @brief Distance between two oriented rectangles; P and Q (optional return +/// values) are the closest points in the rectangles, both are in the local +/// frame of the first rectangle. +FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, + const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, + Vec3f* Q = NULL) { FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; A0_dot_B0 = Rab(0, 0); @@ -138,7 +137,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] bA0_dot_B1 = b[1] * A0_dot_B1; bA1_dot_B1 = b[1] * A1_dot_B1; - Vec3f Tba (Rab.transpose() * Tab); + Vec3f Tba(Rab.transpose() * Tab); Vec3f S; FCL_REAL t, u; @@ -154,15 +153,12 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] AUL_x = ALL_x + aA0_dot_B0; AUU_x = ALU_x + aA0_dot_B0; - if(ALL_x < ALU_x) - { + if (ALL_x < ALU_x) { LA1_lx = ALL_x; LA1_ux = ALU_x; UA1_lx = AUL_x; UA1_ux = AUU_x; - } - else - { + } else { LA1_lx = ALU_x; LA1_ux = ALL_x; UA1_lx = AUU_x; @@ -174,15 +170,12 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] BUL_x = BLL_x + bA0_dot_B0; BUU_x = BLU_x + bA0_dot_B0; - if(BLL_x < BLU_x) - { + if (BLL_x < BLU_x) { LB1_lx = BLL_x; LB1_ux = BLU_x; UB1_lx = BUL_x; UB1_ux = BUU_x; - } - else - { + } else { LB1_lx = BLU_x; LB1_ux = BLL_x; UB1_lx = BUU_x; @@ -191,26 +184,21 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // UA1, UB1 - if((UA1_ux > b[0]) && (UB1_ux > a[0])) - { - if(((UA1_lx > b[0]) || - inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], - A1_dot_B1, aA0_dot_B1 - Tba[1], - -Tab[1] - bA1_dot_B0)) - && - ((UB1_lx > a[0]) || - inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], - A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) - { + if ((UA1_ux > b[0]) && (UB1_ux > a[0])) { + if (((UA1_lx > b[0]) || + inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], A1_dot_B1, + aA0_dot_B1 - Tba[1], -Tab[1] - bA1_dot_B0)) && + ((UB1_lx > a[0]) || + inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], A1_dot_B1, + Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1); - S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ; + S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0]; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; - if(P && Q) - { + if (P && Q) { *P << a[0], t, 0; *Q = S + (*P); } @@ -219,27 +207,21 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] } } - // UA1, LB1 - if((UA1_lx < 0) && (LB1_ux > a[0])) - { - if(((UA1_ux < 0) || - inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0, - A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) - && - ((LB1_lx > a[0]) || - inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], - A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1))) - { + if ((UA1_lx < 0) && (LB1_ux > a[0])) { + if (((UA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0, + A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) && + ((LB1_lx > a[0]) || + inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], A1_dot_B1, Tab[1], + Tba[1] - aA0_dot_B1))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1); S[0] = Tab[0] + Rab(0, 1) * u - a[0]; S[1] = Tab[1] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 1) * u; - if(P && Q) - { + if (P && Q) { *P << a[0], t, 0; *Q = S + (*P); } @@ -250,24 +232,19 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // LA1, UB1 - if((LA1_ux > b[0]) && (UB1_lx < 0)) - { - if(((LA1_lx > b[0]) || - inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], - A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0)) - && - ((UB1_ux < 0) || - inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0, - A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) - { + if ((LA1_ux > b[0]) && (UB1_lx < 0)) { + if (((LA1_lx > b[0]) || + inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], A1_dot_B1, -Tba[1], + -Tab[1] - bA1_dot_B0)) && + ((UB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0, + A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; - if(P && Q) - { + if (P && Q) { *P << 0, t, 0; *Q = S + (*P); } @@ -278,24 +255,18 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // LA1, LB1 - if((LA1_lx < 0) && (LB1_lx < 0)) - { - if (((LA1_ux < 0) || - inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1, - -Tba[1], -Tab[1])) - && - ((LB1_ux < 0) || - inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1, - Tab[1], Tba[1]))) - { + if ((LA1_lx < 0) && (LB1_lx < 0)) { + if (((LA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1, + -Tba[1], -Tab[1])) && + ((LB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1, + Tab[1], Tba[1]))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]); S[0] = Tab[0] + Rab(0, 1) * u; S[1] = Tab[1] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 1) * u; - if(P && Q) - { + if (P && Q) { *P << 0, t, 0; *Q = S + (*P); } @@ -313,30 +284,24 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; - if(ALL_y < ALU_y) - { + if (ALL_y < ALU_y) { LA1_ly = ALL_y; LA1_uy = ALU_y; UA1_ly = AUL_y; UA1_uy = AUU_y; - } - else - { + } else { LA1_ly = ALU_y; LA1_uy = ALL_y; UA1_ly = AUU_y; UA1_uy = AUL_y; } - if(BLL_x < BUL_x) - { + if (BLL_x < BUL_x) { LB0_lx = BLL_x; LB0_ux = BUL_x; UB0_lx = BLU_x; UB0_ux = BUU_x; - } - else - { + } else { LB0_lx = BUL_x; LB0_ux = BLL_x; UB0_lx = BUU_x; @@ -345,25 +310,21 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // UA1, UB0 - if((UA1_uy > b[1]) && (UB0_ux > a[0])) - { - if(((UA1_ly > b[1]) || - inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], - A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) - && - ((UB0_lx > a[0]) || - inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, - A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) - { + if ((UA1_uy > b[1]) && (UB0_ux > a[0])) { + if (((UA1_ly > b[1]) || + inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], A1_dot_B0, + aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) && + ((UB0_lx > a[0]) || + inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, A1_dot_B0, + Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0); - S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0] ; + S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0]; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; - if(P && Q) - { + if (P && Q) { *P << a[0], t, 0; *Q = S + (*P); } @@ -374,24 +335,19 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // UA1, LB0 - if((UA1_ly < 0) && (LB0_ux > a[0])) - { - if(((UA1_uy < 0) || - inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0, - aA0_dot_B0 - Tba[0], -Tab[1])) - && - ((LB0_lx > a[0]) || - inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], - A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0))) - { + if ((UA1_ly < 0) && (LB0_ux > a[0])) { + if (((UA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, + A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1])) && + ((LB0_lx > a[0]) || + inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], A1_dot_B0, Tab[1], + Tba[0] - aA0_dot_B0))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0); S[0] = Tab[0] + Rab(0, 0) * u - a[0]; S[1] = Tab[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 0) * u; - if(P && Q) - { + if (P && Q) { *P << a[0], t, 0; *Q = S + (*P); } @@ -402,54 +358,42 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // LA1, UB0 - if((LA1_uy > b[1]) && (UB0_lx < 0)) - { - if(((LA1_ly > b[1]) || - inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], - A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1)) - && - - ((UB0_ux < 0) || - inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0, - Tab[1] + bA1_dot_B1, Tba[0]))) - { + if ((LA1_uy > b[1]) && (UB0_lx < 0)) { + if (((LA1_ly > b[1]) || + inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], A1_dot_B0, -Tba[0], + -Tab[1] - bA1_dot_B1)) && + + ((UB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, + A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; - if(P && Q) - { + if (P && Q) { *P << 0, t, 0; *Q = S + (*P); } - return S.norm(); } } // LA1, LB0 - if((LA1_ly < 0) && (LB0_lx < 0)) - { - if(((LA1_uy < 0) || - inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0, - -Tba[0], -Tab[1])) - && - ((LB0_ux < 0) || - inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0, - Tab[1], Tba[0]))) - { + if ((LA1_ly < 0) && (LB0_lx < 0)) { + if (((LA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0, + -Tba[0], -Tab[1])) && + ((LB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0, + Tab[1], Tba[0]))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]); S[0] = Tab[0] + Rab(0, 0) * u; S[1] = Tab[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 0) * u; - if(P&& Q) - { + if (P && Q) { *P << 0, t, 0; *Q = S + (*P); } @@ -467,30 +411,24 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; - if(ALL_x < AUL_x) - { + if (ALL_x < AUL_x) { LA0_lx = ALL_x; LA0_ux = AUL_x; UA0_lx = ALU_x; UA0_ux = AUU_x; - } - else - { + } else { LA0_lx = AUL_x; LA0_ux = ALL_x; UA0_lx = AUU_x; UA0_ux = ALU_x; } - if(BLL_y < BLU_y) - { + if (BLL_y < BLU_y) { LB1_ly = BLL_y; LB1_uy = BLU_y; UB1_ly = BUL_y; UB1_uy = BUU_y; - } - else - { + } else { LB1_ly = BLU_y; LB1_uy = BLL_y; UB1_ly = BUU_y; @@ -499,16 +437,13 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // UA0, UB1 - if((UA0_ux > b[0]) && (UB1_uy > a[1])) - { - if(((UA0_lx > b[0]) || - inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], - A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) - && - ((UB1_ly > a[1]) || - inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, - A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) - { + if ((UA0_ux > b[0]) && (UB1_uy > a[1])) { + if (((UA0_lx > b[0]) || + inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], A0_dot_B1, + aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) && + ((UB1_ly > a[1]) || + inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, A0_dot_B1, + Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1); @@ -516,8 +451,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1]; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; - if(P && Q) - { + if (P && Q) { *P << t, a[1], 0; *Q = S + (*P); } @@ -528,24 +462,19 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // UA0, LB1 - if((UA0_lx < 0) && (LB1_uy > a[1])) - { - if(((UA0_ux < 0) || - inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1, - aA1_dot_B1 - Tba[1], -Tab[0])) - && - ((LB1_ly > a[1]) || - inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0], - Tba[1] - aA1_dot_B1))) - { + if ((UA0_lx < 0) && (LB1_uy > a[1])) { + if (((UA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, + A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0])) && + ((LB1_ly > a[1]) || + inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0], + Tba[1] - aA1_dot_B1))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1); S[0] = Tab[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 1) * u - a[1]; S[2] = Tab[2] + Rab(2, 1) * u; - if(P && Q) - { + if (P && Q) { *P << t, a[1], 0; *Q = S + (*P); } @@ -556,24 +485,19 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // LA0, UB1 - if((LA0_ux > b[0]) && (UB1_ly < 0)) - { - if(((LA0_lx > b[0]) || - inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1], - -bA0_dot_B0 - Tab[0])) - && - ((UB1_uy < 0) || - inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1, - Tab[0] + bA0_dot_B0, Tba[1]))) - { + if ((LA0_ux > b[0]) && (UB1_ly < 0)) { + if (((LA0_lx > b[0]) || + inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1], + -bA0_dot_B0 - Tab[0])) && + ((UB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, + A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; - if(P && Q) - { + if (P && Q) { *P << t, 0, 0; *Q = S + (*P); } @@ -584,24 +508,18 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // LA0, LB1 - if((LA0_lx < 0) && (LB1_ly < 0)) - { - if(((LA0_ux < 0) || - inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1], - -Tab[0])) - && - ((LB1_uy < 0) || - inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1, - Tab[0], Tba[1]))) - { + if ((LA0_lx < 0) && (LB1_ly < 0)) { + if (((LA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, + -Tba[1], -Tab[0])) && + ((LB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1, + Tab[0], Tba[1]))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]); S[0] = Tab[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 1) * u; S[2] = Tab[2] + Rab(2, 1) * u; - if(P && Q) - { + if (P && Q) { *P << t, 0, 0; *Q = S + (*P); } @@ -612,30 +530,24 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; - if(ALL_y < AUL_y) - { + if (ALL_y < AUL_y) { LA0_ly = ALL_y; LA0_uy = AUL_y; UA0_ly = ALU_y; UA0_uy = AUU_y; - } - else - { + } else { LA0_ly = AUL_y; LA0_uy = ALL_y; UA0_ly = AUU_y; UA0_uy = ALU_y; } - if(BLL_y < BUL_y) - { + if (BLL_y < BUL_y) { LB0_ly = BLL_y; LB0_uy = BUL_y; UB0_ly = BLU_y; UB0_uy = BUU_y; - } - else - { + } else { LB0_ly = BUL_y; LB0_uy = BLL_y; UB0_ly = BUU_y; @@ -644,16 +556,13 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // UA0, UB0 - if((UA0_uy > b[1]) && (UB0_uy > a[1])) - { - if(((UA0_ly > b[1]) || - inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], - A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) - && - ((UB0_ly > a[1]) || - inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0, - Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) - { + if ((UA0_uy > b[1]) && (UB0_uy > a[1])) { + if (((UA0_ly > b[1]) || + inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], A0_dot_B0, + aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) && + ((UB0_ly > a[1]) || + inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0, + Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0); @@ -661,8 +570,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1]; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; - if(P && Q) - { + if (P && Q) { *P << t, a[1], 0; *Q = S + (*P); } @@ -673,24 +581,19 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // UA0, LB0 - if((UA0_ly < 0) && (LB0_uy > a[1])) - { - if(((UA0_uy < 0) || - inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0, - aA1_dot_B0 - Tba[0], -Tab[0])) - && - ((LB0_ly > a[1]) || - inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], - A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0))) - { + if ((UA0_ly < 0) && (LB0_uy > a[1])) { + if (((UA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, + A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0])) && + ((LB0_ly > a[1]) || + inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], A0_dot_B0, Tab[0], + Tba[0] - aA1_dot_B0))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0); S[0] = Tab[0] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 0) * u - a[1]; S[2] = Tab[2] + Rab(2, 0) * u; - if(P && Q) - { + if (P && Q) { *P << t, a[1], 0; *Q = S + (*P); } @@ -701,25 +604,20 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // LA0, UB0 - if((LA0_uy > b[1]) && (UB0_ly < 0)) - { - if(((LA0_ly > b[1]) || - inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0], - -Tab[0] - bA0_dot_B1)) - && - - ((UB0_uy < 0) || - inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0, - Tab[0] + bA0_dot_B1, Tba[0]))) - { + if ((LA0_uy > b[1]) && (UB0_ly < 0)) { + if (((LA0_ly > b[1]) || + inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0], + -Tab[0] - bA0_dot_B1)) && + + ((UB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, + A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; - if(P && Q) - { + if (P && Q) { *P << t, 0, 0; *Q = S + (*P); } @@ -730,24 +628,18 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] // LA0, LB0 - if((LA0_ly < 0) && (LB0_ly < 0)) - { - if(((LA0_uy < 0) || - inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0, - -Tba[0], -Tab[0])) - && - ((LB0_uy < 0) || - inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0, - Tab[0], Tba[0]))) - { + if ((LA0_ly < 0) && (LB0_ly < 0)) { + if (((LA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0, + -Tba[0], -Tab[0])) && + ((LB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0, + Tab[0], Tba[0]))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]); S[0] = Tab[0] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 0) * u; S[2] = Tab[2] + Rab(2, 0) * u; - if(P && Q) - { + if (P && Q) { *P << t, 0, 0; *Q = S + (*P); } @@ -760,58 +652,46 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] FCL_REAL sep1, sep2; - if(Tab[2] > 0.0) - { + if (Tab[2] > 0.0) { sep1 = Tab[2]; if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0); if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1); - } - else - { + } else { sep1 = -Tab[2]; if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0); if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1); } - if(Tba[2] < 0) - { + if (Tba[2] < 0) { sep2 = -Tba[2]; if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2); if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2); - } - else - { + } else { sep2 = Tba[2]; if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2); if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2); } - if(sep1 >= sep2 && sep1 >= 0) - { - if(Tab[2] > 0) + if (sep1 >= sep2 && sep1 >= 0) { + if (Tab[2] > 0) S << 0, 0, sep1; else S << 0, 0, -sep1; - if(P && Q) - { + if (P && Q) { *Q = S; P->setZero(); } } - if(sep2 >= sep1 && sep2 >= 0) - { + if (sep2 >= sep1 && sep2 >= 0) { Vec3f Q_(Tab[0], Tab[1], Tab[2]); Vec3f P_; - if(Tba[2] < 0) - { + if (Tba[2] < 0) { P_[0] = Rab(0, 2) * sep2 + Tab[0]; P_[1] = Rab(1, 2) * sep2 + Tab[1]; P_[2] = Rab(2, 2) * sep2 + Tab[2]; - } - else - { + } else { P_[0] = -Rab(0, 2) * sep2 + Tab[0]; P_[1] = -Rab(1, 2) * sep2 + Tab[1]; P_[2] = -Rab(2, 2) * sep2 + Tab[2]; @@ -819,8 +699,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] S = Q_ - P_; - if(P && Q) - { + if (P && Q) { *P = P_; *Q = Q_; } @@ -830,32 +709,29 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2] return (sep > 0 ? sep : 0); } - - -bool RSS::overlap(const RSS& other) const -{ +bool RSS::overlap(const RSS& other) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part /// Then compute R1'(T2 - T1) - Vec3f T (axes.transpose() * (other.Tr - Tr)); + Vec3f T(axes.transpose() * (other.Tr - Tr)); /// Now compute R1'R2 - Matrix3f R (axes.transpose() * other.axes); + Matrix3f R(axes.transpose() * other.axes); FCL_REAL dist = rectDistance(R, T, length, other.length); return (dist <= (radius + other.radius)); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2) -{ +bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, + const RSS& b2) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] // R = b2^T RO^T b1 - Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr); + Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr); Vec3f T(b1.axes.transpose() * Ttemp); Matrix3f R(b1.axes.transpose() * R0 * b2.axes); @@ -864,26 +740,24 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2) } bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, - const CollisionRequest& /*request*/, - FCL_REAL& sqrDistLowerBound) -{ + const CollisionRequest& /*request*/, FCL_REAL& sqrDistLowerBound) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] // R = b2^T RO^T b1 - Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr); + Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr); Vec3f T(b1.axes.transpose() * Ttemp); Matrix3f R(b1.axes.transpose() * R0 * b2.axes); - FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius; + FCL_REAL dist = + rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius; if (dist <= 0) return true; sqrDistLowerBound = dist * dist; return false; } -bool RSS::contain(const Vec3f& p) const -{ +bool RSS::contain(const Vec3f& p) const { Vec3f local_p = p - Tr; // FIXME: Vec3f proj (axes.transpose() * local_p); FCL_REAL proj0 = local_p.dot(axes.col(0)); @@ -893,24 +767,20 @@ bool RSS::contain(const Vec3f& p) const Vec3f proj(proj0, proj1, proj2); /// projection is within the rectangle - if((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0)) - { + if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && + (proj1 > 0)) { return (abs_proj2 < radius); - } - else if((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) - { + } else if ((proj0 < length[0]) && (proj0 > 0) && + ((proj1 < 0) || (proj1 > length[1]))) { FCL_REAL y = (proj1 > 0) ? length[1] : 0; Vec3f v(proj0, y, 0); return ((proj - v).squaredNorm() < radius * radius); - } - else if((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) - { + } else if ((proj1 < length[1]) && (proj1 > 0) && + ((proj0 < 0) || (proj0 > length[0]))) { FCL_REAL x = (proj0 > 0) ? length[0] : 0; Vec3f v(x, proj1, 0); return ((proj - v).squaredNorm() < radius * radius); - } - else - { + } else { FCL_REAL x = (proj0 > 0) ? length[0] : 0; FCL_REAL y = (proj1 > 0) ? length[1] : 0; Vec3f v(x, y, 0); @@ -918,8 +788,7 @@ bool RSS::contain(const Vec3f& p) const } } -RSS& RSS::operator += (const Vec3f& p) -{ +RSS& RSS::operator+=(const Vec3f& p) { Vec3f local_p = p - Tr; FCL_REAL proj0 = local_p.dot(axes.col(0)); FCL_REAL proj1 = local_p.dot(axes.col(1)); @@ -928,121 +797,101 @@ RSS& RSS::operator += (const Vec3f& p) Vec3f proj(proj0, proj1, proj2); // projection is within the rectangle - if((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0)) - { - if(abs_proj2 < radius) - ; // do nothing - else - { - radius = 0.5 * (radius + abs_proj2); // enlarge the r + if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && + (proj1 > 0)) { + if (abs_proj2 < radius) + ; // do nothing + else { + radius = 0.5 * (radius + abs_proj2); // enlarge the r // change RSS origin position - if(proj2 > 0) + if (proj2 > 0) Tr[2] += 0.5 * (abs_proj2 - radius); else Tr[2] -= 0.5 * (abs_proj2 - radius); } - } - else if((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) - { + } else if ((proj0 < length[0]) && (proj0 > 0) && + ((proj1 < 0) || (proj1 > length[1]))) { FCL_REAL y = (proj1 > 0) ? length[1] : 0; Vec3f v(proj0, y, 0); FCL_REAL new_r_sqr = (proj - v).squaredNorm(); - if(new_r_sqr < radius * radius) - ; // do nothing - else - { - if(abs_proj2 < radius) - { - FCL_REAL delta_y = - std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y); + if (new_r_sqr < radius * radius) + ; // do nothing + else { + if (abs_proj2 < radius) { + FCL_REAL delta_y = + -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y); length[1] += delta_y; - if(proj1 < 0) - Tr[1] -= delta_y; - } - else - { + if (proj1 < 0) Tr[1] -= delta_y; + } else { FCL_REAL delta_y = fabs(proj1 - y); length[1] += delta_y; - if(proj1 < 0) - Tr[1] -= delta_y; + if (proj1 < 0) Tr[1] -= delta_y; - if(proj2 > 0) + if (proj2 > 0) Tr[2] += 0.5 * (abs_proj2 - radius); else Tr[2] -= 0.5 * (abs_proj2 - radius); } } - } - else if((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) - { + } else if ((proj1 < length[1]) && (proj1 > 0) && + ((proj0 < 0) || (proj0 > length[0]))) { FCL_REAL x = (proj0 > 0) ? length[0] : 0; Vec3f v(x, proj1, 0); FCL_REAL new_r_sqr = (proj - v).squaredNorm(); - if(new_r_sqr < radius * radius) - ; // do nothing - else - { - if(abs_proj2 < radius) - { - FCL_REAL delta_x = - std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x); + if (new_r_sqr < radius * radius) + ; // do nothing + else { + if (abs_proj2 < radius) { + FCL_REAL delta_x = + -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x); length[0] += delta_x; - if(proj0 < 0) - Tr[0] -= delta_x; - } - else - { + if (proj0 < 0) Tr[0] -= delta_x; + } else { FCL_REAL delta_x = fabs(proj0 - x); length[0] += delta_x; - if(proj0 < 0) - Tr[0] -= delta_x; + if (proj0 < 0) Tr[0] -= delta_x; - if(proj2 > 0) + if (proj2 > 0) Tr[2] += 0.5 * (abs_proj2 - radius); else Tr[2] -= 0.5 * (abs_proj2 - radius); } } - } - else - { + } else { FCL_REAL x = (proj0 > 0) ? length[0] : 0; FCL_REAL y = (proj1 > 0) ? length[1] : 0; Vec3f v(x, y, 0); FCL_REAL new_r_sqr = (proj - v).squaredNorm(); - if(new_r_sqr < radius * radius) - ; // do nothing - else - { - if(abs_proj2 < radius) - { + if (new_r_sqr < radius * radius) + ; // do nothing + else { + if (abs_proj2 < radius) { FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2); - FCL_REAL delta_diag = - std::sqrt(radius * radius - proj2 * proj2) + diag; + FCL_REAL delta_diag = + -std::sqrt(radius * radius - proj2 * proj2) + diag; FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x); FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y); length[0] += delta_x; length[1] += delta_y; - if(proj0 < 0 && proj1 < 0) - { + if (proj0 < 0 && proj1 < 0) { Tr[0] -= delta_x; Tr[1] -= delta_y; } - } - else - { + } else { FCL_REAL delta_x = fabs(proj0 - x); FCL_REAL delta_y = fabs(proj1 - y); length[0] += delta_x; length[1] += delta_y; - if(proj0 < 0 && proj1 < 0) - { + if (proj0 < 0 && proj1 < 0) { Tr[0] -= delta_x; Tr[1] -= delta_y; } - if(proj2 > 0) + if (proj2 > 0) Tr[2] += 0.5 * (abs_proj2 - radius); else Tr[2] -= 0.5 * (abs_proj2 - radius); @@ -1053,17 +902,16 @@ RSS& RSS::operator += (const Vec3f& p) return *this; } -RSS RSS::operator + (const RSS& other) const -{ +RSS RSS::operator+(const RSS& other) const { RSS bv; Vec3f v[16]; - Vec3f d0_pos (other.axes.col(0) * (other.length[0] + other.radius)); - Vec3f d1_pos (other.axes.col(1) * (other.length[1] + other.radius)); - Vec3f d0_neg (other.axes.col(0) * (-other.radius)); - Vec3f d1_neg (other.axes.col(1) * (-other.radius)); - Vec3f d2_pos (other.axes.col(2) * other.radius); - Vec3f d2_neg (other.axes.col(2) * (-other.radius)); + Vec3f d0_pos(other.axes.col(0) * (other.length[0] + other.radius)); + Vec3f d1_pos(other.axes.col(1) * (other.length[1] + other.radius)); + Vec3f d0_neg(other.axes.col(0) * (-other.radius)); + Vec3f d1_neg(other.axes.col(1) * (-other.radius)); + Vec3f d2_pos(other.axes.col(2) * other.radius); + Vec3f d2_neg(other.axes.col(2) * (-other.radius)); v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos; v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg; @@ -1081,8 +929,8 @@ RSS RSS::operator + (const RSS& other) const d2_pos.noalias() = axes.col(2) * radius; d2_neg.noalias() = axes.col(2) * (-radius); - v[ 8].noalias() = Tr + d0_pos + d1_pos + d2_pos; - v[ 9].noalias() = Tr + d0_pos + d1_pos + d2_neg; + v[8].noalias() = Tr + d0_pos + d1_pos + d2_pos; + v[9].noalias() = Tr + d0_pos + d1_pos + d2_neg; v[10].noalias() = Tr + d0_pos + d1_neg + d2_pos; v[11].noalias() = Tr + d0_pos + d1_neg + d2_neg; v[12].noalias() = Tr + d0_neg + d1_pos + d2_pos; @@ -1090,51 +938,61 @@ RSS RSS::operator + (const RSS& other) const v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos; v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg; - - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors Matrix3f::Scalar s[3] = {0, 0, 0}; getCovariance(v, NULL, NULL, NULL, 16, M); eigen(M, s, E); int min, mid, max; - if(s[0] > s[1]) { max = 0; min = 1; } - else { min = 0; max = 1; } - if(s[2] < s[min]) { mid = min; min = 2; } - else if(s[2] > s[max]) { mid = max; max = 2; } - else { mid = 2; } + if (s[0] > s[1]) { + max = 0; + min = 1; + } else { + min = 0; + max = 1; + } + if (s[2] < s[min]) { + mid = min; + min = 2; + } else if (s[2] > s[max]) { + mid = max; + max = 2; + } else { + mid = 2; + } // column first matrix, as the axis in RSS bv.axes.col(0) << E[0][max], E[1][max], E[2][max]; bv.axes.col(1) << E[0][mid], E[1][mid], E[2][mid]; - bv.axes.col(2) << E[1][max]*E[2][mid] - E[1][mid]*E[2][max], - E[0][mid]*E[2][max] - E[0][max]*E[2][mid], - E[0][max]*E[1][mid] - E[0][mid]*E[1][max]; + bv.axes.col(2) << E[1][max] * E[2][mid] - E[1][mid] * E[2][max], + E[0][mid] * E[2][max] - E[0][max] * E[2][mid], + E[0][max] * E[1][mid] - E[0][mid] * E[1][max]; // set rss origin, rectangle size and radius - getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.length, bv.radius); + getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, + bv.length, bv.radius); return bv; } -FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const -{ +FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const { // compute what transform [R,T] that takes us from cs1 to cs2. // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] // First compute the rotation part, then translation part - Matrix3f R (axes.transpose() * other.axes); - Vec3f T (axes.transpose() * (other.Tr - Tr)); + Matrix3f R(axes.transpose() * other.axes); + Vec3f T(axes.transpose() * (other.Tr - Tr)); FCL_REAL dist = rectDistance(R, T, length, other.length, P, Q); dist -= (radius + other.radius); return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; } -FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q) -{ - Matrix3f R (b1.axes.transpose() * R0 * b2.axes); - Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr); +FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, + const RSS& b2, Vec3f* P, Vec3f* Q) { + Matrix3f R(b1.axes.transpose() * R0 * b2.axes); + Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr); Vec3f T(b1.axes.transpose() * Ttemp); @@ -1143,17 +1001,12 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; } -RSS translate(const RSS& bv, const Vec3f& t) -{ +RSS translate(const RSS& bv, const Vec3f& t) { RSS res(bv); res.Tr += t; return res; } +} // namespace fcl - - - -} - -} // namespace hpp +} // namespace hpp diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp index 092f283fa..3e92cb30d 100644 --- a/src/BV/kDOP.cpp +++ b/src/BV/kDOP.cpp @@ -41,41 +41,33 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @brief Find the smaller and larger one of two values -inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) -{ - if(a > b) - { +inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) { + if (a > b) { minv = b; maxv = a; - } - else - { + } else { minv = a; maxv = b; } } /// @brief Merge the interval [minv, maxv] and value p/ -inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) -{ - if(p > maxv) maxv = p; - if(p < minv) minv = p; +inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) { + if (p > maxv) maxv = p; + if (p < minv) minv = p; } - -/// @brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes -template +/// @brief Compute the distances to planes with normals from KDOP vectors except +/// those of AABB face planes +template void getDistances(const Vec3f& /*p*/, FCL_REAL* /*d*/) {} /// @brief Specification of getDistances -template<> -inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) -{ +template <> +inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -83,9 +75,8 @@ inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) d[4] = p[0] - p[2]; } -template<> -inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) -{ +template <> +inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -94,9 +85,8 @@ inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) d[5] = p[1] - p[2]; } -template<> -inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) -{ +template <> +inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -108,70 +98,68 @@ inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) d[8] = p[1] + p[2] - p[0]; } -template -KDOP::KDOP() -{ +template +KDOP::KDOP() { FCL_REAL real_max = (std::numeric_limits::max)(); - dist_.template head().setConstant( real_max); - dist_.template tail().setConstant(-real_max); + dist_.template head().setConstant(real_max); + dist_.template tail().setConstant(-real_max); } -template -KDOP::KDOP(const Vec3f& v) -{ - for(short i = 0; i < 3; ++i) - { +template +KDOP::KDOP(const Vec3f& v) { + for (short i = 0; i < 3; ++i) { dist_[i] = dist_[N / 2 + i] = v[i]; } FCL_REAL d[(N - 6) / 2]; getDistances<(N - 6) / 2>(v, d); - for(short i = 0; i < (N - 6) / 2; ++i) - { + for (short i = 0; i < (N - 6) / 2; ++i) { dist_[3 + i] = dist_[3 + i + N / 2] = d[i]; } } -template -KDOP::KDOP(const Vec3f& a, const Vec3f& b) -{ - for(short i = 0; i < 3; ++i) - { +template +KDOP::KDOP(const Vec3f& a, const Vec3f& b) { + for (short i = 0; i < 3; ++i) { minmax(a[i], b[i], dist_[i], dist_[i + N / 2]); } FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2]; getDistances<(N - 6) / 2>(a, ad); getDistances<(N - 6) / 2>(b, bd); - for(short i = 0; i < (N - 6) / 2; ++i) - { + for (short i = 0; i < (N - 6) / 2; ++i) { minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]); } } - -template -bool KDOP::overlap(const KDOP& other) const -{ - if ((dist_.template head() > other.dist_.template tail()).any()) return false; - if ((dist_.template tail() < other.dist_.template head()).any()) return false; + +template +bool KDOP::overlap(const KDOP& other) const { + if ((dist_.template head() > other.dist_.template tail()).any()) + return false; + if ((dist_.template tail() < other.dist_.template head()).any()) + return false; return true; } -template +template bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const -{ - const FCL_REAL breakDistance (request.break_distance + request.security_margin); + FCL_REAL& sqrDistLowerBound) const { + const FCL_REAL breakDistance(request.break_distance + + request.security_margin); - FCL_REAL a = (dist_.template head() - other.dist_.template tail()).minCoeff(); + FCL_REAL a = + (dist_.template head() - other.dist_.template tail()) + .minCoeff(); if (a > breakDistance) { - sqrDistLowerBound = a*a; + sqrDistLowerBound = a * a; return false; } - FCL_REAL b = (other.dist_.template head() - dist_.template tail()).minCoeff(); + FCL_REAL b = + (other.dist_.template head() - dist_.template tail()) + .minCoeff(); if (b > breakDistance) { - sqrDistLowerBound = b*b; + sqrDistLowerBound = b * b; return false; } @@ -179,81 +167,69 @@ bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, return true; } -template -bool KDOP::inside(const Vec3f& p) const -{ +template +bool KDOP::inside(const Vec3f& p) const { if ((p.array() < dist_.template head<3>()).any()) return false; - if ((p.array() > dist_.template segment<3>(N/2)).any()) return false; + if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false; - enum { P = ((N-6)/2) }; + enum { P = ((N - 6) / 2) }; Eigen::Array d; getDistances

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

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

(3+N/2)).any()) return false; + if ((d < dist_.template segment

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

(3 + N / 2)).any()) return false; return true; } -template -KDOP& KDOP::operator += (const Vec3f& p) -{ - for(short i = 0; i < 3; ++i) - { +template +KDOP& KDOP::operator+=(const Vec3f& p) { + for (short i = 0; i < 3; ++i) { minmax(p[i], dist_[i], dist_[N / 2 + i]); } - + FCL_REAL pd[(N - 6) / 2]; getDistances<(N - 6) / 2>(p, pd); - for(short i = 0; i < (N - 6) / 2; ++i) - { + for (short i = 0; i < (N - 6) / 2; ++i) { minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]); } return *this; } -template -KDOP& KDOP::operator += (const KDOP& other) -{ - for(short i = 0; i < N / 2; ++i) - { +template +KDOP& KDOP::operator+=(const KDOP& other) { + for (short i = 0; i < N / 2; ++i) { dist_[i] = std::min(other.dist_[i], dist_[i]); dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]); } return *this; } -template -KDOP KDOP::operator + (const KDOP& other) const -{ +template +KDOP KDOP::operator+(const KDOP& other) const { KDOP res(*this); return res += other; } - -template -FCL_REAL KDOP::distance(const KDOP& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const -{ +template +FCL_REAL KDOP::distance(const KDOP& /*other*/, Vec3f* /*P*/, + Vec3f* /*Q*/) const { std::cerr << "KDOP distance not implemented!" << std::endl; return 0.0; } - -template -KDOP translate(const KDOP& bv, const Vec3f& t) -{ +template +KDOP translate(const KDOP& bv, const Vec3f& t) { KDOP res(bv); - for(short i = 0; i < 3; ++i) - { + for (short i = 0; i < 3; ++i) { res.dist(i) += t[i]; res.dist(short(N / 2 + i)) += t[i]; } FCL_REAL d[(N - 6) / 2]; getDistances<(N - 6) / 2>(t, d); - for(short i = 0; i < (N - 6) / 2; ++i) - { + for (short i = 0; i < (N - 6) / 2; ++i) { res.dist(short(3 + i)) += d[i]; res.dist(short(3 + i + N / 2)) += d[i]; } @@ -261,7 +237,6 @@ KDOP translate(const KDOP& bv, const Vec3f& t) return res; } - template class KDOP<16>; template class KDOP<18>; template class KDOP<24>; @@ -270,6 +245,6 @@ template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&); template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&); template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&); -} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp index 2abb6e017..5ba30335d 100644 --- a/src/BV/kIOS.cpp +++ b/src/BV/kIOS.cpp @@ -42,21 +42,15 @@ #include #include -namespace hpp -{ -namespace fcl -{ - -bool kIOS::overlap(const kIOS& other) const -{ - for(unsigned int i = 0; i < num_spheres; ++i) - { - for(unsigned int j = 0; j < other.num_spheres; ++j) - { +namespace hpp { +namespace fcl { + +bool kIOS::overlap(const kIOS& other) const { + for (unsigned int i = 0; i < num_spheres; ++i) { + for (unsigned int j = 0; j < other.num_spheres; ++j) { FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; - if(o_dist > sum_r * sum_r) - return false; + if (o_dist > sum_r * sum_r) return false; } } @@ -64,17 +58,14 @@ bool kIOS::overlap(const kIOS& other) const } bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const -{ - for(unsigned int i = 0; i < num_spheres; ++i) - { - for(unsigned int j = 0; j < other.num_spheres; ++j) - { + FCL_REAL& sqrDistLowerBound) const { + for (unsigned int i = 0; i < num_spheres; ++i) { + for (unsigned int j = 0; j < other.num_spheres; ++j) { FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; - if(o_dist > sum_r * sum_r) { + if (o_dist > sum_r * sum_r) { o_dist = sqrt(o_dist) - sum_r; - sqrDistLowerBound = o_dist*o_dist; + sqrDistLowerBound = o_dist * o_dist; return false; } } @@ -83,26 +74,20 @@ bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, return obb.overlap(other.obb, request, sqrDistLowerBound); } -bool kIOS::contain(const Vec3f& p) const -{ - for(unsigned int i = 0; i < num_spheres; ++i) - { +bool kIOS::contain(const Vec3f& p) const { + for (unsigned int i = 0; i < num_spheres; ++i) { FCL_REAL r = spheres[i].r; - if((spheres[i].o - p).squaredNorm() > r * r) - return false; + if ((spheres[i].o - p).squaredNorm() > r * r) return false; } return true; } -kIOS& kIOS::operator += (const Vec3f& p) -{ - for(unsigned int i = 0; i < num_spheres; ++i) - { +kIOS& kIOS::operator+=(const Vec3f& p) { + for (unsigned int i = 0; i < num_spheres; ++i) { FCL_REAL r = spheres[i].r; FCL_REAL new_r_sqr = (p - spheres[i].o).squaredNorm(); - if(new_r_sqr > r * r) - { + if (new_r_sqr > r * r) { spheres[i].r = sqrt(new_r_sqr); } } @@ -111,71 +96,49 @@ kIOS& kIOS::operator += (const Vec3f& p) return *this; } -kIOS kIOS::operator + (const kIOS& other) const -{ +kIOS kIOS::operator+(const kIOS& other) const { kIOS result; unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres); - for(unsigned int i = 0; i < new_num_spheres; ++i) - { + for (unsigned int i = 0; i < new_num_spheres; ++i) { result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]); } - + result.num_spheres = new_num_spheres; result.obb = obb + other.obb; - + return result; } -FCL_REAL kIOS::width() const -{ - return obb.width(); -} +FCL_REAL kIOS::width() const { return obb.width(); } -FCL_REAL kIOS::height() const -{ - return obb.height(); -} - -FCL_REAL kIOS::depth() const -{ - return obb.depth(); -} +FCL_REAL kIOS::height() const { return obb.height(); } -FCL_REAL kIOS::volume() const -{ - return obb.volume(); -} +FCL_REAL kIOS::depth() const { return obb.depth(); } -FCL_REAL kIOS::size() const -{ - return volume(); -} +FCL_REAL kIOS::volume() const { return obb.volume(); } -FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const -{ +FCL_REAL kIOS::size() const { return volume(); } + +FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { FCL_REAL d_max = 0; long id_a = -1, id_b = -1; - for(unsigned int i = 0; i < num_spheres; ++i) - { - for(unsigned int j = 0; j < other.num_spheres; ++j) - { - FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() - (spheres[i].r + other.spheres[j].r); - if(d_max < d) - { + for (unsigned int i = 0; i < num_spheres; ++i) { + for (unsigned int j = 0; j < other.num_spheres; ++j) { + FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() - + (spheres[i].r + other.spheres[j].r); + if (d_max < d) { d_max = d; - if(P && Q) - { - id_a = (long)i; id_b = (long)j; + if (P && Q) { + id_a = (long)i; + id_b = (long)j; } } } } - if(P && Q) - { - if(id_a != -1 && id_b != -1) - { + if (P && Q) { + if (id_a != -1 && id_b != -1) { const Vec3f v = spheres[id_a].o - spheres[id_b].o; FCL_REAL len_v = v.norm(); *P = spheres[id_a].o - v * (spheres[id_a].r / len_v); @@ -186,64 +149,53 @@ FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const return d_max; } - -bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2) -{ +bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, + const kIOS& b2) { kIOS b2_temp = b2; - for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) - { + for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; } - b2_temp.obb.To = R0 * b2_temp.obb.To + T0; b2_temp.obb.axes.applyOnTheLeft(R0); return b1.overlap(b2_temp); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, - const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) -{ +bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, + const kIOS& b2, const CollisionRequest& request, + FCL_REAL& sqrDistLowerBound) { kIOS b2_temp = b2; - for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) - { + for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; } - b2_temp.obb.To = R0 * b2_temp.obb.To + T0; b2_temp.obb.axes.applyOnTheLeft(R0); return b1.overlap(b2_temp, request, sqrDistLowerBound); } -FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q) -{ +FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, + const kIOS& b2, Vec3f* P, Vec3f* Q) { kIOS b2_temp = b2; - for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) - { + for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; } return b1.distance(b2_temp, P, Q); } - -kIOS translate(const kIOS& bv, const Vec3f& t) -{ +kIOS translate(const kIOS& bv, const Vec3f& t) { kIOS res(bv); - for(size_t i = 0; i < res.num_spheres; ++i) - { + for (size_t i = 0; i < res.num_spheres; ++i) { res.spheres[i].o += t; } - + translate(res.obb, t); return res; } +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp index a924f32fe..7aff1b6b3 100644 --- a/src/BVH/BVH_model.cpp +++ b/src/BVH/BVH_model.cpp @@ -47,91 +47,73 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { BVHModelBase::BVHModelBase() - : vertices(NULL) - , tri_indices(NULL) - , prev_vertices(NULL) - , num_tris(0) - , num_vertices(0) - , build_state(BVH_BUILD_STATE_EMPTY) - , num_tris_allocated(0) - , num_vertices_allocated(0) - , num_vertex_updated(0) -{ -} - -BVHModelBase::BVHModelBase(const BVHModelBase& other) : - CollisionGeometry(other), - num_tris(other.num_tris), - num_vertices(other.num_vertices), - build_state(other.build_state), - num_tris_allocated(other.num_tris), - num_vertices_allocated(other.num_vertices) -{ - if(other.vertices) - { + : vertices(NULL), + tri_indices(NULL), + prev_vertices(NULL), + num_tris(0), + num_vertices(0), + build_state(BVH_BUILD_STATE_EMPTY), + num_tris_allocated(0), + num_vertices_allocated(0), + num_vertex_updated(0) {} + +BVHModelBase::BVHModelBase(const BVHModelBase& other) + : CollisionGeometry(other), + num_tris(other.num_tris), + num_vertices(other.num_vertices), + build_state(other.build_state), + num_tris_allocated(other.num_tris), + num_vertices_allocated(other.num_vertices) { + if (other.vertices) { vertices = new Vec3f[num_vertices]; std::copy(other.vertices, other.vertices + num_vertices, vertices); - } - else + } else vertices = nullptr; - if(other.tri_indices) - { + if (other.tri_indices) { tri_indices = new Triangle[num_tris]; std::copy(other.tri_indices, other.tri_indices + num_tris, tri_indices); - } - else + } else tri_indices = nullptr; - if(other.prev_vertices) - { + if (other.prev_vertices) { prev_vertices = new Vec3f[num_vertices]; - std::copy(other.prev_vertices, other.prev_vertices + num_vertices, prev_vertices); - } - else + std::copy(other.prev_vertices, other.prev_vertices + num_vertices, + prev_vertices); + } else prev_vertices = nullptr; } -bool BVHModelBase::isEqual(const CollisionGeometry & _other) const -{ - const BVHModelBase * other_ptr = dynamic_cast(&_other); - if(other_ptr == nullptr) return false; - const BVHModelBase & other = *other_ptr; +bool BVHModelBase::isEqual(const CollisionGeometry& _other) const { + const BVHModelBase* other_ptr = dynamic_cast(&_other); + if (other_ptr == nullptr) return false; + const BVHModelBase& other = *other_ptr; bool result = - num_tris == other.num_tris - && num_vertices == other.num_vertices; - - if(!result) return false; - - for(size_t k = 0; k < static_cast(num_tris); ++k) - if(tri_indices[k] != other.tri_indices[k]) - return false; - - for(size_t k = 0; k < static_cast(num_vertices); ++k) - if(vertices[k] != other.vertices[k]) - return false; - - if(prev_vertices != nullptr && other.prev_vertices != nullptr) - { - for(size_t k = 0; k < static_cast(num_vertices); ++k) - { - if(prev_vertices[k] != other.prev_vertices[k]) - return false; + num_tris == other.num_tris && num_vertices == other.num_vertices; + + if (!result) return false; + + for (size_t k = 0; k < static_cast(num_tris); ++k) + if (tri_indices[k] != other.tri_indices[k]) return false; + + for (size_t k = 0; k < static_cast(num_vertices); ++k) + if (vertices[k] != other.vertices[k]) return false; + + if (prev_vertices != nullptr && other.prev_vertices != nullptr) { + for (size_t k = 0; k < static_cast(num_vertices); ++k) { + if (prev_vertices[k] != other.prev_vertices[k]) return false; } } - + return true; } -void BVHModelBase::buildConvexRepresentation(bool share_memory) -{ +void BVHModelBase::buildConvexRepresentation(bool share_memory) { if (!convex) { Vec3f* points = vertices; Triangle* polygons = tri_indices; @@ -142,92 +124,91 @@ void BVHModelBase::buildConvexRepresentation(bool share_memory) polygons = new Triangle[num_tris]; std::copy(tri_indices, tri_indices + num_tris, polygons); } - convex.reset(new Convex(!share_memory, points, num_vertices, polygons, num_tris)); + convex.reset(new Convex(!share_memory, points, num_vertices, + polygons, num_tris)); } } -bool BVHModelBase::buildConvexHull(bool keepTriangle, const char* qhullCommand) -{ - convex.reset( - ConvexBase::convexHull(vertices, num_vertices, keepTriangle, qhullCommand) - ); +bool BVHModelBase::buildConvexHull(bool keepTriangle, + const char* qhullCommand) { + convex.reset(ConvexBase::convexHull(vertices, num_vertices, keepTriangle, + qhullCommand)); return num_vertices == convex->num_points; } -template +template BVHModel::BVHModel(const BVHModel& other) -: BVHModelBase(other) -, bv_splitter(other.bv_splitter) -, bv_fitter(other.bv_fitter) -{ - if(other.primitive_indices) - { + : BVHModelBase(other), + bv_splitter(other.bv_splitter), + bv_fitter(other.bv_fitter) { + if (other.primitive_indices) { unsigned int num_primitives = 0; - switch(other.getModelType()) - { + switch (other.getModelType()) { case BVH_MODEL_TRIANGLES: num_primitives = num_tris; break; case BVH_MODEL_POINTCLOUD: num_primitives = num_vertices; break; - default: - ; + default:; } primitive_indices = new unsigned int[num_primitives]; - std::copy(other.primitive_indices, other.primitive_indices + num_primitives, primitive_indices); - } - else + std::copy(other.primitive_indices, other.primitive_indices + num_primitives, + primitive_indices); + } else primitive_indices = nullptr; num_bvs = num_bvs_allocated = other.num_bvs; - if(other.bvs) - { + if (other.bvs) { bvs = new BVNode[num_bvs]; std::copy(other.bvs, other.bvs + num_bvs, bvs); - } - else + } else bvs = nullptr; } - -int BVHModelBase::beginModel(unsigned int num_tris_, unsigned int num_vertices_) -{ - if(build_state != BVH_BUILD_STATE_EMPTY) - { - delete [] vertices; vertices = nullptr; - delete [] tri_indices; tri_indices = nullptr; - delete [] prev_vertices; prev_vertices = nullptr; +int BVHModelBase::beginModel(unsigned int num_tris_, + unsigned int num_vertices_) { + if (build_state != BVH_BUILD_STATE_EMPTY) { + delete[] vertices; + vertices = nullptr; + delete[] tri_indices; + tri_indices = nullptr; + delete[] prev_vertices; + prev_vertices = nullptr; num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = 0; deleteBVs(); } - if(num_tris_ <= 0) num_tris_ = 8; - if(num_vertices_ <= 0) num_vertices_ = 8; + if (num_tris_ <= 0) num_tris_ = 8; + if (num_vertices_ <= 0) num_vertices_ = 8; num_vertices_allocated = num_vertices_; num_tris_allocated = num_tris_; tri_indices = new Triangle[num_tris_allocated]; - if(!tri_indices) - { - std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl; + if (!tri_indices) { + std::cerr << "BVH Error! Out of memory for tri_indices array on " + "BeginModel() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } - + vertices = new Vec3f[num_vertices_allocated]; - if(!vertices) - { - std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl; + if (!vertices) { + std::cerr + << "BVH Error! Out of memory for vertices array on BeginModel() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } - if(build_state != BVH_BUILD_STATE_EMPTY) - { - std::cerr << "BVH Warning! Call beginModel() on a BVHModel that is not empty. This model was cleared and previous triangles/vertices were lost." << std::endl; + if (build_state != BVH_BUILD_STATE_EMPTY) { + std::cerr + << "BVH Warning! Call beginModel() on a BVHModel that is not empty. " + "This model was cleared and previous triangles/vertices were lost." + << std::endl; build_state = BVH_BUILD_STATE_EMPTY; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } @@ -237,25 +218,26 @@ int BVHModelBase::beginModel(unsigned int num_tris_, unsigned int num_vertices_) return BVH_OK; } -int BVHModelBase::addVertex(const Vec3f& p) -{ - if(build_state != BVH_BUILD_STATE_BEGUN) - { - std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; +int BVHModelBase::addVertex(const Vec3f& p) { + if (build_state != BVH_BUILD_STATE_BEGUN) { + std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() " + "was ignored. Must do a beginModel() to clear the model for " + "addition of new vertices." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - if(num_vertices >= num_vertices_allocated) - { + if (num_vertices >= num_vertices_allocated) { Vec3f* temp = new Vec3f[num_vertices_allocated * 2]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl; + if (!temp) { + std::cerr + << "BVH Error! Out of memory for vertices array on addVertex() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(vertices, vertices + num_vertices, temp); - delete [] vertices; + delete[] vertices; vertices = temp; num_vertices_allocated *= 2; } @@ -266,33 +248,33 @@ int BVHModelBase::addVertex(const Vec3f& p) return BVH_OK; } -int BVHModelBase::addTriangles(const Matrixx3i & triangles) -{ - if(build_state == BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; +int BVHModelBase::addTriangles(const Matrixx3i& triangles) { + if (build_state == BVH_BUILD_STATE_PROCESSED) { + std::cerr << "BVH Warning! Call addSubModel() in a wrong order. " + "addSubModel() was ignored. Must do a beginModel() to clear " + "the model for addition of new vertices." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - + const unsigned int num_tris_to_add = (unsigned int)triangles.rows(); - if(num_tris + num_tris_to_add > num_tris_allocated) - { + if (num_tris + num_tris_to_add > num_tris_allocated) { Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl; + if (!temp) { + std::cerr << "BVH Error! Out of memory for tri_indices array on " + "addSubModel() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(tri_indices, tri_indices + num_tris, temp); - delete [] tri_indices; + delete[] tri_indices; tri_indices = temp; num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add; } - for(Eigen::DenseIndex i = 0; i < triangles.rows(); ++i) - { + for (Eigen::DenseIndex i = 0; i < triangles.rows(); ++i) { const Matrixx3i::ConstRowXpr triangle = triangles.row(i); tri_indices[num_tris++].set(static_cast(triangle[0]), static_cast(triangle[1]), @@ -302,54 +284,57 @@ int BVHModelBase::addTriangles(const Matrixx3i & triangles) return BVH_OK; } -int BVHModelBase::addVertices(const Matrixx3f & points) -{ - if(build_state != BVH_BUILD_STATE_BEGUN) - { - std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertices() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; +int BVHModelBase::addVertices(const Matrixx3f& points) { + if (build_state != BVH_BUILD_STATE_BEGUN) { + std::cerr << "BVH Warning! Call addVertex() in a wrong order. " + "addVertices() was ignored. Must do a beginModel() to clear " + "the model for addition of new vertices." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - if(num_vertices + points.rows() > num_vertices_allocated) - { + if (num_vertices + points.rows() > num_vertices_allocated) { num_vertices_allocated = num_vertices + (unsigned int)points.rows(); - Vec3f * temp = new Vec3f[num_vertices_allocated]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl; + Vec3f* temp = new Vec3f[num_vertices_allocated]; + if (!temp) { + std::cerr + << "BVH Error! Out of memory for vertices array on addVertex() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(vertices, vertices + num_vertices, temp); - delete [] vertices; + delete[] vertices; vertices = temp; } - for(Eigen::DenseIndex id = 0; id < points.rows(); ++id) + for (Eigen::DenseIndex id = 0; id < points.rows(); ++id) vertices[num_vertices++] = points.row(id).transpose(); return BVH_OK; } -int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) -{ - if(build_state == BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Warning! Call addTriangle() in a wrong order. addTriangle() was ignored. Must do a beginModel() to clear the model for addition of new triangles." << std::endl; +int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, + const Vec3f& p3) { + if (build_state == BVH_BUILD_STATE_PROCESSED) { + std::cerr << "BVH Warning! Call addTriangle() in a wrong order. " + "addTriangle() was ignored. Must do a beginModel() to clear " + "the model for addition of new triangles." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - if(num_vertices + 2 >= num_vertices_allocated) - { + if (num_vertices + 2 >= num_vertices_allocated) { Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + 2]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl; + if (!temp) { + std::cerr << "BVH Error! Out of memory for vertices array on " + "addTriangle() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(vertices, vertices + num_vertices, temp); - delete [] vertices; + delete[] vertices; vertices = temp; num_vertices_allocated = num_vertices_allocated * 2 + 2; } @@ -363,17 +348,17 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) vertices[num_vertices] = p3; num_vertices++; - if(num_tris >= num_tris_allocated) - { + if (num_tris >= num_tris_allocated) { Triangle* temp = new Triangle[num_tris_allocated * 2]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for tri_indices array on addTriangle() call!" << std::endl; + if (!temp) { + std::cerr << "BVH Error! Out of memory for tri_indices array on " + "addTriangle() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(tri_indices, tri_indices + num_tris, temp); - delete [] tri_indices; + delete[] tri_indices; tri_indices = temp; num_tris_allocated *= 2; } @@ -386,33 +371,35 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) return BVH_OK; } -int BVHModelBase::addSubModel(const std::vector& ps) -{ - if(build_state == BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; +int BVHModelBase::addSubModel(const std::vector& ps) { + if (build_state == BVH_BUILD_STATE_PROCESSED) { + std::cerr << "BVH Warning! Call addSubModel() in a wrong order. " + "addSubModel() was ignored. Must do a beginModel() to clear " + "the model for addition of new vertices." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } const unsigned int num_vertices_to_add = (unsigned int)ps.size(); - if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) - { - Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl; + if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { + Vec3f* temp = + new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1]; + if (!temp) { + std::cerr << "BVH Error! Out of memory for vertices array on " + "addSubModel() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(vertices, vertices + num_vertices, temp); - delete [] vertices; + delete[] vertices; vertices = temp; - num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; + num_vertices_allocated = + num_vertices_allocated * 2 + num_vertices_to_add - 1; } - for(size_t i = 0; i < (size_t)num_vertices_to_add; ++i) - { + for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { vertices[num_vertices] = ps[i]; num_vertices++; } @@ -420,62 +407,62 @@ int BVHModelBase::addSubModel(const std::vector& ps) return BVH_OK; } -int BVHModelBase::addSubModel(const std::vector& ps, const std::vector& ts) -{ - if(build_state == BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; +int BVHModelBase::addSubModel(const std::vector& ps, + const std::vector& ts) { + if (build_state == BVH_BUILD_STATE_PROCESSED) { + std::cerr << "BVH Warning! Call addSubModel() in a wrong order. " + "addSubModel() was ignored. Must do a beginModel() to clear " + "the model for addition of new vertices." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } const unsigned int num_vertices_to_add = (unsigned int)ps.size(); - if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) - { - Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl; + if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { + Vec3f* temp = + new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1]; + if (!temp) { + std::cerr << "BVH Error! Out of memory for vertices array on " + "addSubModel() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(vertices, vertices + num_vertices, temp); - delete [] vertices; + delete[] vertices; vertices = temp; - num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; + num_vertices_allocated = + num_vertices_allocated * 2 + num_vertices_to_add - 1; } const unsigned int offset = num_vertices; - for(size_t i = 0; i < (size_t)num_vertices_to_add; ++i) - { + for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { vertices[num_vertices] = ps[i]; num_vertices++; } - const unsigned int num_tris_to_add = (unsigned int)ts.size(); - if(num_tris + num_tris_to_add - 1 >= num_tris_allocated) - { + if (num_tris + num_tris_to_add - 1 >= num_tris_allocated) { Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl; + if (!temp) { + std::cerr << "BVH Error! Out of memory for tri_indices array on " + "addSubModel() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(tri_indices, tri_indices + num_tris, temp); - delete [] tri_indices; + delete[] tri_indices; tri_indices = temp; num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1; } - for(size_t i = 0; i < (size_t)num_tris_to_add; ++i) - { + for (size_t i = 0; i < (size_t)num_tris_to_add; ++i) { const Triangle& t = ts[i]; - tri_indices[num_tris].set(t[0] + (size_t)offset, - t[1] + (size_t)offset, + tri_indices[num_tris].set(t[0] + (size_t)offset, t[1] + (size_t)offset, t[2] + (size_t)offset); num_tris++; } @@ -483,60 +470,57 @@ int BVHModelBase::addSubModel(const std::vector& ps, const std::vector num_tris) - { - if(num_tris > 0) - { + if (num_tris_allocated > num_tris) { + if (num_tris > 0) { Triangle* new_tris = new Triangle[num_tris]; - if(!new_tris) - { - std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl; + if (!new_tris) { + std::cerr << "BVH Error! Out of memory for tri_indices array in " + "endModel() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(tri_indices, tri_indices + num_tris, new_tris); - delete [] tri_indices; + delete[] tri_indices; tri_indices = new_tris; num_tris_allocated = num_tris; - } - else - { - delete [] tri_indices; + } else { + delete[] tri_indices; tri_indices = NULL; num_tris = num_tris_allocated = 0; } } - if(num_vertices_allocated > num_vertices) - { + if (num_vertices_allocated > num_vertices) { Vec3f* new_vertices = new Vec3f[num_vertices]; - if(!new_vertices) - { - std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl; + if (!new_vertices) { + std::cerr + << "BVH Error! Out of memory for vertices array in endModel() call!" + << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } std::copy(vertices, vertices + num_vertices, new_vertices); - delete [] vertices; + delete[] vertices; vertices = new_vertices; num_vertices_allocated = num_vertices; } // construct BVH tree - if (!allocateBVs ()) - return BVH_ERR_MODEL_OUT_OF_MEMORY; + if (!allocateBVs()) return BVH_ERR_MODEL_OUT_OF_MEMORY; buildTree(); @@ -546,17 +530,15 @@ int BVHModelBase::endModel() return BVH_OK; } - - -int BVHModelBase::beginReplaceModel() -{ - if(build_state != BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl; +int BVHModelBase::beginReplaceModel() { + if (build_state != BVH_BUILD_STATE_PROCESSED) { + std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has " + "no previous frame." + << std::endl; return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; } - if(prev_vertices) delete [] prev_vertices; + if (prev_vertices) delete[] prev_vertices; prev_vertices = NULL; num_vertex_updated = 0; @@ -566,11 +548,12 @@ int BVHModelBase::beginReplaceModel() return BVH_OK; } -int BVHModelBase::replaceVertex(const Vec3f& p) -{ - if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) - { - std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; +int BVHModelBase::replaceVertex(const Vec3f& p) { + if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { + std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. " + "replaceVertex() was ignored. Must do a beginReplaceModel() " + "for initialization." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } @@ -580,55 +563,60 @@ int BVHModelBase::replaceVertex(const Vec3f& p) return BVH_OK; } -int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) -{ - if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) - { - std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; +int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2, + const Vec3f& p3) { + if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { + std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. " + "replaceTriangle() was ignored. Must do a beginReplaceModel() " + "for initialization." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - vertices[num_vertex_updated] = p1; num_vertex_updated++; - vertices[num_vertex_updated] = p2; num_vertex_updated++; - vertices[num_vertex_updated] = p3; num_vertex_updated++; + vertices[num_vertex_updated] = p1; + num_vertex_updated++; + vertices[num_vertex_updated] = p2; + num_vertex_updated++; + vertices[num_vertex_updated] = p3; + num_vertex_updated++; return BVH_OK; } -int BVHModelBase::replaceSubModel(const std::vector& ps) -{ - if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) - { - std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; +int BVHModelBase::replaceSubModel(const std::vector& ps) { + if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { + std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. " + "replaceSubModel() was ignored. Must do a beginReplaceModel() " + "for initialization." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - for(unsigned int i = 0; i < ps.size(); ++i) - { + for (unsigned int i = 0; i < ps.size(); ++i) { vertices[num_vertex_updated] = ps[i]; num_vertex_updated++; } return BVH_OK; } -int BVHModelBase::endReplaceModel(bool refit, bool bottomup) -{ - if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) - { - std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl; +int BVHModelBase::endReplaceModel(bool refit, bool bottomup) { + if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { + std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. " + "endReplaceModel() was ignored. " + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - if(num_vertex_updated != num_vertices) - { - std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl; + if (num_vertex_updated != num_vertices) { + std::cerr << "BVH Error! The replaced model should have the same number of " + "vertices as the old model." + << std::endl; return BVH_ERR_INCORRECT_DATA; } - if(refit) // refit, do not change BVH structure + if (refit) // refit, do not change BVH structure { refitTree(bottomup); - } - else // reconstruct bvh tree based on current frame data + } else // reconstruct bvh tree based on current frame data { buildTree(); } @@ -638,26 +626,20 @@ int BVHModelBase::endReplaceModel(bool refit, bool bottomup) return BVH_OK; } - - - - -int BVHModelBase::beginUpdateModel() -{ - if(build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED) - { - std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no previous frame." << std::endl; +int BVHModelBase::beginUpdateModel() { + if (build_state != BVH_BUILD_STATE_PROCESSED && + build_state != BVH_BUILD_STATE_UPDATED) { + std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no " + "previous frame." + << std::endl; return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; } - if(prev_vertices) - { + if (prev_vertices) { Vec3f* temp = prev_vertices; prev_vertices = vertices; vertices = temp; - } - else - { + } else { prev_vertices = vertices; vertices = new Vec3f[num_vertices]; } @@ -669,11 +651,12 @@ int BVHModelBase::beginUpdateModel() return BVH_OK; } -int BVHModelBase::updateVertex(const Vec3f& p) -{ - if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) - { - std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; +int BVHModelBase::updateVertex(const Vec3f& p) { + if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { + std::cerr + << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() " + "was ignored. Must do a beginUpdateModel() for initialization." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } @@ -683,55 +666,60 @@ int BVHModelBase::updateVertex(const Vec3f& p) return BVH_OK; } -int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) -{ - if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) - { - std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. updateTriangle() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; +int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2, + const Vec3f& p3) { + if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { + std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. " + "updateTriangle() was ignored. Must do a beginUpdateModel() " + "for initialization." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - vertices[num_vertex_updated] = p1; num_vertex_updated++; - vertices[num_vertex_updated] = p2; num_vertex_updated++; - vertices[num_vertex_updated] = p3; num_vertex_updated++; + vertices[num_vertex_updated] = p1; + num_vertex_updated++; + vertices[num_vertex_updated] = p2; + num_vertex_updated++; + vertices[num_vertex_updated] = p3; + num_vertex_updated++; return BVH_OK; } -int BVHModelBase::updateSubModel(const std::vector& ps) -{ - if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) - { - std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. updateSubModel() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; +int BVHModelBase::updateSubModel(const std::vector& ps) { + if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { + std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. " + "updateSubModel() was ignored. Must do a beginUpdateModel() " + "for initialization." + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - for(unsigned int i = 0; i < ps.size(); ++i) - { + for (unsigned int i = 0; i < ps.size(); ++i) { vertices[num_vertex_updated] = ps[i]; num_vertex_updated++; } return BVH_OK; } -int BVHModelBase::endUpdateModel(bool refit, bool bottomup) -{ - if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) - { - std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. endUpdateModel() was ignored. " << std::endl; +int BVHModelBase::endUpdateModel(bool refit, bool bottomup) { + if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { + std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. " + "endUpdateModel() was ignored. " + << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - if(num_vertex_updated != num_vertices) - { - std::cerr << "BVH Error! The updated model should have the same number of vertices as the old model." << std::endl; + if (num_vertex_updated != num_vertices) { + std::cerr << "BVH Error! The updated model should have the same number of " + "vertices as the old model." + << std::endl; return BVH_ERR_INCORRECT_DATA; } - if(refit) // refit, do not change BVH structure + if (refit) // refit, do not change BVH structure { refitTree(bottomup); - } - else // reconstruct bvh tree based on current frame data + } else // reconstruct bvh tree based on current frame data { buildTree(); @@ -740,29 +728,23 @@ int BVHModelBase::endUpdateModel(bool refit, bool bottomup) refitTree(bottomup); } - build_state = BVH_BUILD_STATE_UPDATED; return BVH_OK; } - - -void BVHModelBase::computeLocalAABB() -{ +void BVHModelBase::computeLocalAABB() { AABB aabb_; - for(unsigned int i = 0; i < num_vertices; ++i) - { + for (unsigned int i = 0; i < num_vertices; ++i) { aabb_ += vertices[i]; } aabb_center = aabb_.center(); aabb_radius = 0; - for(unsigned int i = 0; i < num_vertices; ++i) - { + for (unsigned int i = 0; i < num_vertices; ++i) { FCL_REAL r = (aabb_center - vertices[i]).squaredNorm(); - if(r > aabb_radius) aabb_radius = r; + if (r > aabb_radius) aabb_radius = r; } aabb_radius = sqrt(aabb_radius); @@ -770,44 +752,40 @@ void BVHModelBase::computeLocalAABB() aabb_local = aabb_; } - - /// @brief Constructing an empty BVH -template -BVHModel::BVHModel() : - BVHModelBase (), - bv_splitter(new BVSplitter(SPLIT_METHOD_MEAN)), - bv_fitter(new BVFitter()), - num_bvs_allocated(0), - primitive_indices(NULL), - bvs(NULL), - num_bvs(0) -{ -} - -template -void BVHModel::deleteBVs() -{ - delete [] bvs; bvs = NULL; - delete [] primitive_indices; primitive_indices = NULL; +/// @brief Constructing an empty BVH +template +BVHModel::BVHModel() + : BVHModelBase(), + bv_splitter(new BVSplitter(SPLIT_METHOD_MEAN)), + bv_fitter(new BVFitter()), + num_bvs_allocated(0), + primitive_indices(NULL), + bvs(NULL), + num_bvs(0) {} + +template +void BVHModel::deleteBVs() { + delete[] bvs; + bvs = NULL; + delete[] primitive_indices; + primitive_indices = NULL; num_bvs_allocated = num_bvs = 0; } -template -bool BVHModel::allocateBVs() -{ +template +bool BVHModel::allocateBVs() { // construct BVH tree unsigned int num_bvs_to_be_allocated = 0; - if(num_tris == 0) + if (num_tris == 0) num_bvs_to_be_allocated = 2 * num_vertices - 1; else num_bvs_to_be_allocated = 2 * num_tris - 1; - - bvs = new BVNode [num_bvs_to_be_allocated]; - primitive_indices = new unsigned int [num_bvs_to_be_allocated]; - if(!bvs || !primitive_indices) - { - std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl; + bvs = new BVNode[num_bvs_to_be_allocated]; + primitive_indices = new unsigned int[num_bvs_to_be_allocated]; + if (!bvs || !primitive_indices) { + std::cerr << "BVH Error! Out of memory for BV array in endModel()!" + << std::endl; return false; } num_bvs_allocated = num_bvs_to_be_allocated; @@ -815,17 +793,15 @@ bool BVHModel::allocateBVs() return true; } -template -int BVHModel::memUsage(const bool msg) const -{ +template +int BVHModel::memUsage(const bool msg) const { unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs; unsigned int mem_tri_list = (unsigned int)sizeof(Triangle) * num_tris; unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3f) * num_vertices; unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + - (unsigned int)sizeof(BVHModel); - if(msg) - { + (unsigned int)sizeof(BVHModel); + if (msg) { std::cerr << "Total for model " << total_mem << " bytes." << std::endl; std::cerr << "BVs: " << num_bvs << " allocated." << std::endl; std::cerr << "Tris: " << num_tris << " allocated." << std::endl; @@ -835,9 +811,8 @@ int BVHModel::memUsage(const bool msg) const return static_cast(total_mem); } -template -int BVHModel::buildTree() -{ +template +int BVHModel::buildTree() { // set BVFitter bv_fitter->set(vertices, tri_indices, getModelType()); // set SplitRule @@ -846,8 +821,7 @@ int BVHModel::buildTree() num_bvs = 1; unsigned int num_primitives = 0; - switch(getModelType()) - { + switch (getModelType()) { case BVH_MODEL_TRIANGLES: num_primitives = (unsigned int)num_tris; break; @@ -859,8 +833,7 @@ int BVHModel::buildTree() return BVH_ERR_UNSUPPORTED_FUNCTION; } - for(unsigned int i = 0; i < num_primitives; ++i) - primitive_indices[i] = i; + for (unsigned int i = 0; i < num_primitives; ++i) primitive_indices[i] = i; recursiveBuildTree(0, 0, num_primitives); bv_fitter->clear(); @@ -869,9 +842,9 @@ int BVHModel::buildTree() return BVH_OK; } -template -int BVHModel::recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives) -{ +template +int BVHModel::recursiveBuildTree(int bv_id, unsigned int first_primitive, + unsigned int num_primitives) { BVHModelType type = getModelType(); BVNode* bvnode = bvs + bv_id; unsigned int* cur_primitive_indices = primitive_indices + first_primitive; @@ -884,48 +857,39 @@ int BVHModel::recursiveBuildTree(int bv_id, unsigned int first_primitive, un bvnode->first_primitive = first_primitive; bvnode->num_primitives = num_primitives; - if(num_primitives == 1) - { + if (num_primitives == 1) { bvnode->first_child = -((int)(*cur_primitive_indices) + 1); - } - else - { + } else { bvnode->first_child = (int)num_bvs; num_bvs += 2; unsigned int c1 = 0; - for(unsigned int i = 0; i < num_primitives; ++i) - { + for (unsigned int i = 0; i < num_primitives; ++i) { Vec3f p; - if(type == BVH_MODEL_POINTCLOUD) p = vertices[cur_primitive_indices[i]]; - else if(type == BVH_MODEL_TRIANGLES) - { + if (type == BVH_MODEL_POINTCLOUD) + p = vertices[cur_primitive_indices[i]]; + else if (type == BVH_MODEL_TRIANGLES) { const Triangle& t = tri_indices[cur_primitive_indices[i]]; const Vec3f& p1 = vertices[t[0]]; const Vec3f& p2 = vertices[t[1]]; const Vec3f& p3 = vertices[t[2]]; p = (p1 + p2 + p3) / 3.; - } - else - { + } else { std::cerr << "BVH Error: Model type not supported!" << std::endl; return BVH_ERR_UNSUPPORTED_FUNCTION; } - // loop invariant: up to (but not including) index c1 in group 1, // then up to (but not including) index i in group 2 // // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x] // c1 i // - if(bv_splitter->apply(p)) // in the right side + if (bv_splitter->apply(p)) // in the right side { // do nothing - } - else - { + } else { unsigned int temp = cur_primitive_indices[i]; cur_primitive_indices[i] = cur_primitive_indices[c1]; cur_primitive_indices[c1] = temp; @@ -933,90 +897,76 @@ int BVHModel::recursiveBuildTree(int bv_id, unsigned int first_primitive, un } } - - if((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2; + if ((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2; const unsigned int num_first_half = c1; recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half); - recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half); + recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, + num_primitives - num_first_half); } return BVH_OK; } -template -int BVHModel::refitTree(bool bottomup) -{ - if(bottomup) +template +int BVHModel::refitTree(bool bottomup) { + if (bottomup) return refitTree_bottomup(); else return refitTree_topdown(); } -template -int BVHModel::refitTree_bottomup() -{ +template +int BVHModel::refitTree_bottomup() { // TODO the recomputation of the BV is done manually, without using // bv_fitter. The manual BV recomputation seems bugged. Using bv_fitter // seems to correct the bug. - //bv_fitter->set(vertices, tri_indices, getModelType()); + // bv_fitter->set(vertices, tri_indices, getModelType()); int res = recursiveRefitTree_bottomup(0); - //bv_fitter->clear(); + // bv_fitter->clear(); return res; } - -template -int BVHModel::recursiveRefitTree_bottomup(int bv_id) -{ +template +int BVHModel::recursiveRefitTree_bottomup(int bv_id) { BVNode* bvnode = bvs + bv_id; - if(bvnode->isLeaf()) - { + if (bvnode->isLeaf()) { BVHModelType type = getModelType(); int primitive_id = -(bvnode->first_child + 1); - if(type == BVH_MODEL_POINTCLOUD) - { + if (type == BVH_MODEL_POINTCLOUD) { BV bv; - if(prev_vertices) - { + if (prev_vertices) { Vec3f v[2]; v[0] = prev_vertices[primitive_id]; v[1] = vertices[primitive_id]; fit(v, 2, bv); - } - else + } else fit(vertices + primitive_id, 1, bv); bvnode->bv = bv; - } - else if(type == BVH_MODEL_TRIANGLES) - { + } else if (type == BVH_MODEL_TRIANGLES) { BV bv; const Triangle& triangle = tri_indices[primitive_id]; - if(prev_vertices) - { + if (prev_vertices) { Vec3f v[6]; - for(Triangle::index_type i = 0; i < 3; ++i) - { + for (Triangle::index_type i = 0; i < 3; ++i) { v[i] = prev_vertices[triangle[i]]; v[i + 3] = vertices[triangle[i]]; } fit(v, 6, bv); - } - else - { - //TODO use bv_fitter to build BV. See comment in refitTree_bottomup - //unsigned int* cur_primitive_indices = primitive_indices + bvnode->first_primitive; - //bv = bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives); + } else { + // TODO use bv_fitter to build BV. See comment in refitTree_bottomup + // unsigned int* cur_primitive_indices = primitive_indices + + // bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices, + // bvnode->num_primitives); Vec3f v[3]; - for(int i = 0; i < 3; ++i) - { + for (int i = 0; i < 3; ++i) { v[i] = vertices[triangle[(Triangle::index_type)i]]; } @@ -1024,33 +974,29 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) } bvnode->bv = bv; - } - else - { + } else { std::cerr << "BVH Error: Model type not supported!" << std::endl; return BVH_ERR_UNSUPPORTED_FUNCTION; } - } - else - { + } else { recursiveRefitTree_bottomup(bvnode->leftChild()); recursiveRefitTree_bottomup(bvnode->rightChild()); bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv; - //TODO use bv_fitter to build BV. See comment in refitTree_bottomup - //unsigned int* cur_primitive_indices = primitive_indices + bvnode->first_primitive; - //bvnode->bv = bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives); + // TODO use bv_fitter to build BV. See comment in refitTree_bottomup + // unsigned int* cur_primitive_indices = primitive_indices + + // bvnode->first_primitive; bvnode->bv = + // bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives); } return BVH_OK; } -template -int BVHModel::refitTree_topdown() -{ +template +int BVHModel::refitTree_topdown() { bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType()); - for(unsigned int i = 0; i < num_bvs; ++i) - { - BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, bvs[i].num_primitives); + for (unsigned int i = 0; i < num_bvs; ++i) { + BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, + bvs[i].num_primitives); bvs[i].bv = bv; } @@ -1059,13 +1005,11 @@ int BVHModel::refitTree_topdown() return BVH_OK; } - -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c) -{ +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, + const Vec3f& parent_c) { OBB& obb = bvs[bv_id].bv; - if(!bvs[bv_id].isLeaf()) - { + if (!bvs[bv_id].isLeaf()) { makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To); makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To); @@ -1075,16 +1019,15 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, // obb.axes = parent_axes.transpose() * obb.axes; obb.axes.applyOnTheLeft(parent_axes.transpose()); - Vec3f t (obb.To - parent_c); + Vec3f t(obb.To - parent_c); obb.To.noalias() = parent_axes.transpose() * t; } -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c) -{ +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, + const Vec3f& parent_c) { RSS& rss = bvs[bv_id].bv; - if(!bvs[bv_id].isLeaf()) - { + if (!bvs[bv_id].isLeaf()) { makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axes, rss.Tr); makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axes, rss.Tr); @@ -1094,17 +1037,17 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, // rss.axes = parent_axes.transpose() * rss.axes; rss.axes.applyOnTheLeft(parent_axes.transpose()); - Vec3f t (rss.Tr - parent_c); + Vec3f t(rss.Tr - parent_c); rss.Tr.noalias() = parent_axes.transpose() * t; } -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c) -{ +template <> +void BVHModel::makeParentRelativeRecurse(int bv_id, + Matrix3f& parent_axes, + const Vec3f& parent_c) { OBB& obb = bvs[bv_id].bv.obb; RSS& rss = bvs[bv_id].bv.rss; - if(!bvs[bv_id].isLeaf()) - { + if (!bvs[bv_id].isLeaf()) { makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To); makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To); @@ -1114,58 +1057,48 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axe rss.axes.noalias() = parent_axes.transpose() * obb.axes; obb.axes = rss.axes; - Vec3f t (obb.To - parent_c); + Vec3f t(obb.To - parent_c); obb.To.noalias() = parent_axes.transpose() * t; rss.Tr = obb.To; } - - -template<> -NODE_TYPE BVHModel::getNodeType() const -{ +template <> +NODE_TYPE BVHModel::getNodeType() const { return BV_AABB; } -template<> -NODE_TYPE BVHModel::getNodeType() const -{ +template <> +NODE_TYPE BVHModel::getNodeType() const { return BV_OBB; } -template<> -NODE_TYPE BVHModel::getNodeType() const -{ +template <> +NODE_TYPE BVHModel::getNodeType() const { return BV_RSS; } -template<> -NODE_TYPE BVHModel::getNodeType() const -{ +template <> +NODE_TYPE BVHModel::getNodeType() const { return BV_kIOS; } -template<> -NODE_TYPE BVHModel::getNodeType() const -{ +template <> +NODE_TYPE BVHModel::getNodeType() const { return BV_OBBRSS; } -template<> -NODE_TYPE BVHModel >::getNodeType() const -{ +template <> +NODE_TYPE BVHModel >::getNodeType() const { return BV_KDOP16; } -template<> -NODE_TYPE BVHModel >::getNodeType() const -{ +template <> +NODE_TYPE BVHModel >::getNodeType() const { return BV_KDOP18; } -template<> -NODE_TYPE BVHModel >::getNodeType() const -{ +template <> +NODE_TYPE BVHModel >::getNodeType() const { return BV_KDOP24; } @@ -1178,6 +1111,6 @@ template class BVHModel; template class BVHModel; template class BVHModel; -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp index 3c9ac8815..12c3f0009 100644 --- a/src/BVH/BVH_utility.cpp +++ b/src/BVH/BVH_utility.cpp @@ -35,146 +35,143 @@ /** \author Jia Pan */ - #include #include #include -namespace hpp -{ -namespace fcl -{ - -namespace details -{ - template - BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& _aabb) - { - assert(model.getModelType() == BVH_MODEL_TRIANGLES); - const Matrix3f& q = pose.getRotation(); - AABB aabb = translate (_aabb, - pose.getTranslation()); - - Transform3f box_pose; Box box; - constructBox(_aabb, box, box_pose); - box_pose = pose.inverseTimes (box_pose); - - GJKSolver gjk; - - // Check what triangles should be kept. - // TODO use the BV hierarchy - std::vector keep_vertex(model.num_vertices, false); - std::vector keep_tri (model.num_tris, false); - unsigned int ntri = 0; - for (unsigned int i = 0; i < model.num_tris; ++i) { - const Triangle& t = model.tri_indices[i]; - - bool keep_this_tri = keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]]; - - if (!keep_this_tri) { - for (unsigned int j = 0; j < 3; ++j) { - if (aabb.contain(q * model.vertices[t[j]])) { - keep_this_tri = true; - break; - } - } - const Vec3f& p0 = model.vertices[t[0]]; - const Vec3f& p1 = model.vertices[t[1]]; - const Vec3f& p2 = model.vertices[t[2]]; - Vec3f c1, c2, normal; - FCL_REAL distance; - if (!keep_this_tri && gjk.shapeTriangleInteraction - (box, box_pose, p0, p1, p2, Transform3f (), distance, c1, c2, - normal)) { +namespace hpp { +namespace fcl { + +namespace details { +template +BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, + const AABB& _aabb) { + assert(model.getModelType() == BVH_MODEL_TRIANGLES); + const Matrix3f& q = pose.getRotation(); + AABB aabb = translate(_aabb, -pose.getTranslation()); + + Transform3f box_pose; + Box box; + constructBox(_aabb, box, box_pose); + box_pose = pose.inverseTimes(box_pose); + + GJKSolver gjk; + + // Check what triangles should be kept. + // TODO use the BV hierarchy + std::vector keep_vertex(model.num_vertices, false); + std::vector keep_tri(model.num_tris, false); + unsigned int ntri = 0; + for (unsigned int i = 0; i < model.num_tris; ++i) { + const Triangle& t = model.tri_indices[i]; + + bool keep_this_tri = + keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]]; + + if (!keep_this_tri) { + for (unsigned int j = 0; j < 3; ++j) { + if (aabb.contain(q * model.vertices[t[j]])) { keep_this_tri = true; + break; } } - if (keep_this_tri) { - keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true; - keep_tri[i] = true; - ntri++; + const Vec3f& p0 = model.vertices[t[0]]; + const Vec3f& p1 = model.vertices[t[1]]; + const Vec3f& p2 = model.vertices[t[2]]; + Vec3f c1, c2, normal; + FCL_REAL distance; + if (!keep_this_tri && + gjk.shapeTriangleInteraction(box, box_pose, p0, p1, p2, Transform3f(), + distance, c1, c2, normal)) { + keep_this_tri = true; } } - - if (ntri == 0) return NULL; - - BVHModel* new_model (new BVHModel()); - new_model->beginModel(ntri, - std::min(ntri * 3, model.num_vertices)); - std::vector idxConversion (model.num_vertices); - assert(new_model->num_vertices == 0); - for (unsigned int i = 0; i < keep_vertex.size(); ++i) { - if (keep_vertex[i]) { - idxConversion[i] = new_model->num_vertices; - new_model->vertices[new_model->num_vertices] = model.vertices[i]; - new_model->num_vertices++; - } + if (keep_this_tri) { + keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true; + keep_tri[i] = true; + ntri++; } - assert(new_model->num_tris == 0); - for (unsigned int i = 0; i < keep_tri.size(); ++i) { - if (keep_tri[i]) { - new_model->tri_indices[new_model->num_tris].set ( - idxConversion[model.tri_indices[i][0]], - idxConversion[model.tri_indices[i][1]], - idxConversion[model.tri_indices[i][2]] - ); - new_model->num_tris++; - } + } + + if (ntri == 0) return NULL; + + BVHModel* new_model(new BVHModel()); + new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices)); + std::vector idxConversion(model.num_vertices); + assert(new_model->num_vertices == 0); + for (unsigned int i = 0; i < keep_vertex.size(); ++i) { + if (keep_vertex[i]) { + idxConversion[i] = new_model->num_vertices; + new_model->vertices[new_model->num_vertices] = model.vertices[i]; + new_model->num_vertices++; } - if (new_model->endModel() != BVH_OK) { - delete new_model; - return NULL; + } + assert(new_model->num_tris == 0); + for (unsigned int i = 0; i < keep_tri.size(); ++i) { + if (keep_tri[i]) { + new_model->tri_indices[new_model->num_tris].set( + idxConversion[model.tri_indices[i][0]], + idxConversion[model.tri_indices[i][1]], + idxConversion[model.tri_indices[i][2]]); + new_model->num_tris++; } - return new_model; } + if (new_model->endModel() != BVH_OK) { + delete new_model; + return NULL; + } + return new_model; } +} // namespace details -template<> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb) -{ - return details::BVHExtract(model,pose,aabb); +template <> +BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, + const AABB& aabb) { + return details::BVHExtract(model, pose, aabb); } -template<> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb) -{ - return details::BVHExtract(model,pose,aabb); +template <> +BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, + const AABB& aabb) { + return details::BVHExtract(model, pose, aabb); } -template<> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb) -{ - return details::BVHExtract(model,pose,aabb); +template <> +BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, + const AABB& aabb) { + return details::BVHExtract(model, pose, aabb); } -template<> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb) -{ - return details::BVHExtract(model,pose,aabb); +template <> +BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, + const AABB& aabb) { + return details::BVHExtract(model, pose, aabb); } -template<> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb) -{ - return details::BVHExtract(model,pose,aabb); +template <> +BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, const AABB& aabb) { + return details::BVHExtract(model, pose, aabb); } -template<> BVHModel >* BVHExtract(const BVHModel >& model, const Transform3f& pose, const AABB& aabb) -{ - return details::BVHExtract(model,pose,aabb); +template <> +BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3f& pose, const AABB& aabb) { + return details::BVHExtract(model, pose, aabb); } -template<> BVHModel >* BVHExtract(const BVHModel >& model, const Transform3f& pose, const AABB& aabb) -{ - return details::BVHExtract(model,pose,aabb); +template <> +BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3f& pose, const AABB& aabb) { + return details::BVHExtract(model, pose, aabb); } -template<> BVHModel >* BVHExtract(const BVHModel >& model, const Transform3f& pose, const AABB& aabb) -{ - return details::BVHExtract(model,pose,aabb); +template <> +BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3f& pose, const AABB& aabb) { + return details::BVHExtract(model, pose, aabb); } -void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, Matrix3f& M) -{ - Vec3f S1 (Vec3f::Zero()); - Vec3f S2[3] = { Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero() }; +void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, + unsigned int n, Matrix3f& M) { + Vec3f S1(Vec3f::Zero()); + Vec3f S2[3] = {Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero()}; - if(ts) - { - for(unsigned int i = 0; i < n; ++i) - { + if (ts) { + for (unsigned int i = 0; i < n; ++i) { const Triangle& t = (indices) ? ts[indices[i]] : ts[i]; const Vec3f& p1 = ps[t[0]]; @@ -191,8 +188,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, u S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); - if(ps2) - { + if (ps2) { const Vec3f& p1 = ps2[t[0]]; const Vec3f& p2 = ps2[t[1]]; const Vec3f& p3 = ps2[t[2]]; @@ -209,11 +205,8 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, u S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); } } - } - else - { - for(unsigned int i = 0; i < n; ++i) - { + } else { + for (unsigned int i = 0; i < n; ++i) { const Vec3f& p = (indices) ? ps[indices[i]] : ps[i]; S1 += p; S2[0][0] += (p[0] * p[0]); @@ -223,7 +216,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, u S2[0][2] += (p[0] * p[2]); S2[1][2] += (p[1] * p[2]); - if(ps2) // another frame + if (ps2) // another frame { const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i]; S1 += p; @@ -239,41 +232,39 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, u unsigned int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; - M(0, 0) = S2[0][0] - S1[0]*S1[0] / n_points; - M(1, 1) = S2[1][1] - S1[1]*S1[1] / n_points; - M(2, 2) = S2[2][2] - S1[2]*S1[2] / n_points; - M(0, 1) = S2[0][1] - S1[0]*S1[1] / n_points; - M(1, 2) = S2[1][2] - S1[1]*S1[2] / n_points; - M(0, 2) = S2[0][2] - S1[0]*S1[2] / n_points; + M(0, 0) = S2[0][0] - S1[0] * S1[0] / n_points; + M(1, 1) = S2[1][1] - S1[1] * S1[1] / n_points; + M(2, 2) = S2[2][2] - S1[2] * S1[2] / n_points; + M(0, 1) = S2[0][1] - S1[0] * S1[1] / n_points; + M(1, 2) = S2[1][2] - S1[1] * S1[2] / n_points; + M(0, 2) = S2[0][2] - S1[0] * S1[2] / n_points; M(1, 0) = M(0, 1); M(2, 0) = M(0, 2); M(2, 1) = M(1, 2); } - -/** @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin. - * The bounding volume axes are known. +/** @brief Compute the RSS bounding volume parameters: radius, rectangle size + * and the origin. The bounding volume axes are known. */ -void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r) -{ +void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + const Matrix3f& axes, Vec3f& origin, + FCL_REAL l[2], FCL_REAL& r) { bool indirect_index = true; - if(!indices) indirect_index = false; + if (!indices) indirect_index = false; unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; - FCL_REAL (*P)[3] = new FCL_REAL[size_P][3]; + FCL_REAL(*P)[3] = new FCL_REAL[size_P][3]; int P_id = 0; - - if(ts) - { - for(unsigned int i = 0; i < n; ++i) - { + + if (ts) { + for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Triangle& t = ts[index]; - for(Triangle::index_type j = 0; j < 3; ++j) - { + for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; const Vec3f& p = ps[point_id]; Vec3f v(p[0], p[1], p[2]); @@ -283,10 +274,8 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns P_id++; } - if(ps2) - { - for(Triangle::index_type j = 0; j < 3; ++j) - { + if (ps2) { + for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; const Vec3f& p = ps2[point_id]; // FIXME Is this right ????? @@ -298,11 +287,8 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns } } } - } - else - { - for(unsigned int i = 0; i < n; ++i) - { + } else { + for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Vec3f& p = ps[index]; @@ -312,8 +298,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns P[P_id][2] = axes.col(2).dot(v); P_id++; - if(ps2) - { + if (ps2) { const Vec3f& v = ps2[index]; P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); @@ -322,18 +307,19 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns } } } - + FCL_REAL minx, maxx, miny, maxy, minz, maxz; FCL_REAL cz, radsqr; minz = maxz = P[0][2]; - for(unsigned int i = 1; i < size_P; ++i) - { + for (unsigned int i = 1; i < size_P; ++i) { FCL_REAL z_value = P[i][2]; - if(z_value < minz) minz = z_value; - else if(z_value > maxz) maxz = z_value; + if (z_value < minz) + minz = z_value; + else if (z_value > maxz) + maxz = z_value; } r = (FCL_REAL)0.5 * (maxz - minz); @@ -348,16 +334,12 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns FCL_REAL mintmp, maxtmp; mintmp = maxtmp = P[0][0]; - for(unsigned int i = 1; i < size_P; ++i) - { + for (unsigned int i = 1; i < size_P; ++i) { FCL_REAL x_value = P[i][0]; - if(x_value < mintmp) - { + if (x_value < mintmp) { minindex = i; mintmp = x_value; - } - else if(x_value > maxtmp) - { + } else if (x_value > maxtmp) { maxindex = i; maxtmp = x_value; } @@ -369,22 +351,17 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns dz = P[maxindex][2] - cz; maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); - // grow minx/maxx - for(unsigned int i = 0; i < size_P; ++i) - { - if(P[i][0] < minx) - { + for (unsigned int i = 0; i < size_P; ++i) { + if (P[i][0] < minx) { dz = P[i][2] - cz; x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); - if(x < minx) minx = x; - } - else if(P[i][0] > maxx) - { + if (x < minx) minx = x; + } else if (P[i][0] > maxx) { dz = P[i][2] - cz; x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); - if(x > maxx) maxx = x; + if (x > maxx) maxx = x; } } @@ -394,16 +371,12 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns minindex = maxindex = 0; mintmp = maxtmp = P[0][1]; - for(unsigned int i = 1; i < size_P; ++i) - { + for (unsigned int i = 1; i < size_P; ++i) { FCL_REAL y_value = P[i][1]; - if(y_value < mintmp) - { + if (y_value < mintmp) { minindex = i; mintmp = y_value; - } - else if(y_value > maxtmp) - { + } else if (y_value > maxtmp) { maxindex = i; maxtmp = y_value; } @@ -417,91 +390,69 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns // grow miny/maxy - for(unsigned int i = 0; i < size_P; ++i) - { - if(P[i][1] < miny) - { + for (unsigned int i = 0; i < size_P; ++i) { + if (P[i][1] < miny) { dz = P[i][2] - cz; y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); - if(y < miny) miny = y; - } - else if(P[i][1] > maxy) - { + if (y < miny) miny = y; + } else if (P[i][1] > maxy) { dz = P[i][2] - cz; y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); - if(y > maxy) maxy = y; + if (y > maxy) maxy = y; } } - // corners may have some points which are not covered - grow lengths if necessary - // quite conservative (can be improved) + // corners may have some points which are not covered - grow lengths if + // necessary quite conservative (can be improved) FCL_REAL dx, dy, u, t; FCL_REAL a = std::sqrt((FCL_REAL)0.5); - for(unsigned int i = 0; i < size_P; ++i) - { - if(P[i][0] > maxx) - { - if(P[i][1] > maxy) - { + for (unsigned int i = 0; i < size_P; ++i) { + if (P[i][0] > maxx) { + if (P[i][1] > maxy) { dx = P[i][0] - maxx; dy = P[i][1] - maxy; u = dx * a + dy * a; - t = (a*u - dx)*(a*u - dx) + - (a*u - dy)*(a*u - dy) + - (cz - P[i][2])*(cz - P[i][2]); + t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) + + (cz - P[i][2]) * (cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); - if(u > 0) - { - maxx += u*a; - maxy += u*a; + if (u > 0) { + maxx += u * a; + maxy += u * a; } - } - else if(P[i][1] < miny) - { + } else if (P[i][1] < miny) { dx = P[i][0] - maxx; dy = P[i][1] - miny; u = dx * a - dy * a; - t = (a*u - dx)*(a*u - dx) + - (-a*u - dy)*(-a*u - dy) + - (cz - P[i][2])*(cz - P[i][2]); + t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) + + (cz - P[i][2]) * (cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); - if(u > 0) - { - maxx += u*a; - miny -= u*a; + if (u > 0) { + maxx += u * a; + miny -= u * a; } } - } - else if(P[i][0] < minx) - { - if(P[i][1] > maxy) - { + } else if (P[i][0] < minx) { + if (P[i][1] > maxy) { dx = P[i][0] - minx; dy = P[i][1] - maxy; u = dy * a - dx * a; - t = (-a*u - dx)*(-a*u - dx) + - (a*u - dy)*(a*u - dy) + - (cz - P[i][2])*(cz - P[i][2]); + t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) + + (cz - P[i][2]) * (cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); - if(u > 0) - { - minx -= u*a; - maxy += u*a; + if (u > 0) { + minx -= u * a; + maxy += u * a; } - } - else if(P[i][1] < miny) - { + } else if (P[i][1] < miny) { dx = P[i][0] - minx; dy = P[i][1] - miny; u = -dx * a - dy * a; - t = (-a*u - dx)*(-a*u - dx) + - (-a*u - dy)*(-a*u - dy) + - (cz - P[i][2])*(cz - P[i][2]); + t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) + + (cz - P[i][2]) * (cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); - if (u > 0) - { - minx -= u*a; - miny -= u*a; + if (u > 0) { + minx -= u * a; + miny -= u * a; } } } @@ -509,49 +460,45 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns origin.noalias() = axes * Vec3f(minx, miny, cz); - l[0] = std::max(maxx - minx,0); - l[1] = std::max(maxy - miny,0); - - delete [] P; + l[0] = std::max(maxx - minx, 0); + l[1] = std::max(maxy - miny, 0); + delete[] P; } - -/** @brief Compute the bounding volume extent and center for a set or subset of points. - * The bounding volume axes are known. +/** @brief Compute the bounding volume extent and center for a set or subset of + * points. The bounding volume axes are known. */ -static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, unsigned int n, Matrix3f& axes, Vec3f& center, Vec3f& extent) -{ +static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, + unsigned int* indices, + unsigned int n, Matrix3f& axes, + Vec3f& center, Vec3f& extent) { bool indirect_index = true; - if(!indices) indirect_index = false; + if (!indices) indirect_index = false; FCL_REAL real_max = (std::numeric_limits::max)(); - Vec3f min_coord (real_max, real_max, real_max); - Vec3f max_coord (-real_max, -real_max, -real_max); + Vec3f min_coord(real_max, real_max, real_max); + Vec3f max_coord(-real_max, -real_max, -real_max); - for(unsigned int i = 0; i < n; ++i) - { + for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Vec3f& p = ps[index]; - Vec3f proj (axes.transpose() * p); + Vec3f proj(axes.transpose() * p); - for(int j = 0; j < 3; ++j) - { - if(proj[j] > max_coord[j]) max_coord[j] = proj[j]; - if(proj[j] < min_coord[j]) min_coord[j] = proj[j]; + for (int j = 0; j < 3; ++j) { + if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; + if (proj[j] < min_coord[j]) min_coord[j] = proj[j]; } - if(ps2) - { + if (ps2) { const Vec3f& v = ps2[index]; proj.noalias() = axes.transpose() * v; - for(int j = 0; j < 3; ++j) - { - if(proj[j] > max_coord[j]) max_coord[j] = proj[j]; - if(proj[j] < min_coord[j]) min_coord[j] = proj[j]; + for (int j = 0; j < 3; ++j) { + if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; + if (proj[j] < min_coord[j]) min_coord[j] = proj[j]; } } } @@ -561,50 +508,45 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned extent.noalias() = (max_coord - min_coord) / 2; } - -/** @brief Compute the bounding volume extent and center for a set or subset of points. - * The bounding volume axes are known. +/** @brief Compute the bounding volume extent and center for a set or subset of + * points. The bounding volume axes are known. */ -static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, Matrix3f& axes, Vec3f& center, Vec3f& extent) -{ +static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, + unsigned int n, Matrix3f& axes, + Vec3f& center, Vec3f& extent) { bool indirect_index = true; - if(!indices) indirect_index = false; + if (!indices) indirect_index = false; FCL_REAL real_max = (std::numeric_limits::max)(); - Vec3f min_coord (real_max, real_max, real_max); - Vec3f max_coord (-real_max, -real_max, -real_max); + Vec3f min_coord(real_max, real_max, real_max); + Vec3f max_coord(-real_max, -real_max, -real_max); - for(unsigned int i = 0; i < n; ++i) - { - unsigned int index = indirect_index? indices[i] : i; + for (unsigned int i = 0; i < n; ++i) { + unsigned int index = indirect_index ? indices[i] : i; const Triangle& t = ts[index]; - for(Triangle::index_type j = 0; j < 3; ++j) - { + for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; const Vec3f& p = ps[point_id]; Vec3f proj(axes.transpose() * p); - for(int k = 0; k < 3; ++k) - { - if(proj[k] > max_coord[k]) max_coord[k] = proj[k]; - if(proj[k] < min_coord[k]) min_coord[k] = proj[k]; + for (int k = 0; k < 3; ++k) { + if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; + if (proj[k] < min_coord[k]) min_coord[k] = proj[k]; } } - if(ps2) - { - for(Triangle::index_type j = 0; j < 3; ++j) - { + if (ps2) { + for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; const Vec3f& p = ps2[point_id]; Vec3f proj(axes.transpose() * p); - for(int k = 0; k < 3; ++k) - { - if(proj[k] > max_coord[k]) max_coord[k] = proj[k]; - if(proj[k] < min_coord[k]) min_coord[k] = proj[k]; + for (int k = 0; k < 3; ++k) { + if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; + if (proj[k] < min_coord[k]) min_coord[k] = proj[k]; } } } @@ -615,19 +557,19 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, center.noalias() = axes * o; extent.noalias() = (max_coord - min_coord) / 2; - } -void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, Matrix3f& axes, Vec3f& center, Vec3f& extent) -{ - if(ts) +void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, Matrix3f& axes, + Vec3f& center, Vec3f& extent) { + if (ts) getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent); else getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent); } -void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius) -{ +void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, + Vec3f& center, FCL_REAL& radius) { Vec3f e1 = a - c; Vec3f e2 = b - c; FCL_REAL e1_len2 = e1.squaredNorm(); @@ -640,36 +582,33 @@ void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; } - -static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, const Vec3f& query) -{ +static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, + unsigned int n, + const Vec3f& query) { bool indirect_index = true; - if(!indices) indirect_index = false; - + if (!indices) indirect_index = false; + FCL_REAL maxD = 0; - for(unsigned int i = 0; i < n; ++i) - { + for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Triangle& t = ts[index]; - for(Triangle::index_type j = 0; j < 3; ++j) - { + for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; const Vec3f& p = ps[point_id]; - + FCL_REAL d = (p - query).squaredNorm(); - if(d > maxD) maxD = d; + if (d > maxD) maxD = d; } - if(ps2) - { - for(Triangle::index_type j = 0; j < 3; ++j) - { + if (ps2) { + for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; const Vec3f& p = ps2[point_id]; - + FCL_REAL d = (p - query).squaredNorm(); - if(d > maxD) maxD = d; + if (d > maxD) maxD = d; } } } @@ -677,40 +616,40 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, return std::sqrt(maxD); } -static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, unsigned int n, const Vec3f& query) -{ +static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, + unsigned int* indices, + unsigned int n, + const Vec3f& query) { bool indirect_index = true; - if(!indices) indirect_index = false; + if (!indices) indirect_index = false; FCL_REAL maxD = 0; - for(unsigned int i = 0; i < n; ++i) - { + for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Vec3f& p = ps[index]; FCL_REAL d = (p - query).squaredNorm(); - if(d > maxD) maxD = d; + if (d > maxD) maxD = d; - if(ps2) - { + if (ps2) { const Vec3f& v = ps2[index]; FCL_REAL d = (v - query).squaredNorm(); - if(d > maxD) maxD = d; + if (d > maxD) maxD = d; } } return std::sqrt(maxD); } -FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, const Vec3f& query) -{ - if(ts) +FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + const Vec3f& query) { + if (ts) return maximumDistance_mesh(ps, ps2, ts, indices, n, query); else return maximumDistance_pointcloud(ps, ps2, indices, n, query); } +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp index b501ad395..730447a38 100644 --- a/src/BVH/BV_fitter.cpp +++ b/src/BVH/BV_fitter.cpp @@ -40,44 +40,50 @@ #include #include -namespace hpp -{ -namespace fcl -{ - +namespace hpp { +namespace fcl { static const double kIOS_RATIO = 1.5; static const double invSinA = 2; static const double cosA = sqrt(3.0) / 2.0; -static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], Matrix3f& axes) -{ +static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], + Matrix3f& axes) { int min, mid, max; - if(eigenS[0] > eigenS[1]) { max = 0; min = 1; } - else { min = 0; max = 1; } - if(eigenS[2] < eigenS[min]) { mid = min; min = 2; } - else if(eigenS[2] > eigenS[max]) { mid = max; max = 2; } - else { mid = 2; } + if (eigenS[0] > eigenS[1]) { + max = 0; + min = 1; + } else { + min = 0; + max = 1; + } + if (eigenS[2] < eigenS[min]) { + mid = min; + min = 2; + } else if (eigenS[2] > eigenS[max]) { + mid = max; + max = 2; + } else { + mid = 2; + } axes.col(0) << eigenV[0][max], eigenV[1][max], eigenV[2][max]; axes.col(1) << eigenV[0][mid], eigenV[1][mid], eigenV[2][mid]; - axes.col(2) << eigenV[1][max]*eigenV[2][mid] - eigenV[1][mid]*eigenV[2][max], - eigenV[0][mid]*eigenV[2][max] - eigenV[0][max]*eigenV[2][mid], - eigenV[0][max]*eigenV[1][mid] - eigenV[0][mid]*eigenV[1][max]; + axes.col(2) << eigenV[1][max] * eigenV[2][mid] - + eigenV[1][mid] * eigenV[2][max], + eigenV[0][mid] * eigenV[2][max] - eigenV[0][max] * eigenV[2][mid], + eigenV[0][max] * eigenV[1][mid] - eigenV[0][mid] * eigenV[1][max]; } -namespace OBB_fit_functions -{ +namespace OBB_fit_functions { -void fit1(Vec3f* ps, OBB& bv) -{ +void fit1(Vec3f* ps, OBB& bv) { bv.To.noalias() = ps[0]; bv.axes.setIdentity(); bv.extent.setZero(); } -void fit2(Vec3f* ps, OBB& bv) -{ +void fit2(Vec3f* ps, OBB& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; Vec3f p1p2 = p1 - p2; @@ -91,8 +97,7 @@ void fit2(Vec3f* ps, OBB& bv) bv.To.noalias() = (p1 + p2) / 2; } -void fit3(Vec3f* ps, OBB& bv) -{ +void fit3(Vec3f* ps, OBB& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; const Vec3f& p3 = ps[2]; @@ -106,8 +111,8 @@ void fit3(Vec3f* ps, OBB& bv) len[2] = e[2].squaredNorm(); int imax = 0; - if(len[1] > len[0]) imax = 1; - if(len[2] > len[imax]) imax = 2; + if (len[1] > len[0]) imax = 1; + if (len[2] > len[imax]) imax = 2; bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); bv.axes.col(0).noalias() = e[imax].normalized(); @@ -116,20 +121,17 @@ void fit3(Vec3f* ps, OBB& bv) getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent); } -void fit6(Vec3f* ps, OBB& bv) -{ +void fit6(Vec3f* ps, OBB& bv) { OBB bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } - -void fitn(Vec3f* ps, unsigned int n, OBB& bv) -{ +void fitn(Vec3f* ps, unsigned int n, OBB& bv) { Matrix3f M; Vec3f E[3]; - Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values + Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -139,13 +141,10 @@ void fitn(Vec3f* ps, unsigned int n, OBB& bv) getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axes, bv.To, bv.extent); } -} - +} // namespace OBB_fit_functions -namespace RSS_fit_functions -{ -void fit1(Vec3f* ps, RSS& bv) -{ +namespace RSS_fit_functions { +void fit1(Vec3f* ps, RSS& bv) { bv.Tr.noalias() = ps[0]; bv.axes.setIdentity(); bv.length[0] = 0; @@ -153,8 +152,7 @@ void fit1(Vec3f* ps, RSS& bv) bv.radius = 0; } -void fit2(Vec3f* ps, RSS& bv) -{ +void fit2(Vec3f* ps, RSS& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; bv.axes.col(0).noalias() = p1 - p2; @@ -169,8 +167,7 @@ void fit2(Vec3f* ps, RSS& bv) bv.radius = 0; } -void fit3(Vec3f* ps, RSS& bv) -{ +void fit3(Vec3f* ps, RSS& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; const Vec3f& p3 = ps[2]; @@ -184,28 +181,27 @@ void fit3(Vec3f* ps, RSS& bv) len[2] = e[2].squaredNorm(); int imax = 0; - if(len[1] > len[0]) imax = 1; - if(len[2] > len[imax]) imax = 2; + if (len[1] > len[0]) imax = 1; + if (len[2] > len[imax]) imax = 2; bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); bv.axes.col(0).noalias() = e[imax].normalized(); bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0)); - getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.length, bv.radius); + getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, + bv.length, bv.radius); } -void fit6(Vec3f* ps, RSS& bv) -{ +void fit6(Vec3f* ps, RSS& bv) { RSS bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } -void fitn(Vec3f* ps, unsigned int n, RSS& bv) -{ - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors +void fitn(Vec3f* ps, unsigned int n, RSS& bv) { + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors Matrix3f::Scalar s[3] = {0, 0, 0}; getCovariance(ps, NULL, NULL, NULL, n, M); @@ -213,16 +209,15 @@ void fitn(Vec3f* ps, unsigned int n, RSS& bv) axisFromEigen(E, s, bv.axes); // set rss origin, rectangle size and radius - getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.length, bv.radius); + getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, + bv.length, bv.radius); } -} +} // namespace RSS_fit_functions -namespace kIOS_fit_functions -{ +namespace kIOS_fit_functions { -void fit1(Vec3f* ps, kIOS& bv) -{ +void fit1(Vec3f* ps, kIOS& bv) { bv.num_spheres = 1; bv.spheres[0].o.noalias() = ps[0]; bv.spheres[0].r = 0; @@ -232,8 +227,7 @@ void fit1(Vec3f* ps, kIOS& bv) bv.obb.To.noalias() = ps[0]; } -void fit2(Vec3f* ps, kIOS& bv) -{ +void fit2(Vec3f* ps, kIOS& bv) { bv.num_spheres = 5; const Vec3f& p1 = ps[0]; @@ -241,11 +235,11 @@ void fit2(Vec3f* ps, kIOS& bv) Vec3f p1p2 = p1 - p2; FCL_REAL len_p1p2 = p1p2.norm(); p1p2.normalize(); - + Matrix3f& axes = bv.obb.axes; axes.col(0).noalias() = p1p2; generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2)); - + FCL_REAL r0 = len_p1p2 * 0.5; bv.obb.extent << r0, 0, 0; bv.obb.To = (p1 + p2) * 0.5; @@ -268,10 +262,9 @@ void fit2(Vec3f* ps, kIOS& bv) bv.spheres[4].o = bv.spheres[0].o + delta; } -void fit3(Vec3f* ps, kIOS& bv) -{ +void fit3(Vec3f* ps, kIOS& bv) { bv.num_spheres = 3; - + const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; const Vec3f& p3 = ps[2]; @@ -283,16 +276,17 @@ void fit3(Vec3f* ps, kIOS& bv) len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); - + int imax = 0; - if(len[1] > len[0]) imax = 1; - if(len[2] > len[imax]) imax = 2; - + if (len[1] > len[0]) imax = 1; + if (len[2] > len[imax]) imax = 2; + bv.obb.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); bv.obb.axes.col(0).noalias() = e[imax].normalized(); bv.obb.axes.col(1).noalias() = bv.obb.axes.col(2).cross(bv.obb.axes.col(0)); - getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axes, bv.obb.To, bv.obb.extent); + getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axes, bv.obb.To, + bv.obb.extent); // compute radius and center FCL_REAL r0; @@ -304,22 +298,21 @@ void fit3(Vec3f* ps, kIOS& bv) FCL_REAL r1 = r0 * invSinA; Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA); - + bv.spheres[1].r = r1; bv.spheres[1].o = center - delta; bv.spheres[2].r = r1; bv.spheres[2].o = center + delta; } -void fitn(Vec3f* ps, unsigned int n, kIOS& bv) -{ +void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { Matrix3f M; Vec3f E[3]; - Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values; + Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); - + Matrix3f& axes = bv.obb.axes; axisFromEigen(E, s, axes); @@ -329,26 +322,25 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) const Vec3f& center = bv.obb.To; const Vec3f& extent = bv.obb.extent; FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); - + // decide the k in kIOS - if(extent[0] > kIOS_RATIO * extent[2]) - { - if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5; - else bv.num_spheres = 3; - } - else bv.num_spheres = 1; + if (extent[0] > kIOS_RATIO * extent[2]) { + if (extent[0] > kIOS_RATIO * extent[1]) + bv.num_spheres = 5; + else + bv.num_spheres = 3; + } else + bv.num_spheres = 1; - bv.spheres[0].o = center; bv.spheres[0].r = r0; - if(bv.num_spheres >= 3) - { + if (bv.num_spheres >= 3) { FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; - + FCL_REAL r11 = 0, r12 = 0; r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o); r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o); @@ -359,185 +351,176 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) bv.spheres[2].r = r10; } - if(bv.num_spheres >= 5) - { + if (bv.num_spheres >= 5) { FCL_REAL r10 = bv.spheres[1].r; - Vec3f delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); + Vec3f delta = + axes.col(1) * + (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - + extent[1]); bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; - + FCL_REAL r21 = 0, r22 = 0; r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o); r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o); bv.spheres[3].o += axes.col(1) * (-r10 + r21); bv.spheres[4].o += axes.col(1) * (r10 - r22); - + bv.spheres[3].r = r10; bv.spheres[4].r = r10; } } -} +} // namespace kIOS_fit_functions -namespace OBBRSS_fit_functions -{ -void fit1(Vec3f* ps, OBBRSS& bv) -{ +namespace OBBRSS_fit_functions { +void fit1(Vec3f* ps, OBBRSS& bv) { OBB_fit_functions::fit1(ps, bv.obb); RSS_fit_functions::fit1(ps, bv.rss); } -void fit2(Vec3f* ps, OBBRSS& bv) -{ +void fit2(Vec3f* ps, OBBRSS& bv) { OBB_fit_functions::fit2(ps, bv.obb); RSS_fit_functions::fit2(ps, bv.rss); } -void fit3(Vec3f* ps, OBBRSS& bv) -{ +void fit3(Vec3f* ps, OBBRSS& bv) { OBB_fit_functions::fit3(ps, bv.obb); RSS_fit_functions::fit3(ps, bv.rss); } -void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) -{ +void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) { OBB_fit_functions::fitn(ps, n, bv.obb); RSS_fit_functions::fitn(ps, n, bv.rss); } -} - - - -template<> -void fit(Vec3f* ps, unsigned int n, OBB& bv) -{ - switch(n) - { - case 1: - OBB_fit_functions::fit1(ps, bv); - break; - case 2: - OBB_fit_functions::fit2(ps, bv); - break; - case 3: - OBB_fit_functions::fit3(ps, bv); - break; - case 6: - OBB_fit_functions::fit6(ps, bv); - break; - default: - OBB_fit_functions::fitn(ps, n, bv); +} // namespace OBBRSS_fit_functions + +template <> +void fit(Vec3f* ps, unsigned int n, OBB& bv) { + switch (n) { + case 1: + OBB_fit_functions::fit1(ps, bv); + break; + case 2: + OBB_fit_functions::fit2(ps, bv); + break; + case 3: + OBB_fit_functions::fit3(ps, bv); + break; + case 6: + OBB_fit_functions::fit6(ps, bv); + break; + default: + OBB_fit_functions::fitn(ps, n, bv); } } -template<> -void fit(Vec3f* ps, unsigned int n, RSS& bv) -{ - switch(n) - { - case 1: - RSS_fit_functions::fit1(ps, bv); - break; - case 2: - RSS_fit_functions::fit2(ps, bv); - break; - case 3: - RSS_fit_functions::fit3(ps, bv); - break; - default: - RSS_fit_functions::fitn(ps, n, bv); +template <> +void fit(Vec3f* ps, unsigned int n, RSS& bv) { + switch (n) { + case 1: + RSS_fit_functions::fit1(ps, bv); + break; + case 2: + RSS_fit_functions::fit2(ps, bv); + break; + case 3: + RSS_fit_functions::fit3(ps, bv); + break; + default: + RSS_fit_functions::fitn(ps, n, bv); } } -template<> -void fit(Vec3f* ps, unsigned int n, kIOS& bv) -{ - switch(n) - { - case 1: - kIOS_fit_functions::fit1(ps, bv); - break; - case 2: - kIOS_fit_functions::fit2(ps, bv); - break; - case 3: - kIOS_fit_functions::fit3(ps, bv); - break; - default: - kIOS_fit_functions::fitn(ps, n, bv); +template <> +void fit(Vec3f* ps, unsigned int n, kIOS& bv) { + switch (n) { + case 1: + kIOS_fit_functions::fit1(ps, bv); + break; + case 2: + kIOS_fit_functions::fit2(ps, bv); + break; + case 3: + kIOS_fit_functions::fit3(ps, bv); + break; + default: + kIOS_fit_functions::fitn(ps, n, bv); } } -template<> -void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) -{ - switch(n) - { - case 1: - OBBRSS_fit_functions::fit1(ps, bv); - break; - case 2: - OBBRSS_fit_functions::fit2(ps, bv); - break; - case 3: - OBBRSS_fit_functions::fit3(ps, bv); - break; - default: - OBBRSS_fit_functions::fitn(ps, n, bv); +template <> +void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) { + switch (n) { + case 1: + OBBRSS_fit_functions::fit1(ps, bv); + break; + case 2: + OBBRSS_fit_functions::fit2(ps, bv); + break; + case 3: + OBBRSS_fit_functions::fit3(ps, bv); + break; + default: + OBBRSS_fit_functions::fitn(ps, n, bv); } } -template<> -void fit(Vec3f* ps, unsigned int n, AABB& bv) -{ +template <> +void fit(Vec3f* ps, unsigned int n, AABB& bv) { if (n <= 0) return; - bv = AABB (ps[0]); - for(unsigned int i = 1; i < n; ++i) - { + bv = AABB(ps[0]); + for (unsigned int i = 1; i < n; ++i) { bv += ps[i]; } } -OBB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) -{ +OBB BVFitter::fit(unsigned int* primitive_indices, + unsigned int num_primitives) { OBB bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3]; // three eigen values + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors + Matrix3f::Scalar s[3]; // three eigen values - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); + getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.axes); // set obb centers and extensions - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, bv.To, bv.extent); + getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, bv.axes, bv.To, bv.extent); return bv; } -OBBRSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) -{ +OBBRSS BVFitter::fit(unsigned int* primitive_indices, + unsigned int num_primitives) { OBBRSS bv; Matrix3f M; Vec3f E[3]; Matrix3f::Scalar s[3]; - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); + getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.obb.axes); bv.rss.axes.noalias() = bv.obb.axes; - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent); + getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent); Vec3f origin; FCL_REAL l[2]; FCL_REAL r; - getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axes, origin, l, r); + getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, + primitive_indices, num_primitives, + bv.rss.axes, origin, l, r); bv.rss.Tr = origin; bv.rss.length[0] = l[0]; @@ -547,14 +530,15 @@ OBBRSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_p return bv; } -RSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) -{ +RSS BVFitter::fit(unsigned int* primitive_indices, + unsigned int num_primitives) { RSS bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3]; // three eigen values - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors + Matrix3f::Scalar s[3]; // three eigen values + getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.axes); @@ -563,58 +547,66 @@ RSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primiti Vec3f origin; FCL_REAL l[2]; FCL_REAL r; - getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, origin, l, r); + getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, + primitive_indices, num_primitives, bv.axes, + origin, l, r); bv.Tr = origin; bv.length[0] = l[0]; bv.length[1] = l[1]; bv.radius = r; - return bv; } -kIOS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) -{ +kIOS BVFitter::fit(unsigned int* primitive_indices, + unsigned int num_primitives) { kIOS bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors Matrix3f::Scalar s[3]; - - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); + + getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, M); eigen(M, s, E); Matrix3f& axes = bv.obb.axes; axisFromEigen(E, s, axes); // get centers and extensions - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axes, bv.obb.To, bv.obb.extent); + getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, axes, bv.obb.To, bv.obb.extent); const Vec3f& center = bv.obb.To; const Vec3f& extent = bv.obb.extent; - FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center); + FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, + primitive_indices, num_primitives, center); // decide k in kIOS - if(extent[0] > kIOS_RATIO * extent[2]) - { - if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5; - else bv.num_spheres = 3; - } - else bv.num_spheres = 1; + if (extent[0] > kIOS_RATIO * extent[2]) { + if (extent[0] > kIOS_RATIO * extent[1]) + bv.num_spheres = 5; + else + bv.num_spheres = 3; + } else + bv.num_spheres = 1; bv.spheres[0].o = center; bv.spheres[0].r = r0; - if(bv.num_spheres >= 3) - { + if (bv.num_spheres >= 3) { FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; - FCL_REAL r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o); - FCL_REAL r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o); + FCL_REAL r11 = + maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, bv.spheres[1].o); + FCL_REAL r12 = + maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, + num_primitives, bv.spheres[2].o); bv.spheres[1].o += axes.col(2) * (-r10 + r11); bv.spheres[2].o += axes.col(2) * (r10 - r12); @@ -623,20 +615,24 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primi bv.spheres[2].r = r10; } - if(bv.num_spheres >= 5) - { + if (bv.num_spheres >= 5) { FCL_REAL r10 = bv.spheres[1].r; - Vec3f delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); + Vec3f delta = + axes.col(1) * + (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - + extent[1]); bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; - + FCL_REAL r21 = 0, r22 = 0; - r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o); - r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o); + r21 = maximumDistance(vertices, prev_vertices, tri_indices, + primitive_indices, num_primitives, bv.spheres[3].o); + r22 = maximumDistance(vertices, prev_vertices, tri_indices, + primitive_indices, num_primitives, bv.spheres[4].o); bv.spheres[3].o += axes.col(1) * (-r10 + r21); bv.spheres[4].o += axes.col(1) * (r10 - r22); - + bv.spheres[3].r = r10; bv.spheres[4].r = r10; } @@ -644,24 +640,23 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primi return bv; } -AABB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) -{ +AABB BVFitter::fit(unsigned int* primitive_indices, + unsigned int num_primitives) { AABB bv; if (num_primitives == 0) return bv; - if(type == BVH_MODEL_TRIANGLES) /// The primitive is triangle + if (type == BVH_MODEL_TRIANGLES) /// The primitive is triangle { Triangle t0 = tri_indices[primitive_indices[0]]; - bv = AABB (vertices[t0[0]]); + bv = AABB(vertices[t0[0]]); - for(unsigned int i = 0; i < num_primitives; ++i) - { + for (unsigned int i = 0; i < num_primitives; ++i) { Triangle t = tri_indices[primitive_indices[i]]; bv += vertices[t[0]]; bv += vertices[t[1]]; bv += vertices[t[2]]; - if(prev_vertices) /// can fitting both current and previous frame + if (prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[t[0]]; bv += prev_vertices[t[1]]; @@ -669,15 +664,13 @@ AABB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primi } } return bv; - } - else if(type == BVH_MODEL_POINTCLOUD) /// The primitive is point + } else if (type == BVH_MODEL_POINTCLOUD) /// The primitive is point { - bv = AABB (vertices[primitive_indices[0]]); - for(unsigned int i = 0; i < num_primitives; ++i) - { + bv = AABB(vertices[primitive_indices[0]]); + for (unsigned int i = 0; i < num_primitives; ++i) { bv += vertices[primitive_indices[i]]; - if(prev_vertices) /// can fitting both current and previous frame + if (prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[primitive_indices[i]]; } @@ -686,6 +679,6 @@ AABB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primi return bv; } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp index 334005080..64dc26733 100644 --- a/src/BVH/BV_splitter.cpp +++ b/src/BVH/BV_splitter.cpp @@ -37,21 +37,16 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { - -template -void computeSplitVector(const BV& bv, Vec3f& split_vector) -{ +template +void computeSplitVector(const BV& bv, Vec3f& split_vector) { split_vector = bv.axes.col(0); } -template<> -void computeSplitVector(const kIOS& bv, Vec3f& split_vector) -{ +template <> +void computeSplitVector(const kIOS& bv, Vec3f& split_vector) { /* switch(bv.num_spheres) { @@ -83,28 +78,26 @@ void computeSplitVector(const kIOS& bv, Vec3f& split_vector) split_vector = bv.obb.axes.col(0); } -template<> -void computeSplitVector(const OBBRSS& bv, Vec3f& split_vector) -{ +template <> +void computeSplitVector(const OBBRSS& bv, Vec3f& split_vector) { split_vector = bv.obb.axes.col(0); } -template -void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value) -{ +template +void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value) { Vec3f center = bv.center(); split_value = center[0]; } -template -void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, const Vec3f& split_vector, FCL_REAL& split_value) -{ - if(type == BVH_MODEL_TRIANGLES) - { - Vec3f c (Vec3f::Zero()); +template +void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, + unsigned int* primitive_indices, + unsigned int num_primitives, BVHModelType type, + const Vec3f& split_vector, FCL_REAL& split_value) { + if (type == BVH_MODEL_TRIANGLES) { + Vec3f c(Vec3f::Zero()); - for(unsigned int i = 0; i < num_primitives; ++i) - { + for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle& t = triangles[primitive_indices[i]]; const Vec3f& p1 = vertices[t[0]]; const Vec3f& p2 = vertices[t[1]]; @@ -113,12 +106,9 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, uns c += p1 + p2 + p3; } split_value = c.dot(split_vector) / (3 * num_primitives); - } - else if(type == BVH_MODEL_POINTCLOUD) - { + } else if (type == BVH_MODEL_POINTCLOUD) { FCL_REAL sum = 0; - for(unsigned int i = 0; i < num_primitives; ++i) - { + for (unsigned int i = 0; i < num_primitives; ++i) { const Vec3f& p = vertices[primitive_indices[i]]; sum += p.dot(split_vector); } @@ -127,30 +117,27 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, uns } } -template -void computeSplitValue_median(const BV&, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, const Vec3f& split_vector, FCL_REAL& split_value) -{ +template +void computeSplitValue_median(const BV&, Vec3f* vertices, Triangle* triangles, + unsigned int* primitive_indices, + unsigned int num_primitives, BVHModelType type, + const Vec3f& split_vector, + FCL_REAL& split_value) { std::vector proj(num_primitives); - if(type == BVH_MODEL_TRIANGLES) - { - for(unsigned int i = 0; i < num_primitives; ++i) - { + if (type == BVH_MODEL_TRIANGLES) { + for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle& t = triangles[primitive_indices[i]]; const Vec3f& p1 = vertices[t[0]]; const Vec3f& p2 = vertices[t[1]]; const Vec3f& p3 = vertices[t[2]]; - Vec3f centroid3(p1[0] + p2[0] + p3[0], - p1[1] + p2[1] + p3[1], + Vec3f centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1], p1[2] + p2[2] + p3[2]); proj[i] = centroid3.dot(split_vector) / 3; } - } - else if(type == BVH_MODEL_POINTCLOUD) - { - for(unsigned int i = 0; i < num_primitives; ++i) - { + } else if (type == BVH_MODEL_POINTCLOUD) { + for (unsigned int i = 0; i < num_primitives; ++i) { const Vec3f& p = vertices[primitive_indices[i]]; Vec3f v(p[0], p[1], p[2]); proj[i] = v.dot(split_vector); @@ -159,132 +146,143 @@ void computeSplitValue_median(const BV&, Vec3f* vertices, Triangle* triangles, u std::sort(proj.begin(), proj.end()); - if(num_primitives % 2 == 1) - { + if (num_primitives % 2 == 1) { split_value = proj[(num_primitives - 1) / 2]; - } - else - { + } else { split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; - } + } } -template<> -void BVSplitter::computeRule_bvcenter(const OBB& bv, unsigned int*, unsigned int) -{ +template <> +void BVSplitter::computeRule_bvcenter(const OBB& bv, unsigned int*, + unsigned int) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } -template<> -void BVSplitter::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives) -{ +template <> +void BVSplitter::computeRule_mean(const OBB& bv, + unsigned int* primitive_indices, + unsigned int num_primitives) { computeSplitVector(bv, split_vector); - computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); + computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, + num_primitives, type, split_vector, split_value); } -template<> -void BVSplitter::computeRule_median(const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives) -{ +template <> +void BVSplitter::computeRule_median(const OBB& bv, + unsigned int* primitive_indices, + unsigned int num_primitives) { computeSplitVector(bv, split_vector); - computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); + computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, + num_primitives, type, split_vector, + split_value); } -template<> -void BVSplitter::computeRule_bvcenter(const RSS& bv, unsigned int*, unsigned int) -{ +template <> +void BVSplitter::computeRule_bvcenter(const RSS& bv, unsigned int*, + unsigned int) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } - -template<> -void BVSplitter::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives) -{ + +template <> +void BVSplitter::computeRule_mean(const RSS& bv, + unsigned int* primitive_indices, + unsigned int num_primitives) { computeSplitVector(bv, split_vector); - computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); + computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, + num_primitives, type, split_vector, split_value); } -template<> -void BVSplitter::computeRule_median(const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives) -{ +template <> +void BVSplitter::computeRule_median(const RSS& bv, + unsigned int* primitive_indices, + unsigned int num_primitives) { computeSplitVector(bv, split_vector); - computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); + computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, + num_primitives, type, split_vector, + split_value); } -template<> -void BVSplitter::computeRule_bvcenter(const kIOS& bv, unsigned int*, unsigned int) -{ +template <> +void BVSplitter::computeRule_bvcenter(const kIOS& bv, unsigned int*, + unsigned int) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } -template<> -void BVSplitter::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives) -{ +template <> +void BVSplitter::computeRule_mean(const kIOS& bv, + unsigned int* primitive_indices, + unsigned int num_primitives) { computeSplitVector(bv, split_vector); - computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); + computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, + num_primitives, type, split_vector, split_value); } -template<> -void BVSplitter::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives) -{ +template <> +void BVSplitter::computeRule_median(const kIOS& bv, + unsigned int* primitive_indices, + unsigned int num_primitives) { computeSplitVector(bv, split_vector); - computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); + computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, + num_primitives, type, split_vector, + split_value); } -template<> -void BVSplitter::computeRule_bvcenter -(const OBBRSS& bv, unsigned int*, unsigned int) -{ +template <> +void BVSplitter::computeRule_bvcenter(const OBBRSS& bv, unsigned int*, + unsigned int) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } -template<> -void BVSplitter::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives) -{ +template <> +void BVSplitter::computeRule_mean(const OBBRSS& bv, + unsigned int* primitive_indices, + unsigned int num_primitives) { computeSplitVector(bv, split_vector); - computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); + computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, + num_primitives, type, split_vector, + split_value); } -template<> -void BVSplitter::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives) -{ +template <> +void BVSplitter::computeRule_median(const OBBRSS& bv, + unsigned int* primitive_indices, + unsigned int num_primitives) { computeSplitVector(bv, split_vector); - computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); + computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, + num_primitives, type, split_vector, + split_value); } - -template<> -bool BVSplitter::apply(const Vec3f& q) const -{ +template <> +bool BVSplitter::apply(const Vec3f& q) const { return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; } -template<> -bool BVSplitter::apply(const Vec3f& q) const -{ +template <> +bool BVSplitter::apply(const Vec3f& q) const { return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; } -template<> -bool BVSplitter::apply(const Vec3f& q) const -{ +template <> +bool BVSplitter::apply(const Vec3f& q) const { return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; } -template<> -bool BVSplitter::apply(const Vec3f& q) const -{ +template <> +bool BVSplitter::apply(const Vec3f& q) const { return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; } - template class BVSplitter; template class BVSplitter; template class BVSplitter; template class BVSplitter; -} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index 80fc0b91a..bc51940cf 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -37,50 +37,38 @@ #include "hpp/fcl/broadphase/broadphase_SSaP.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /** @brief Functor sorting objects according to the AABB lower x bound */ -struct SortByXLow -{ - bool operator()(const CollisionObject* a, const CollisionObject* b) const - { - if(a->getAABB().min_[0] < b->getAABB().min_[0]) - return true; +struct SortByXLow { + bool operator()(const CollisionObject* a, const CollisionObject* b) const { + if (a->getAABB().min_[0] < b->getAABB().min_[0]) return true; return false; } }; /** @brief Functor sorting objects according to the AABB lower y bound */ -struct SortByYLow -{ - bool operator()(const CollisionObject* a, const CollisionObject* b) const - { - if(a->getAABB().min_[1] < b->getAABB().min_[1]) - return true; +struct SortByYLow { + bool operator()(const CollisionObject* a, const CollisionObject* b) const { + if (a->getAABB().min_[1] < b->getAABB().min_[1]) return true; return false; } }; /** @brief Functor sorting objects according to the AABB lower z bound */ -struct SortByZLow -{ - bool operator()(const CollisionObject* a, const CollisionObject* b) const - { - if(a->getAABB().min_[2] < b->getAABB().min_[2]) - return true; +struct SortByZLow { + bool operator()(const CollisionObject* a, const CollisionObject* b) const { + if (a->getAABB().min_[2] < b->getAABB().min_[2]) return true; return false; } }; /** @brief Dummy collision object with a point AABB */ -class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject -{ -public: - DummyCollisionObject(const AABB& aabb_) : CollisionObject(shared_ptr()) - { +class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject { + public: + DummyCollisionObject(const AABB& aabb_) + : CollisionObject(shared_ptr()) { this->aabb = aabb_; } @@ -88,19 +76,17 @@ class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject }; //============================================================================== -void SSaPCollisionManager::unregisterObject(CollisionObject* obj) -{ +void SSaPCollisionManager::unregisterObject(CollisionObject* obj) { setup(); DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); auto pos_start1 = objs_x.begin(); - auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); + auto pos_end1 = + std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); - while(pos_start1 < pos_end1) - { - if(*pos_start1 == obj) - { + while (pos_start1 < pos_end1) { + if (*pos_start1 == obj) { objs_x.erase(pos_start1); break; } @@ -108,12 +94,11 @@ void SSaPCollisionManager::unregisterObject(CollisionObject* obj) } auto pos_start2 = objs_y.begin(); - auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); + auto pos_end2 = + std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); - while(pos_start2 < pos_end2) - { - if(*pos_start2 == obj) - { + while (pos_start2 < pos_end2) { + if (*pos_start2 == obj) { objs_y.erase(pos_start2); break; } @@ -121,12 +106,11 @@ void SSaPCollisionManager::unregisterObject(CollisionObject* obj) } auto pos_start3 = objs_z.begin(); - auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); + auto pos_end3 = + std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); - while(pos_start3 < pos_end3) - { - if(*pos_start3 == obj) - { + while (pos_start3 < pos_end3) { + if (*pos_start3 == obj) { objs_z.erase(pos_start3); break; } @@ -135,14 +119,12 @@ void SSaPCollisionManager::unregisterObject(CollisionObject* obj) } //============================================================================== -SSaPCollisionManager::SSaPCollisionManager() : setup_(false) -{ +SSaPCollisionManager::SSaPCollisionManager() : setup_(false) { // Do nothing } //============================================================================== -void SSaPCollisionManager::registerObject(CollisionObject* obj) -{ +void SSaPCollisionManager::registerObject(CollisionObject* obj) { objs_x.push_back(obj); objs_y.push_back(obj); objs_z.push_back(obj); @@ -150,10 +132,8 @@ void SSaPCollisionManager::registerObject(CollisionObject* obj) } //============================================================================== -void SSaPCollisionManager::setup() -{ - if(!setup_) - { +void SSaPCollisionManager::setup() { + if (!setup_) { std::sort(objs_x.begin(), objs_x.end(), SortByXLow()); std::sort(objs_y.begin(), objs_y.end(), SortByYLow()); std::sort(objs_z.begin(), objs_z.end(), SortByZLow()); @@ -162,15 +142,13 @@ void SSaPCollisionManager::setup() } //============================================================================== -void SSaPCollisionManager::update() -{ +void SSaPCollisionManager::update() { setup_ = false; setup(); } //============================================================================== -void SSaPCollisionManager::clear() -{ +void SSaPCollisionManager::clear() { objs_x.clear(); objs_y.clear(); objs_z.clear(); @@ -178,26 +156,22 @@ void SSaPCollisionManager::clear() } //============================================================================== -void SSaPCollisionManager::getObjects(std::vector& objs) const -{ +void SSaPCollisionManager::getObjects( + std::vector& objs) const { objs.resize(objs_x.size()); std::copy(objs_x.begin(), objs_x.end(), objs.begin()); } //============================================================================== -bool SSaPCollisionManager::checkColl(std::vector::const_iterator pos_start, - std::vector::const_iterator pos_end, - CollisionObject* obj, - CollisionCallBackBase * callback) const -{ - while(pos_start < pos_end) - { - if(*pos_start != obj) // no collision between the same object +bool SSaPCollisionManager::checkColl( + std::vector::const_iterator pos_start, + std::vector::const_iterator pos_end, CollisionObject* obj, + CollisionCallBackBase* callback) const { + while (pos_start < pos_end) { + if (*pos_start != obj) // no collision between the same object { - if((*pos_start)->getAABB().overlap(obj->getAABB())) - { - if((*callback)(*pos_start, obj)) - return true; + if ((*pos_start)->getAABB().overlap(obj->getAABB())) { + if ((*callback)(*pos_start, obj)) return true; } } pos_start++; @@ -206,20 +180,16 @@ bool SSaPCollisionManager::checkColl(std::vector::const_iterat } //============================================================================== -bool SSaPCollisionManager::checkDis(typename std::vector::const_iterator pos_start, - typename std::vector::const_iterator pos_end, - CollisionObject* obj, - DistanceCallBackBase * callback, - FCL_REAL& min_dist) const -{ - while(pos_start < pos_end) - { - if(*pos_start != obj) // no distance between the same object +bool SSaPCollisionManager::checkDis( + typename std::vector::const_iterator pos_start, + typename std::vector::const_iterator pos_end, + CollisionObject* obj, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const { + while (pos_start < pos_end) { + if (*pos_start != obj) // no distance between the same object { - if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) - { - if((*callback)(*pos_start, obj, min_dist)) - return true; + if ((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) { + if ((*callback)(*pos_start, obj, min_dist)) return true; } } pos_start++; @@ -229,152 +199,150 @@ bool SSaPCollisionManager::checkDis(typename std::vector::cons } //============================================================================== -void SSaPCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +void SSaPCollisionManager::collide(CollisionObject* obj, + CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; collide_(obj, callback); } //============================================================================== -bool SSaPCollisionManager::collide_(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +bool SSaPCollisionManager::collide_(CollisionObject* obj, + CollisionCallBackBase* callback) const { static const unsigned int CUTOFF = 100; DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); bool coll_res = false; const auto pos_start1 = objs_x.begin(); - const auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); + const auto pos_end1 = + std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); long d1 = pos_end1 - pos_start1; - if(d1 > CUTOFF) - { + if (d1 > CUTOFF) { const auto pos_start2 = objs_y.begin(); - const auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); + const auto pos_end2 = + std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); long d2 = pos_end2 - pos_start2; - if(d2 > CUTOFF) - { + if (d2 > CUTOFF) { const auto pos_start3 = objs_z.begin(); - const auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); + const auto pos_end3 = + std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); long d3 = pos_end3 - pos_start3; - if(d3 > CUTOFF) - { - if(d3 <= d2 && d3 <= d1) + if (d3 > CUTOFF) { + if (d3 <= d2 && d3 <= d1) coll_res = checkColl(pos_start3, pos_end3, obj, callback); - else - { - if(d2 <= d3 && d2 <= d1) + else { + if (d2 <= d3 && d2 <= d1) coll_res = checkColl(pos_start2, pos_end2, obj, callback); else coll_res = checkColl(pos_start1, pos_end1, obj, callback); } - } - else + } else coll_res = checkColl(pos_start3, pos_end3, obj, callback); - } - else + } else coll_res = checkColl(pos_start2, pos_end2, obj, callback); - } - else + } else coll_res = checkColl(pos_start1, pos_end1, obj, callback); return coll_res; } //============================================================================== -void SSaPCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase * callback) const -{ +void SSaPCollisionManager::distance(CollisionObject* obj, + DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== -bool SSaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const -{ +bool SSaPCollisionManager::distance_(CollisionObject* obj, + DistanceCallBackBase* callback, + FCL_REAL& min_dist) const { static const unsigned int CUTOFF = 100; Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; Vec3f dummy_vector = obj->getAABB().max_; - if(min_dist < (std::numeric_limits::max)()) - dummy_vector +=Vec3f(min_dist, min_dist, min_dist); - - typename std::vector::const_iterator pos_start1 = objs_x.begin(); - typename std::vector::const_iterator pos_start2 = objs_y.begin(); - typename std::vector::const_iterator pos_start3 = objs_z.begin(); - typename std::vector::const_iterator pos_end1 = objs_x.end(); - typename std::vector::const_iterator pos_end2 = objs_y.end(); - typename std::vector::const_iterator pos_end3 = objs_z.end(); + if (min_dist < (std::numeric_limits::max)()) + dummy_vector += Vec3f(min_dist, min_dist, min_dist); + + typename std::vector::const_iterator pos_start1 = + objs_x.begin(); + typename std::vector::const_iterator pos_start2 = + objs_y.begin(); + typename std::vector::const_iterator pos_start3 = + objs_z.begin(); + typename std::vector::const_iterator pos_end1 = + objs_x.end(); + typename std::vector::const_iterator pos_end2 = + objs_y.end(); + typename std::vector::const_iterator pos_end3 = + objs_z.end(); int status = 1; FCL_REAL old_min_distance; - while(1) - { + while (1) { old_min_distance = min_dist; DummyCollisionObject dummyHigh((AABB(dummy_vector))); - pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); + pos_end1 = + std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); long d1 = pos_end1 - pos_start1; bool dist_res = false; - if(d1 > CUTOFF) - { - pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); + if (d1 > CUTOFF) { + pos_end2 = + std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); long d2 = pos_end2 - pos_start2; - if(d2 > CUTOFF) - { - pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); + if (d2 > CUTOFF) { + pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, + SortByZLow()); long d3 = pos_end3 - pos_start3; - if(d3 > CUTOFF) - { - if(d3 <= d2 && d3 <= d1) + if (d3 > CUTOFF) { + if (d3 <= d2 && d3 <= d1) dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist); - else - { - if(d2 <= d3 && d2 <= d1) - dist_res = checkDis(pos_start2, pos_end2, obj, callback, min_dist); + else { + if (d2 <= d3 && d2 <= d1) + dist_res = + checkDis(pos_start2, pos_end2, obj, callback, min_dist); else - dist_res = checkDis(pos_start1, pos_end1, obj, callback, min_dist); + dist_res = + checkDis(pos_start1, pos_end1, obj, callback, min_dist); } - } - else + } else dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist); - } - else + } else dist_res = checkDis(pos_start2, pos_end2, obj, callback, min_dist); - } - else - { + } else { dist_res = checkDis(pos_start1, pos_end1, obj, callback, min_dist); } - if(dist_res) return true; + if (dist_res) return true; - if(status == 1) - { - if(old_min_distance < (std::numeric_limits::max)()) + if (status == 1) { + if (old_min_distance < (std::numeric_limits::max)()) break; - else - { + else { // from infinity to a finite one, only need one additional loop // to check the possible missed ones to the right of the objs array - if(min_dist < old_min_distance) - { - dummy_vector = obj->getAABB().max_ +Vec3f(min_dist, min_dist, min_dist); + if (min_dist < old_min_distance) { + dummy_vector = + obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist); status = 0; - } - else // need more loop + } else // need more loop { - if(dummy_vector.isApprox(obj->getAABB().max_, std::numeric_limits::epsilon() * 100)) + if (dummy_vector.isApprox( + obj->getAABB().max_, + std::numeric_limits::epsilon() * 100)) dummy_vector = dummy_vector + delta; else dummy_vector = dummy_vector * 2 - obj->getAABB().max_; @@ -385,8 +353,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1; // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2; // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3; - } - else if(status == 0) + } else if (status == 0) break; } @@ -394,183 +361,161 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase } //============================================================================== -int SSaPCollisionManager::selectOptimalAxis(const std::vector& objs_x, - const std::vector& objs_y, - const std::vector& objs_z, - typename std::vector::const_iterator& it_beg, - typename std::vector::const_iterator& it_end) -{ +int SSaPCollisionManager::selectOptimalAxis( + const std::vector& objs_x, + const std::vector& objs_y, + const std::vector& objs_z, + typename std::vector::const_iterator& it_beg, + typename std::vector::const_iterator& it_end) { /// simple sweep and prune method - FCL_REAL delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0]; - FCL_REAL delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1]; - FCL_REAL delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2]; + FCL_REAL delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - + (objs_x[0])->getAABB().min_[0]; + FCL_REAL delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - + (objs_y[0])->getAABB().min_[1]; + FCL_REAL delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - + (objs_z[0])->getAABB().min_[2]; int axis = 0; - if(delta_y > delta_x && delta_y > delta_z) + if (delta_y > delta_x && delta_y > delta_z) axis = 1; - else if(delta_z > delta_y && delta_z > delta_x) + else if (delta_z > delta_y && delta_z > delta_x) axis = 2; - switch(axis) - { - case 0: - it_beg = objs_x.begin(); - it_end = objs_x.end(); - break; - case 1: - it_beg = objs_y.begin(); - it_end = objs_y.end(); - break; - case 2: - it_beg = objs_z.begin(); - it_end = objs_z.end(); - break; + switch (axis) { + case 0: + it_beg = objs_x.begin(); + it_end = objs_x.end(); + break; + case 1: + it_beg = objs_y.begin(); + it_end = objs_y.end(); + break; + case 2: + it_beg = objs_z.begin(); + it_end = objs_z.end(); + break; } return axis; } //============================================================================== -void SSaPCollisionManager::collide(CollisionCallBackBase * callback) const -{ +void SSaPCollisionManager::collide(CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; typename std::vector::const_iterator pos, run_pos, pos_end; - int axis = selectOptimalAxis(objs_x, objs_y, objs_z, - pos, pos_end); + int axis = selectOptimalAxis(objs_x, objs_y, objs_z, pos, pos_end); int axis2 = (axis + 1 > 2) ? 0 : (axis + 1); int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1); run_pos = pos; - while((run_pos < pos_end) && (pos < pos_end)) - { + while ((run_pos < pos_end) && (pos < pos_end)) { CollisionObject* obj = *(pos++); - while(1) - { - if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) - { + while (1) { + if ((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) { run_pos++; - if(run_pos == pos_end) break; + if (run_pos == pos_end) break; continue; - } - else - { + } else { run_pos++; break; } } - if(run_pos < pos_end) - { + if (run_pos < pos_end) { typename std::vector::const_iterator run_pos2 = run_pos; - while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) - { + while ((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) { CollisionObject* obj2 = *run_pos2; run_pos2++; - if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) - { - if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) - { - if((*callback)(obj, obj2)) - return; + if ((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && + (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) { + if ((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && + (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) { + if ((*callback)(obj, obj2)) return; } } - if(run_pos2 == pos_end) break; + if (run_pos2 == pos_end) break; } } } } //============================================================================== -void SSaPCollisionManager::distance(DistanceCallBackBase * callback) const -{ +void SSaPCollisionManager::distance(DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; typename std::vector::const_iterator it, it_end; selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); FCL_REAL min_dist = (std::numeric_limits::max)(); - for(; it != it_end; ++it) - { - if(distance_(*it, callback, min_dist)) - return; + for (; it != it_end; ++it) { + if (distance_(*it, callback, min_dist)) return; } } //============================================================================== -void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase * callback) const -{ +void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const { callback->init(); - SSaPCollisionManager* other_manager = static_cast(other_manager_); + SSaPCollisionManager* other_manager = + static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { collide(callback); return; } - typename std::vector::const_iterator it, end; - if(this->size() < other_manager->size()) - { - for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it) - if(other_manager->collide_(*it, callback)) return; - } - else - { - for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) - if(collide_(*it, callback)) return; + if (this->size() < other_manager->size()) { + for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it) + if (other_manager->collide_(*it, callback)) return; + } else { + for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); + it != end; ++it) + if (collide_(*it, callback)) return; } } //============================================================================== -void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase * callback) const -{ +void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const { callback->init(); - SSaPCollisionManager* other_manager = static_cast(other_manager_); + SSaPCollisionManager* other_manager = + static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { distance(callback); return; } FCL_REAL min_dist = (std::numeric_limits::max)(); typename std::vector::const_iterator it, end; - if(this->size() < other_manager->size()) - { - for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it) - if(other_manager->distance_(*it, callback, min_dist)) return; - } - else - { - for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) - if(distance_(*it, callback, min_dist)) return; + if (this->size() < other_manager->size()) { + for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it) + if (other_manager->distance_(*it, callback, min_dist)) return; + } else { + for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); + it != end; ++it) + if (distance_(*it, callback, min_dist)) return; } } //============================================================================== -bool SSaPCollisionManager::empty() const -{ - return objs_x.empty(); -} +bool SSaPCollisionManager::empty() const { return objs_x.empty(); } //============================================================================== -size_t SSaPCollisionManager::size() const -{ - return objs_x.size(); -} +size_t SSaPCollisionManager::size() const { return objs_x.size(); } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 6bb210feb..22bdab6f8 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -31,53 +31,46 @@ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. - */ + */ /** @author Jia Pan */ #include "hpp/fcl/broadphase/broadphase_SaP.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { //============================================================================== -void SaPCollisionManager::unregisterObject(CollisionObject* obj) -{ +void SaPCollisionManager::unregisterObject(CollisionObject* obj) { auto it = AABB_arr.begin(); - for(auto end = AABB_arr.end(); it != end; ++it) - { - if((*it)->obj == obj) - break; + for (auto end = AABB_arr.end(); it != end; ++it) { + if ((*it)->obj == obj) break; } AABB_arr.erase(it); obj_aabb_map.erase(obj); - if(it == AABB_arr.end()) - return; + if (it == AABB_arr.end()) return; SaPAABB* curr = *it; *it = nullptr; - for(int coord = 0; coord < 3; ++coord) - { - //first delete the lo endpoint of the interval. - if(curr->lo->prev[coord] == nullptr) + for (int coord = 0; coord < 3; ++coord) { + // first delete the lo endpoint of the interval. + if (curr->lo->prev[coord] == nullptr) elist[coord] = curr->lo->next[coord]; else curr->lo->prev[coord]->next[coord] = curr->lo->next[coord]; curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord]; - //then, delete the "hi" endpoint. - if(curr->hi->prev[coord] == nullptr) + // then, delete the "hi" endpoint. + if (curr->hi->prev[coord] == nullptr) elist[coord] = curr->hi->next[coord]; else curr->hi->prev[coord]->next[coord] = curr->hi->next[coord]; - if(curr->hi->next[coord] != nullptr) + if (curr->hi->next[coord] != nullptr) curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord]; } @@ -87,10 +80,9 @@ void SaPCollisionManager::unregisterObject(CollisionObject* obj) overlap_pairs.remove_if(isUnregistered(obj)); } -\ + //============================================================================== -SaPCollisionManager::SaPCollisionManager() -{ +SaPCollisionManager::SaPCollisionManager() { elist[0] = nullptr; elist[1] = nullptr; elist[2] = nullptr; @@ -99,24 +91,19 @@ SaPCollisionManager::SaPCollisionManager() } //============================================================================== -SaPCollisionManager::~SaPCollisionManager() -{ - clear(); -} +SaPCollisionManager::~SaPCollisionManager() { clear(); } //============================================================================== -void SaPCollisionManager::registerObjects(const std::vector& other_objs) -{ - if(other_objs.empty()) return; +void SaPCollisionManager::registerObjects( + const std::vector& other_objs) { + if (other_objs.empty()) return; - if(size() > 0) + if (size() > 0) BroadPhaseCollisionManager::registerObjects(other_objs); - else - { + else { std::vector endpoints(2 * other_objs.size()); - for(size_t i = 0; i < other_objs.size(); ++i) - { + for (size_t i = 0; i < other_objs.size(); ++i) { SaPAABB* sapaabb = new SaPAABB(); sapaabb->obj = other_objs[i]; sapaabb->lo = new EndPoint(); @@ -132,54 +119,54 @@ void SaPCollisionManager::registerObjects(const std::vector& o obj_aabb_map[other_objs[i]] = sapaabb; } - FCL_REAL scale[3]; - for(int coord = 0; coord < 3; ++coord) - { - std::sort(endpoints.begin(), endpoints.end(), - std::bind(std::less(), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, coord), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, coord))); + for (int coord = 0; coord < 3; ++coord) { + std::sort( + endpoints.begin(), endpoints.end(), + std::bind(std::less(), + std::bind(static_cast( + &EndPoint::getVal), + std::placeholders::_1, coord), + std::bind(static_cast( + &EndPoint::getVal), + std::placeholders::_2, coord))); endpoints[0]->prev[coord] = nullptr; endpoints[0]->next[coord] = endpoints[1]; - for(size_t i = 1; i < endpoints.size() - 1; ++i) - { - endpoints[i]->prev[coord] = endpoints[i-1]; - endpoints[i]->next[coord] = endpoints[i+1]; + for (size_t i = 1; i < endpoints.size() - 1; ++i) { + endpoints[i]->prev[coord] = endpoints[i - 1]; + endpoints[i]->next[coord] = endpoints[i + 1]; } - endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2]; + endpoints[endpoints.size() - 1]->prev[coord] = + endpoints[endpoints.size() - 2]; endpoints[endpoints.size() - 1]->next[coord] = nullptr; elist[coord] = endpoints[0]; - scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord]; + scale[coord] = endpoints.back()->aabb->cached.max_[coord] - + endpoints[0]->aabb->cached.min_[coord]; } int axis = 0; - if(scale[axis] < scale[1]) axis = 1; - if(scale[axis] < scale[2]) axis = 2; + if (scale[axis] < scale[1]) axis = 1; + if (scale[axis] < scale[2]) axis = 2; EndPoint* pos = elist[axis]; - while(pos != nullptr) - { + while (pos != nullptr) { EndPoint* pos_next = nullptr; SaPAABB* aabb = pos->aabb; EndPoint* pos_it = pos->next[axis]; - while(pos_it != nullptr) - { - if(pos_it->aabb == aabb) - { - if(pos_next == nullptr) pos_next = pos_it; + while (pos_it != nullptr) { + if (pos_it->aabb == aabb) { + if (pos_next == nullptr) pos_next = pos_it; break; } - if(pos_it->minmax == 0) - { - if(pos_next == nullptr) pos_next = pos_it; - if(pos_it->aabb->cached.overlap(aabb->cached)) + if (pos_it->minmax == 0) { + if (pos_next == nullptr) pos_next = pos_it; + if (pos_it->aabb->cached.overlap(aabb->cached)) overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj); } pos_it = pos_it->next[axis]; @@ -193,8 +180,7 @@ void SaPCollisionManager::registerObjects(const std::vector& o } //============================================================================== -void SaPCollisionManager::registerObject(CollisionObject* obj) -{ +void SaPCollisionManager::registerObject(CollisionObject* obj) { SaPAABB* curr = new SaPAABB; curr->cached = obj->getAABB(); curr->obj = obj; @@ -206,36 +192,32 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) curr->hi->minmax = 1; curr->hi->aabb = curr; - for(int coord = 0; coord < 3; ++coord) - { + for (int coord = 0; coord < 3; ++coord) { EndPoint* current = elist[coord]; // first insert the lo end point - if(current == nullptr) // empty list + if (current == nullptr) // empty list { elist[coord] = curr->lo; curr->lo->prev[coord] = curr->lo->next[coord] = nullptr; - } - else // otherwise, find the correct location in the list and insert + } else // otherwise, find the correct location in the list and insert { EndPoint* curr_lo = curr->lo; FCL_REAL curr_lo_val = curr_lo->getVal()[coord]; - while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != nullptr)) + while ((current->getVal()[coord] < curr_lo_val) && + (current->next[coord] != nullptr)) current = current->next[coord]; - if(current->getVal()[coord] >= curr_lo_val) - { + if (current->getVal()[coord] >= curr_lo_val) { curr_lo->prev[coord] = current->prev[coord]; curr_lo->next[coord] = current; - if(current->prev[coord] == nullptr) + if (current->prev[coord] == nullptr) elist[coord] = curr_lo; else current->prev[coord]->next[coord] = curr_lo; current->prev[coord] = curr_lo; - } - else - { + } else { curr_lo->prev[coord] = current; curr_lo->next[coord] = nullptr; current->next[coord] = curr_lo; @@ -248,36 +230,31 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) EndPoint* curr_hi = curr->hi; FCL_REAL curr_hi_val = curr_hi->getVal()[coord]; - if(coord == 0) - { - while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr)) - { - if(current != curr->lo) - if(current->aabb->cached.overlap(curr->cached)) + if (coord == 0) { + while ((current->getVal()[coord] < curr_hi_val) && + (current->next[coord] != nullptr)) { + if (current != curr->lo) + if (current->aabb->cached.overlap(curr->cached)) overlap_pairs.emplace_back(current->aabb->obj, obj); current = current->next[coord]; } - } - else - { - while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr)) + } else { + while ((current->getVal()[coord] < curr_hi_val) && + (current->next[coord] != nullptr)) current = current->next[coord]; } - if(current->getVal()[coord] >= curr_hi_val) - { + if (current->getVal()[coord] >= curr_hi_val) { curr_hi->prev[coord] = current->prev[coord]; curr_hi->next[coord] = current; - if(current->prev[coord] == nullptr) + if (current->prev[coord] == nullptr) elist[coord] = curr_hi; else current->prev[coord]->next[coord] = curr_hi; current->prev[coord] = curr_hi; - } - else - { + } else { curr_hi->prev[coord] = current; curr_hi->next[coord] = nullptr; current->next[coord] = curr_hi; @@ -292,70 +269,63 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) } //============================================================================== -void SaPCollisionManager::setup() -{ - if(size() == 0) return; +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);; + scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1); + ; scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); int axis = 0; - if(scale[axis] < scale[1]) axis = 1; - if(scale[axis] < scale[2]) axis = 2; + if (scale[axis] < scale[1]) axis = 1; + if (scale[axis] < scale[2]) axis = 2; optimal_axis = axis; } //============================================================================== -void SaPCollisionManager::update_(SaPAABB* updated_aabb) -{ - if(updated_aabb->cached == updated_aabb->obj->getAABB()) - return; +void SaPCollisionManager::update_(SaPAABB* updated_aabb) { + if (updated_aabb->cached == updated_aabb->obj->getAABB()) return; SaPAABB* current = updated_aabb; - Vec3f new_min = current->obj->getAABB().min_; - Vec3f new_max = current->obj->getAABB().max_; + Vec3f new_min = current->obj->getAABB().min_; + Vec3f new_max = current->obj->getAABB().max_; SaPAABB dummy; dummy.cached = current->obj->getAABB(); - for(int coord = 0; coord < 3; ++coord) - { - int direction; // -1 reverse, 0 nochange, 1 forward + for (int coord = 0; coord < 3; ++coord) { + int direction; // -1 reverse, 0 nochange, 1 forward EndPoint* temp; - if(current->lo->getVal((size_t)coord) > new_min[coord]) + if (current->lo->getVal((size_t)coord) > new_min[coord]) direction = -1; - else if(current->lo->getVal((size_t)coord) < new_min[coord]) + else if (current->lo->getVal((size_t)coord) < new_min[coord]) direction = 1; - else direction = 0; + else + direction = 0; - if(direction == -1) - { - //first update the "lo" endpoint of the interval - if(current->lo->prev[coord] != nullptr) - { + if (direction == -1) { + // first update the "lo" endpoint of the interval + if (current->lo->prev[coord] != nullptr) { temp = current->lo; - while((temp != nullptr) && (temp->getVal((size_t)coord) > new_min[coord])) - { - if(temp->minmax == 1) - if(temp->aabb->cached.overlap(dummy.cached)) + while ((temp != nullptr) && + (temp->getVal((size_t)coord) > new_min[coord])) { + if (temp->minmax == 1) + if (temp->aabb->cached.overlap(dummy.cached)) addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->prev[coord]; } - if(temp == nullptr) - { + if (temp == nullptr) { current->lo->prev[coord]->next[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = nullptr; current->lo->next[coord] = elist[coord]; elist[coord]->prev[coord] = current->lo; elist[coord] = current->lo; - } - else - { + } else { current->lo->prev[coord]->next[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = temp; @@ -369,48 +339,42 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) // update hi end point temp = current->hi; - while(temp->getVal((size_t)coord) > new_max[coord]) - { - if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached))) + while (temp->getVal((size_t)coord) > new_max[coord]) { + if ((temp->minmax == 0) && + (temp->aabb->cached.overlap(current->cached))) removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->prev[coord]; } current->hi->prev[coord]->next[coord] = current->hi->next[coord]; - if(current->hi->next[coord] != nullptr) + if (current->hi->next[coord] != nullptr) current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp; current->hi->next[coord] = temp->next[coord]; - if(temp->next[coord] != nullptr) + if (temp->next[coord] != nullptr) temp->next[coord]->prev[coord] = current->hi; temp->next[coord] = current->hi; current->hi->getVal((size_t)coord) = new_max[coord]; - } - else if(direction == 1) - { - //here, we first update the "hi" endpoint. - if(current->hi->next[coord] != nullptr) - { + } else if (direction == 1) { + // here, we first update the "hi" endpoint. + if (current->hi->next[coord] != nullptr) { temp = current->hi; - while((temp->next[coord] != nullptr) && (temp->getVal((size_t)coord) < new_max[coord])) - { - if(temp->minmax == 0) - if(temp->aabb->cached.overlap(dummy.cached)) + while ((temp->next[coord] != nullptr) && + (temp->getVal((size_t)coord) < new_max[coord])) { + if (temp->minmax == 0) + if (temp->aabb->cached.overlap(dummy.cached)) addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->next[coord]; } - if(temp->getVal((size_t)coord) < new_max[coord]) - { + if (temp->getVal((size_t)coord) < new_max[coord]) { current->hi->prev[coord]->next[coord] = current->hi->next[coord]; current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp; current->hi->next[coord] = nullptr; temp->next[coord] = current->hi; - } - else - { + } else { current->hi->prev[coord]->next[coord] = current->hi->next[coord]; current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp->prev[coord]; @@ -422,24 +386,24 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) current->hi->getVal((size_t)coord) = new_max[coord]; - //then, update the "lo" endpoint of the interval. + // then, update the "lo" endpoint of the interval. temp = current->lo; - while(temp->getVal((size_t)coord) < new_min[coord]) - { - if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached))) + while (temp->getVal((size_t)coord) < new_min[coord]) { + if ((temp->minmax == 1) && + (temp->aabb->cached.overlap(current->cached))) removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->next[coord]; } - if(current->lo->prev[coord] != nullptr) + if (current->lo->prev[coord] != nullptr) current->lo->prev[coord]->next[coord] = current->lo->next[coord]; else elist[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = temp->prev[coord]; current->lo->next[coord] = temp; - if(temp->prev[coord] != nullptr) + if (temp->prev[coord] != nullptr) temp->prev[coord]->next[coord] = current->lo; else elist[coord] = current->lo; @@ -450,15 +414,12 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) } //============================================================================== -void SaPCollisionManager::updateVelist() -{ - for(int coord = 0; coord < 3; ++coord) - { +void SaPCollisionManager::updateVelist() { + for (int coord = 0; coord < 3; ++coord) { velist[coord].resize(size() * 2); EndPoint* current = elist[coord]; size_t id = 0; - while(current) - { + while (current) { velist[coord][id] = current; current = current->next[coord]; id++; @@ -467,8 +428,7 @@ void SaPCollisionManager::updateVelist() } //============================================================================== -void SaPCollisionManager::update(CollisionObject* updated_obj) -{ +void SaPCollisionManager::update(CollisionObject* updated_obj) { update_(obj_aabb_map[updated_obj]); updateVelist(); @@ -477,9 +437,9 @@ void SaPCollisionManager::update(CollisionObject* updated_obj) } //============================================================================== -void SaPCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0; i < updated_objs.size(); ++i) +void SaPCollisionManager::update( + const std::vector& updated_objs) { + for (size_t i = 0; i < updated_objs.size(); ++i) update_(obj_aabb_map[updated_objs[i]]); updateVelist(); @@ -488,10 +448,8 @@ void SaPCollisionManager::update(const std::vector& updated_ob } //============================================================================== -void SaPCollisionManager::update() -{ - for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) - { +void SaPCollisionManager::update() { + for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { update_(*it); } @@ -501,10 +459,8 @@ void SaPCollisionManager::update() } //============================================================================== -void SaPCollisionManager::clear() -{ - for(auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) - { +void SaPCollisionManager::clear() { + for (auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) { delete (*it)->hi; delete (*it)->lo; delete *it; @@ -526,19 +482,19 @@ void SaPCollisionManager::clear() } //============================================================================== -void SaPCollisionManager::getObjects(std::vector& objs) const -{ +void SaPCollisionManager::getObjects( + std::vector& objs) const { objs.resize(AABB_arr.size()); size_t i = 0; - for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it, ++i) - { + for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; + ++it, ++i) { objs[i] = (*it)->obj; } } //============================================================================== -bool SaPCollisionManager::collide_(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +bool SaPCollisionManager::collide_(CollisionObject* obj, + CollisionCallBackBase* callback) const { int axis = optimal_axis; const AABB& obj_aabb = obj->getAABB(); @@ -551,27 +507,29 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, CollisionCallBackBase * dummy.minmax = 1; dummy.aabb = &dummy_aabb; - // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly - const auto res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, - std::bind(std::less(), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); + // compute stop_pos by binary search, this is cheaper than check it in while + // iteration linearly + const auto res_it = std::upper_bound( + velist[axis].begin(), velist[axis].end(), &dummy, + std::bind(std::less(), + std::bind(static_cast( + &EndPoint::getVal), + std::placeholders::_1, axis), + std::bind(static_cast( + &EndPoint::getVal), + std::placeholders::_2, axis))); EndPoint* end_pos = nullptr; - if(res_it != velist[axis].end()) - end_pos = *res_it; + if (res_it != velist[axis].end()) end_pos = *res_it; EndPoint* pos = elist[axis]; - while(pos != end_pos) - { - if(pos->aabb->obj != obj) - { - if((pos->minmax == 0) && (pos->aabb->hi->getVal((size_t)axis) >= min_val)) - { - if(pos->aabb->cached.overlap(obj->getAABB())) - if((*callback)(obj, pos->aabb->obj)) - return true; + while (pos != end_pos) { + if (pos->aabb->obj != obj) { + if ((pos->minmax == 0) && + (pos->aabb->hi->getVal((size_t)axis) >= min_val)) { + if (pos->aabb->cached.overlap(obj->getAABB())) + if ((*callback)(obj, pos->aabb->obj)) return true; } } pos = pos->next[axis]; @@ -581,31 +539,24 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, CollisionCallBackBase * } //============================================================================== -void SaPCollisionManager::addToOverlapPairs(const SaPPair& p) -{ +void SaPCollisionManager::addToOverlapPairs(const SaPPair& p) { bool repeated = false; - for(auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) - { - if(*it == p) - { + for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; + ++it) { + if (*it == p) { repeated = true; break; } } - if(!repeated) - overlap_pairs.push_back(p); + if (!repeated) overlap_pairs.push_back(p); } //============================================================================== -void SaPCollisionManager::removeFromOverlapPairs(const SaPPair& p) -{ - for(auto it = overlap_pairs.begin(), end = overlap_pairs.end(); - it != end; - ++it) - { - if(*it == p) - { +void SaPCollisionManager::removeFromOverlapPairs(const SaPPair& p) { + for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; + ++it) { + if (*it == p) { overlap_pairs.erase(it); break; } @@ -615,23 +566,23 @@ void SaPCollisionManager::removeFromOverlapPairs(const SaPPair& p) } //============================================================================== -void SaPCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +void SaPCollisionManager::collide(CollisionObject* obj, + CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; collide_(obj, callback); } //============================================================================== -bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const -{ - Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; +bool SaPCollisionManager::distance_(CollisionObject* obj, + DistanceCallBackBase* callback, + FCL_REAL& min_dist) const { + Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); - if(min_dist < (std::numeric_limits::max)()) - { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + if (min_dist < (std::numeric_limits::max)()) { + Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } @@ -642,8 +593,7 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase * EndPoint* start_pos = elist[axis]; - while(1) - { + while (1) { old_min_distance = min_dist; FCL_REAL min_val = aabb.min_[axis]; // FCL_REAL max_val = aabb.max_[axis]; @@ -654,43 +604,36 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase * dummy.minmax = 1; dummy.aabb = &dummy_aabb; - - const auto res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, - std::bind(std::less(), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); + const auto res_it = std::upper_bound( + velist[axis].begin(), velist[axis].end(), &dummy, + std::bind(std::less(), + std::bind(static_cast( + &EndPoint::getVal), + std::placeholders::_1, axis), + std::bind(static_cast( + &EndPoint::getVal), + std::placeholders::_2, axis))); EndPoint* end_pos = nullptr; - if(res_it != velist[axis].end()) - end_pos = *res_it; + if (res_it != velist[axis].end()) end_pos = *res_it; EndPoint* pos = start_pos; - while(pos != end_pos) - { - // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos. - // but this seems slower. - if((pos->minmax == 0) && (pos->aabb->hi->getVal((size_t)axis) >= min_val)) - { + while (pos != end_pos) { + // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and + // then update start_pos to end_pos. but this seems slower. + if ((pos->minmax == 0) && + (pos->aabb->hi->getVal((size_t)axis) >= min_val)) { CollisionObject* curr_obj = pos->aabb->obj; - if(curr_obj != obj) - { - if(!this->enable_tested_set_) - { - if(pos->aabb->cached.distance(obj->getAABB()) < min_dist) - { - if((*callback)(curr_obj, obj, min_dist)) - return true; + if (curr_obj != obj) { + if (!this->enable_tested_set_) { + if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) { + if ((*callback)(curr_obj, obj, min_dist)) return true; } - } - else - { - if(!this->inTestedSet(curr_obj, obj)) - { - if(pos->aabb->cached.distance(obj->getAABB()) < min_dist) - { - if((*callback)(curr_obj, obj, min_dist)) - return true; + } else { + if (!this->inTestedSet(curr_obj, obj)) { + if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) { + if ((*callback)(curr_obj, obj, min_dist)) return true; } this->insertTestedSet(curr_obj, obj); @@ -702,28 +645,22 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase * pos = pos->next[axis]; } - if(status == 1) - { - if(old_min_distance < (std::numeric_limits::max)()) + if (status == 1) { + if (old_min_distance < (std::numeric_limits::max)()) break; - else - { - if(min_dist < old_min_distance) - { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + else { + if (min_dist < old_min_distance) { + Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; - } - else - { - if(aabb == obj->getAABB()) + } else { + if (aabb == obj->getAABB()) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } - } - else if(status == 0) + } else if (status == 0) break; } @@ -731,10 +668,10 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase * } //============================================================================== -void SaPCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase * callback) const -{ +void SaPCollisionManager::distance(CollisionObject* obj, + DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); @@ -742,36 +679,31 @@ void SaPCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase * } //============================================================================== -void SaPCollisionManager::collide(CollisionCallBackBase * callback) const -{ +void SaPCollisionManager::collide(CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; - for(auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end; ++it) - { + for (auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end; + ++it) { CollisionObject* obj1 = it->obj1; CollisionObject* obj2 = it->obj2; - if((*callback)(obj1, obj2)) - return; + if ((*callback)(obj1, obj2)) return; } } //============================================================================== -void SaPCollisionManager::distance(DistanceCallBackBase * callback) const -{ +void SaPCollisionManager::distance(DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; this->enable_tested_set_ = true; this->tested_set.clear(); FCL_REAL min_dist = (std::numeric_limits::max)(); - for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) - { - if(distance_((*it)->obj, callback, min_dist)) - break; + for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { + if (distance_((*it)->obj, callback, min_dist)) break; } this->enable_tested_set_ = false; @@ -779,159 +711,137 @@ void SaPCollisionManager::distance(DistanceCallBackBase * callback) const } //============================================================================== -void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase * callback) const -{ +void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const { callback->init(); - SaPCollisionManager* other_manager = static_cast(other_manager_); + SaPCollisionManager* other_manager = + static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { collide(callback); return; } - if(this->size() < other_manager->size()) - { - for(auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it) - { - if(other_manager->collide_((*it)->obj, callback)) - return; + if (this->size() < other_manager->size()) { + for (auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it) { + if (other_manager->collide_((*it)->obj, callback)) return; } - } - else - { - for(auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it) - { - if(collide_((*it)->obj, callback)) - return; + } else { + for (auto it = other_manager->AABB_arr.cbegin(), + end = other_manager->AABB_arr.cend(); + it != end; ++it) { + if (collide_((*it)->obj, callback)) return; } } } //============================================================================== -void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase * callback) const -{ +void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const { callback->init(); - SaPCollisionManager* other_manager = static_cast(other_manager_); + SaPCollisionManager* other_manager = + static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { distance(callback); return; } FCL_REAL min_dist = (std::numeric_limits::max)(); - if(this->size() < other_manager->size()) - { - for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) - { - if(other_manager->distance_((*it)->obj, callback, min_dist)) - return; + if (this->size() < other_manager->size()) { + for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { + if (other_manager->distance_((*it)->obj, callback, min_dist)) return; } - } - else - { - for(auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it) - { - if(distance_((*it)->obj, callback, min_dist)) - return; + } else { + for (auto it = other_manager->AABB_arr.cbegin(), + end = other_manager->AABB_arr.cend(); + it != end; ++it) { + if (distance_((*it)->obj, callback, min_dist)) return; } } } //============================================================================== -bool SaPCollisionManager::empty() const -{ - return AABB_arr.size(); -} +bool SaPCollisionManager::empty() const { return AABB_arr.size(); } //============================================================================== -size_t SaPCollisionManager::size() const -{ - return AABB_arr.size(); -} +size_t SaPCollisionManager::size() const { return AABB_arr.size(); } //============================================================================== -const Vec3f&SaPCollisionManager::EndPoint::getVal() const -{ - if(minmax) return aabb->cached.max_; - else return aabb->cached.min_; +const Vec3f& SaPCollisionManager::EndPoint::getVal() const { + if (minmax) + return aabb->cached.max_; + else + return aabb->cached.min_; } //============================================================================== -Vec3f&SaPCollisionManager::EndPoint::getVal() -{ - if(minmax) return aabb->cached.max_; - else return aabb->cached.min_; +Vec3f& SaPCollisionManager::EndPoint::getVal() { + if (minmax) + return aabb->cached.max_; + else + return aabb->cached.min_; } //============================================================================== -FCL_REAL SaPCollisionManager::EndPoint::getVal(size_t i) const -{ - if(minmax) +FCL_REAL SaPCollisionManager::EndPoint::getVal(size_t i) const { + if (minmax) return aabb->cached.max_[(int)i]; else return aabb->cached.min_[(int)i]; } //============================================================================== -FCL_REAL& SaPCollisionManager::EndPoint::getVal(size_t i) -{ - if(minmax) +FCL_REAL& SaPCollisionManager::EndPoint::getVal(size_t i) { + if (minmax) return aabb->cached.max_[(int)i]; else return aabb->cached.min_[(int)i]; } //============================================================================== -SaPCollisionManager::SaPPair::SaPPair(CollisionObject* a, CollisionObject* b) -{ - if(a < b) - { +SaPCollisionManager::SaPPair::SaPPair(CollisionObject* a, CollisionObject* b) { + if (a < b) { obj1 = a; obj2 = b; - } - else - { + } else { obj1 = b; obj2 = a; } } //============================================================================== -bool SaPCollisionManager::SaPPair::operator ==(const SaPPair& other) const -{ +bool SaPCollisionManager::SaPPair::operator==(const SaPPair& other) const { return ((obj1 == other.obj1) && (obj2 == other.obj2)); } //============================================================================== -SaPCollisionManager::isUnregistered::isUnregistered(CollisionObject* obj_) : obj(obj_) -{} +SaPCollisionManager::isUnregistered::isUnregistered(CollisionObject* obj_) + : obj(obj_) {} //============================================================================== -bool SaPCollisionManager::isUnregistered::operator()(const SaPPair& pair) const -{ +bool SaPCollisionManager::isUnregistered::operator()( + const SaPPair& pair) const { return (pair.obj1 == obj) || (pair.obj2 == obj); } //============================================================================== -SaPCollisionManager::isNotValidPair::isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_), - obj2(obj2_) -{ +SaPCollisionManager::isNotValidPair::isNotValidPair(CollisionObject* obj1_, + CollisionObject* obj2_) + : obj1(obj1_), obj2(obj2_) { // Do nothing } //============================================================================== -bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) -{ +bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) { return (pair.obj1 == obj1) && (pair.obj2 == obj2); } -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index fe3726cb1..17e50dda1 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -31,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 Jia Pan */ @@ -44,192 +44,160 @@ namespace hpp { namespace fcl { //============================================================================== -NaiveCollisionManager::NaiveCollisionManager() -{ +NaiveCollisionManager::NaiveCollisionManager() { // Do nothing } //============================================================================== -void NaiveCollisionManager::registerObjects(const std::vector& other_objs) -{ +void NaiveCollisionManager::registerObjects( + const std::vector& other_objs) { std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs)); } //============================================================================== -void NaiveCollisionManager::unregisterObject(CollisionObject* obj) -{ +void NaiveCollisionManager::unregisterObject(CollisionObject* obj) { objs.remove(obj); } //============================================================================== -void NaiveCollisionManager::registerObject(CollisionObject* obj) -{ +void NaiveCollisionManager::registerObject(CollisionObject* obj) { objs.push_back(obj); } //============================================================================== -void NaiveCollisionManager::setup() -{ +void NaiveCollisionManager::setup() { // Do nothing } //============================================================================== -void NaiveCollisionManager::update() -{ +void NaiveCollisionManager::update() { // Do nothing } //============================================================================== -void NaiveCollisionManager::clear() -{ - objs.clear(); -} +void NaiveCollisionManager::clear() { objs.clear(); } //============================================================================== -void NaiveCollisionManager::getObjects(std::vector& objs_) const -{ +void NaiveCollisionManager::getObjects( + std::vector& objs_) const { objs_.resize(objs.size()); std::copy(objs.begin(), objs.end(), objs_.begin()); } //============================================================================== -void NaiveCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +void NaiveCollisionManager::collide(CollisionObject* obj, + CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; - for(auto* obj2 : objs) - { - if((*callback)(obj, obj2)) - return; + for (auto* obj2 : objs) { + if ((*callback)(obj, obj2)) return; } } //============================================================================== -void NaiveCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase * callback) const -{ +void NaiveCollisionManager::distance(CollisionObject* obj, + DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); - for(auto* obj2 : objs) - { - if(obj->getAABB().distance(obj2->getAABB()) < min_dist) - { - if((*callback)(obj, obj2, min_dist)) - return; + for (auto* obj2 : objs) { + if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { + if ((*callback)(obj, obj2, min_dist)) return; } } } //============================================================================== -void NaiveCollisionManager::collide(CollisionCallBackBase * callback) const -{ +void NaiveCollisionManager::collide(CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; - - for(typename std::list::const_iterator it1 = objs.begin(), end = objs.end(); - it1 != end; ++it1) - { - typename std::list::const_iterator it2 = it1; it2++; - for(; it2 != end; ++it2) - { - if((*it1)->getAABB().overlap((*it2)->getAABB())) - { - if((*callback)(*it1, *it2)) - return; + if (size() == 0) return; + + for (typename std::list::const_iterator it1 = objs.begin(), + end = objs.end(); + it1 != end; ++it1) { + typename std::list::const_iterator it2 = it1; + it2++; + for (; it2 != end; ++it2) { + if ((*it1)->getAABB().overlap((*it2)->getAABB())) { + if ((*callback)(*it1, *it2)) return; } } } } //============================================================================== -void NaiveCollisionManager::distance(DistanceCallBackBase * callback) const -{ +void NaiveCollisionManager::distance(DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); - for(typename std::list::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) - { - typename std::list::const_iterator it2 = it1; it2++; - for(; it2 != end; ++it2) - { - if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) - { - if((*callback)(*it1, *it2, min_dist)) - return; + for (typename std::list::const_iterator it1 = objs.begin(), + end = objs.end(); + it1 != end; ++it1) { + typename std::list::const_iterator it2 = it1; + it2++; + for (; it2 != end; ++it2) { + if ((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) { + if ((*callback)(*it1, *it2, min_dist)) return; } } } } //============================================================================== -void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase * callback) const -{ +void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const { callback->init(); - NaiveCollisionManager* other_manager = static_cast(other_manager_); + NaiveCollisionManager* other_manager = + static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { collide(callback); return; } - for(auto* obj1 : objs) - { - for(auto* obj2 : other_manager->objs) - { - if(obj1->getAABB().overlap(obj2->getAABB())) - { - if((*callback)(obj1, obj2)) - return; + for (auto* obj1 : objs) { + for (auto* obj2 : other_manager->objs) { + if (obj1->getAABB().overlap(obj2->getAABB())) { + if ((*callback)(obj1, obj2)) return; } } } } //============================================================================== -void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase * callback) const -{ +void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const { callback->init(); - NaiveCollisionManager* other_manager = static_cast(other_manager_); + NaiveCollisionManager* other_manager = + static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { distance(callback); return; } FCL_REAL min_dist = (std::numeric_limits::max)(); - for(auto* obj1 : objs) - { - for(auto* obj2 : other_manager->objs) - { - if(obj1->getAABB().distance(obj2->getAABB()) < min_dist) - { - if((*callback)(obj1, obj2, min_dist)) - return; + for (auto* obj1 : objs) { + for (auto* obj2 : other_manager->objs) { + if (obj1->getAABB().distance(obj2->getAABB()) < min_dist) { + if ((*callback)(obj1, obj2, min_dist)) return; } } } } //============================================================================== -bool NaiveCollisionManager::empty() const -{ - return objs.empty(); -} +bool NaiveCollisionManager::empty() const { return objs.empty(); } //============================================================================== -size_t NaiveCollisionManager::size() const -{ - return objs.size(); -} +size_t NaiveCollisionManager::size() const { return objs.size(); } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 878ad297a..90f03249c 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -42,28 +42,23 @@ namespace fcl { //============================================================================== BroadPhaseCollisionManager::BroadPhaseCollisionManager() - : enable_tested_set_(false) -{ + : enable_tested_set_(false) { // Do nothing } //============================================================================== -BroadPhaseCollisionManager::~BroadPhaseCollisionManager() -{ +BroadPhaseCollisionManager::~BroadPhaseCollisionManager() { // Do nothing } //============================================================================== void BroadPhaseCollisionManager::registerObjects( - const std::vector& other_objs) -{ - for(size_t i = 0; i < other_objs.size(); ++i) - registerObject(other_objs[i]); + const std::vector& other_objs) { + for (size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); } //============================================================================== -void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) -{ +void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) { HPP_FCL_UNUSED_VARIABLE(updated_obj); update(); @@ -71,28 +66,29 @@ void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) //============================================================================== void BroadPhaseCollisionManager::update( - const std::vector& updated_objs) -{ + const std::vector& updated_objs) { HPP_FCL_UNUSED_VARIABLE(updated_objs); update(); } //============================================================================== -bool BroadPhaseCollisionManager::inTestedSet( - CollisionObject* a, CollisionObject* b) const -{ - if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end(); - else return tested_set.find(std::make_pair(b, a)) != tested_set.end(); +bool BroadPhaseCollisionManager::inTestedSet(CollisionObject* a, + CollisionObject* b) const { + if (a < b) + return tested_set.find(std::make_pair(a, b)) != tested_set.end(); + else + return tested_set.find(std::make_pair(b, a)) != tested_set.end(); } //============================================================================== -void BroadPhaseCollisionManager::insertTestedSet( - CollisionObject* a, CollisionObject* b) const -{ - if(a < b) tested_set.insert(std::make_pair(a, b)); - else tested_set.insert(std::make_pair(b, a)); +void BroadPhaseCollisionManager::insertTestedSet(CollisionObject* a, + CollisionObject* b) const { + if (a < b) + tested_set.insert(std::make_pair(a, b)); + else + tested_set.insert(std::make_pair(b, a)); } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 746aeb4c2..908a849a7 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -55,26 +55,19 @@ namespace dynamic_AABB_tree { #if HPP_FCL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Transform3f& tf2, - CollisionCallBackBase * callback) -{ - if(!root2) - { - if(root1->isLeaf()) - { + const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Transform3f& tf2, + CollisionCallBackBase* callback) { + if (!root2) { + if (root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); - if(!obj1->collisionGeometry()->isFree()) - { + if (!obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3f::Identity(), obb1); convertBV(root2_bv, tf2, obb2); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); @@ -85,29 +78,25 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, return (*callback)(obj1, &obj2); } } - } - else - { - if(collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, tf2, callback)) + } else { + if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, tf2, + callback)) return true; - if(collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, tf2, callback)) + if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, tf2, + callback)) return true; } return false; - } - else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { + } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); - if(!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) - { + if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3f::Identity(), obb1); convertBV(root2_bv, tf2, obb2); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); @@ -117,43 +106,39 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); - } - else return false; - } - else return false; + } else + return false; + } else + return false; } OBB obb1, obb2; convertBV(root1->bv, Transform3f::Identity(), obb1); convertBV(root2_bv, tf2, obb2); - if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; + if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, callback)) + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { + if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, + callback)) return true; - if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, callback)) + if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, + callback)) return true; - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(root1, tree2, child, child_bv, tf2, callback)) + if (collisionRecurse_(root1, tree2, child, child_bv, tf2, callback)) return true; - } - else - { + } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(root1, tree2, nullptr, child_bv, tf2, callback)) + if (collisionRecurse_(root1, tree2, nullptr, child_bv, tf2, callback)) return true; } } @@ -163,71 +148,59 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Transform3f& tf2, - DistanceCallBackBase * callback, - FCL_REAL & min_dist) -{ - if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - if(tree2->isNodeOccupied(root2)) - { + const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Transform3f& tf2, + DistanceCallBackBase* callback, FCL_REAL& min_dist) { + if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { + if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); - return (*callback)(static_cast(root1->data), &obj, min_dist); - } - else return false; + return (*callback)(static_cast(root1->data), &obj, + min_dist); + } else + return false; } - if(!tree2->isNodeOccupied(root2)) return false; + if (!tree2->isNodeOccupied(root2)) return false; - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { AABB aabb2; convertBV(root2_bv, tf2, aabb2); FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, + callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, + callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, + callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, + callback, min_dist)) return true; } } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -236,9 +209,9 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, convertBV(child_bv, tf2, aabb2); FCL_REAL d = root1->bv.distance(aabb2); - if(d < min_dist) - { - if(distanceRecurse_(root1, tree2, child, child_bv, tf2, callback, min_dist)) + if (d < min_dist) { + if (distanceRecurse_(root1, tree2, child, child_bv, tf2, callback, + min_dist)) return true; } } @@ -250,31 +223,27 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Transform3f& tf2, - CollisionCallBackBase * callback) -{ - if(tf2.rotation().isIdentity()) - return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), callback); - else // has rotation + const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Transform3f& tf2, + CollisionCallBackBase* callback) { + if (tf2.rotation().isIdentity()) + return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), + callback); + else // has rotation return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, callback); } //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Transform3f& tf2, - DistanceCallBackBase * callback, - FCL_REAL & min_dist) -{ - if(tf2.rotation().isIdentity()) - return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), callback, min_dist); + const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Transform3f& tf2, + DistanceCallBackBase* callback, FCL_REAL& min_dist) { + if (tf2.rotation().isIdentity()) + return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), + callback, min_dist); else - return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, callback, min_dist); + return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, callback, + min_dist); } #endif @@ -282,70 +251,59 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, - CollisionCallBackBase * callback) -{ - if(root1->isLeaf() && root2->isLeaf()) - { - if(!root1->bv.overlap(root2->bv)) return false; - return (*callback)(static_cast(root1->data), static_cast(root2->data)); + CollisionCallBackBase* callback) { + if (root1->isLeaf() && root2->isLeaf()) { + if (!root1->bv.overlap(root2->bv)) return false; + return (*callback)(static_cast(root1->data), + static_cast(root2->data)); } - if(!root1->bv.overlap(root2->bv)) return false; + if (!root1->bv.overlap(root2->bv)) return false; - if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) - { - if(collisionRecurse(root1->children[0], root2, callback)) - return true; - if(collisionRecurse(root1->children[1], root2, callback)) - return true; - } - else - { - if(collisionRecurse(root1, root2->children[0], callback)) - return true; - if(collisionRecurse(root1, root2->children[1], callback)) - return true; + if (root2->isLeaf() || + (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { + if (collisionRecurse(root1->children[0], root2, callback)) return true; + if (collisionRecurse(root1->children[1], root2, callback)) return true; + } else { + if (collisionRecurse(root1, root2->children[0], callback)) return true; + if (collisionRecurse(root1, root2->children[1], callback)) return true; } return false; } //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, - CollisionObject* query, - CollisionCallBackBase * callback) -{ - if(root->isLeaf()) - { - if(!root->bv.overlap(query->getAABB())) return false; + CollisionObject* query, CollisionCallBackBase* callback) { + if (root->isLeaf()) { + if (!root->bv.overlap(query->getAABB())) return false; return (*callback)(static_cast(root->data), query); } - if(!root->bv.overlap(query->getAABB())) return false; + if (!root->bv.overlap(query->getAABB())) return false; - size_t select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1])); + size_t select_res = + select(query->getAABB(), *(root->children[0]), *(root->children[1])); - if(collisionRecurse(root->children[select_res], query, callback)) + if (collisionRecurse(root->children[select_res], query, callback)) return true; - if(collisionRecurse(root->children[1-select_res], query, callback)) + if (collisionRecurse(root->children[1 - select_res], query, callback)) return true; return false; } //============================================================================== -bool selfCollisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, - CollisionCallBackBase * callback) -{ - if(root->isLeaf()) return false; +bool selfCollisionRecurse( + DynamicAABBTreeCollisionManager::DynamicAABBNode* root, + CollisionCallBackBase* callback) { + if (root->isLeaf()) return false; - if(selfCollisionRecurse(root->children[0], callback)) - return true; + if (selfCollisionRecurse(root->children[0], callback)) return true; - if(selfCollisionRecurse(root->children[1], callback)) - return true; + if (selfCollisionRecurse(root->children[1], callback)) return true; - if(collisionRecurse(root->children[0], root->children[1], callback)) + if (collisionRecurse(root->children[0], root->children[1], callback)) return true; return false; @@ -354,80 +312,61 @@ bool selfCollisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, - DistanceCallBackBase * callback, - FCL_REAL & min_dist) -{ - if(root1->isLeaf() && root2->isLeaf()) - { + DistanceCallBackBase* callback, FCL_REAL& min_dist) { + if (root1->isLeaf() && root2->isLeaf()) { CollisionObject* root1_obj = static_cast(root1->data); CollisionObject* root2_obj = static_cast(root2->data); return (*callback)(root1_obj, root2_obj, min_dist); } - if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) - { + if (root2->isLeaf() || + (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv); FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(root1->children[1], root2, callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse(root1->children[1], root2, callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse(root1->children[0], root2, callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse(root1->children[0], root2, callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(root1->children[0], root2, callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse(root1->children[0], root2, callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse(root1->children[1], root2, callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse(root1->children[1], root2, callback, min_dist)) return true; } } - } - else - { + } else { FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv); FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(root1, root2->children[1], callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse(root1, root2->children[1], callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse(root1, root2->children[0], callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse(root1, root2->children[0], callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(root1, root2->children[0], callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse(root1, root2->children[0], callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse(root1, root2->children[1], callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse(root1, root2->children[1], callback, min_dist)) return true; } } @@ -438,12 +377,9 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, - CollisionObject* query, - DistanceCallBackBase * callback, - FCL_REAL & min_dist) -{ - if(root->isLeaf()) - { + CollisionObject* query, DistanceCallBackBase* callback, + FCL_REAL& min_dist) { + if (root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return (*callback)(root_obj, query, min_dist); } @@ -451,31 +387,24 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv); FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(root->children[1], query, callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse(root->children[1], query, callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse(root->children[0], query, callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse(root->children[0], query, callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(root->children[0], query, callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse(root->children[0], query, callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse(root->children[1], query, callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse(root->children[1], query, callback, min_dist)) return true; } } @@ -485,30 +414,25 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, //============================================================================== bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, - DistanceCallBackBase * callback, - FCL_REAL& min_dist) -{ - if(root->isLeaf()) return false; + DistanceCallBackBase* callback, FCL_REAL& min_dist) { + if (root->isLeaf()) return false; - if(selfDistanceRecurse(root->children[0], callback, min_dist)) - return true; + if (selfDistanceRecurse(root->children[0], callback, min_dist)) return true; - if(selfDistanceRecurse(root->children[1], callback, min_dist)) - return true; + if (selfDistanceRecurse(root->children[1], callback, min_dist)) return true; - if(distanceRecurse(root->children[0], root->children[1], callback, min_dist)) + if (distanceRecurse(root->children[0], root->children[1], callback, min_dist)) return true; return false; } -} // namespace dynamic_AABB_tree +} // namespace dynamic_AABB_tree -} // namespace detail +} // namespace detail //============================================================================== -DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager() -{ +DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager() { tree_topdown_balance_threshold = &dtree.bu_threshold; tree_topdown_level = &dtree.topdown_level; max_tree_nonbalanced_level = 10; @@ -525,21 +449,17 @@ DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager() //============================================================================== void DynamicAABBTreeCollisionManager::registerObjects( - const std::vector& other_objs) -{ - if(other_objs.empty()) return; + const std::vector& other_objs) { + if (other_objs.empty()) return; - if(size() > 0) - { + if (size() > 0) { BroadPhaseCollisionManager::registerObjects(other_objs); - } - else - { + } else { std::vector leaves(other_objs.size()); table.rehash(other_objs.size()); - for(size_t i = 0, size = other_objs.size(); i < size; ++i) - { - DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree + for (size_t i = 0, size = other_objs.size(); i < size; ++i) { + DynamicAABBNode* node = + new DynamicAABBNode; // node will be managed by the dtree node->bv = other_objs[i]->getAABB(); node->parent = nullptr; node->children[1] = nullptr; @@ -555,35 +475,31 @@ void DynamicAABBTreeCollisionManager::registerObjects( } //============================================================================== -void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) -{ +void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) { DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj); table[obj] = node; } //============================================================================== -void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) -{ +void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) { DynamicAABBNode* node = table[obj]; table.erase(obj); dtree.remove(node); } //============================================================================== -void DynamicAABBTreeCollisionManager::setup() -{ - if(!setup_) - { +void DynamicAABBTreeCollisionManager::setup() { + if (!setup_) { size_t num = dtree.size(); - if(num == 0) - { + if (num == 0) { setup_ = true; return; } size_t height = dtree.getMaxHeight(); - if(((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0)) < max_tree_nonbalanced_level) + if (((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0)) < + max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else dtree.balanceTopdown(); @@ -593,14 +509,12 @@ void DynamicAABBTreeCollisionManager::setup() } //============================================================================== -void DynamicAABBTreeCollisionManager::update() -{ - for(auto it = table.cbegin(); it != table.cend(); ++it) - { +void DynamicAABBTreeCollisionManager::update() { + for (auto it = table.cbegin(); it != table.cend(); ++it) { CollisionObject* obj = it->first; DynamicAABBNode* node = it->second; node->bv = obj->getAABB(); - if(node->bv.volume() <= 0.) + if (node->bv.volume() <= 0.) throw std::invalid_argument("The bounding volume has a negative volume."); } @@ -611,161 +525,154 @@ void DynamicAABBTreeCollisionManager::update() } //============================================================================== -void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) -{ +void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) { const auto it = table.find(updated_obj); - if(it != table.end()) - { + if (it != table.end()) { DynamicAABBNode* node = it->second; - if(!(node->bv == updated_obj->getAABB())) + if (!(node->bv == updated_obj->getAABB())) dtree.update(node, updated_obj->getAABB()); } setup_ = false; } //============================================================================== -void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) -{ +void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) { update_(updated_obj); setup(); } //============================================================================== -void DynamicAABBTreeCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0, size = updated_objs.size(); i < size; ++i) +void DynamicAABBTreeCollisionManager::update( + const std::vector& updated_objs) { + for (size_t i = 0, size = updated_objs.size(); i < size; ++i) update_(updated_objs[i]); setup(); } //============================================================================== -void DynamicAABBTreeCollisionManager::clear() -{ +void DynamicAABBTreeCollisionManager::clear() { dtree.clear(); table.clear(); } //============================================================================== -void DynamicAABBTreeCollisionManager::getObjects(std::vector& objs) const -{ +void DynamicAABBTreeCollisionManager::getObjects( + std::vector& objs) const { objs.resize(this->size()); - std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); + std::transform( + table.begin(), table.end(), objs.begin(), + std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } //============================================================================== -void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +void DynamicAABBTreeCollisionManager::collide( + CollisionObject* obj, CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; - switch(obj->collisionGeometry()->getNodeType()) - { + if (size() == 0) return; + switch (obj->collisionGeometry()->getNodeType()) { #if HPP_FCL_HAVE_OCTOMAP - case GEOM_OCTREE: - { - if(!octree_as_geometry_collide) - { - const OcTree* octree = static_cast(obj->collisionGeometry().get()); - detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), callback); - } - else - detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, callback); - } - break; + case GEOM_OCTREE: { + if (!octree_as_geometry_collide) { + const OcTree* octree = + static_cast(obj->collisionGeometry().get()); + detail::dynamic_AABB_tree::collisionRecurse( + dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), + obj->getTransform(), callback); + } else + detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, + callback); + } break; #endif - default: - detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, callback); + default: + detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, + callback); } } //============================================================================== -void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, - DistanceCallBackBase * callback) const -{ +void DynamicAABBTreeCollisionManager::distance( + CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); - switch(obj->collisionGeometry()->getNodeType()) - { + switch (obj->collisionGeometry()->getNodeType()) { #if HPP_FCL_HAVE_OCTOMAP - case GEOM_OCTREE: - { - if(!octree_as_geometry_distance) - { - const OcTree* octree = static_cast(obj->collisionGeometry().get()); - detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), callback, min_dist); - } - else - detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, callback, min_dist); - } - break; + case GEOM_OCTREE: { + if (!octree_as_geometry_distance) { + const OcTree* octree = + static_cast(obj->collisionGeometry().get()); + detail::dynamic_AABB_tree::distanceRecurse( + dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), + obj->getTransform(), callback, min_dist); + } else + detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, + callback, min_dist); + } break; #endif - default: - detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, callback, min_dist); + default: + detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, callback, + min_dist); } } //============================================================================== -void DynamicAABBTreeCollisionManager::collide(CollisionCallBackBase * callback) const -{ +void DynamicAABBTreeCollisionManager::collide( + CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; detail::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), callback); } //============================================================================== -void DynamicAABBTreeCollisionManager::distance(DistanceCallBackBase * callback) const -{ +void DynamicAABBTreeCollisionManager::distance( + DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); - detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback, min_dist); + detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback, + min_dist); } //============================================================================== -void DynamicAABBTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, - CollisionCallBackBase * callback) const -{ +void DynamicAABBTreeCollisionManager::collide( + BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const { callback->init(); - DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; - detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), callback); + DynamicAABBTreeCollisionManager* other_manager = + static_cast(other_manager_); + if ((size() == 0) || (other_manager->size() == 0)) return; + detail::dynamic_AABB_tree::collisionRecurse( + dtree.getRoot(), other_manager->dtree.getRoot(), callback); } //============================================================================== -void DynamicAABBTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, - DistanceCallBackBase * callback) const -{ +void DynamicAABBTreeCollisionManager::distance( + BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const { callback->init(); - DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + DynamicAABBTreeCollisionManager* other_manager = + static_cast(other_manager_); + if ((size() == 0) || (other_manager->size() == 0)) return; FCL_REAL min_dist = (std::numeric_limits::max)(); - detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist); + detail::dynamic_AABB_tree::distanceRecurse( + dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist); } //============================================================================== -bool DynamicAABBTreeCollisionManager::empty() const -{ - return dtree.empty(); -} +bool DynamicAABBTreeCollisionManager::empty() const { return dtree.empty(); } //============================================================================== -size_t DynamicAABBTreeCollisionManager::size() const -{ - return dtree.size(); -} +size_t DynamicAABBTreeCollisionManager::size() const { return dtree.size(); } //============================================================================== -const detail::HierarchyTree& -DynamicAABBTreeCollisionManager::getTree() const -{ +const detail::HierarchyTree& DynamicAABBTreeCollisionManager::getTree() + const { return dtree; } -detail::HierarchyTree& -DynamicAABBTreeCollisionManager::getTree() -{ +detail::HierarchyTree& DynamicAABBTreeCollisionManager::getTree() { return dtree; } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 78f4435d5..279bd1d5d 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -40,42 +40,31 @@ #if HPP_FCL_HAVE_OCTOMAP #include "hpp/fcl/octree.h" #endif -namespace hpp -{ -namespace fcl -{ -namespace detail -{ -namespace dynamic_AABB_tree_array -{ +namespace hpp { +namespace fcl { +namespace detail { +namespace dynamic_AABB_tree_array { #if HPP_FCL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, - size_t root1_id, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Transform3f& tf2, - CollisionCallBackBase * callback) -{ - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; - if(!root2) - { - if(root1->isLeaf()) - { + size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Transform3f& tf2, + CollisionCallBackBase* callback) { + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = + nodes1 + root1_id; + if (!root2) { + if (root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); - if(!obj1->collisionGeometry()->isFree()) - { + if (!obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3f::Identity(), obb1); convertBV(root2_bv, tf2, obb2); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); @@ -86,28 +75,24 @@ bool collisionRecurse_( return (*callback)(obj1, &obj2); } } - } - else - { - if(collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, root2_bv, tf2, callback)) + } else { + if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, + root2_bv, tf2, callback)) return true; - if(collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, root2_bv, tf2, callback)) + if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, + root2_bv, tf2, callback)) return true; } return false; - } - else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { + } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); - if(!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) - { + if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3f::Identity(), obb1); convertBV(root2_bv, tf2, obb2); - if(obb1.overlap(obb2)) - { + if (obb1.overlap(obb2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); @@ -117,43 +102,41 @@ bool collisionRecurse_( CollisionObject obj2(shared_ptr(box), box_tf); return (*callback)(obj1, &obj2); - } - else return false; - } - else return false; + } else + return false; + } else + return false; } OBB obb1, obb2; convertBV(root1->bv, Transform3f::Identity(), obb1); convertBV(root2_bv, tf2, obb2); - if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; + if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, callback)) + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { + if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + tf2, callback)) return true; - if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, callback)) + if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + tf2, callback)) return true; - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, callback)) + if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, + callback)) return true; - } - else - { + } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, tf2, callback)) + if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, tf2, + callback)) return true; } } @@ -163,74 +146,63 @@ bool collisionRecurse_( } //============================================================================== -bool distanceRecurse_(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, - size_t root1_id, - const OcTree* tree2, - const OcTree::OcTreeNode* root2, - const AABB& root2_bv, - const Transform3f& tf2, - DistanceCallBackBase * callback, - FCL_REAL& min_dist) -{ - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; - if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - if(tree2->isNodeOccupied(root2)) - { +bool distanceRecurse_( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, + size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Transform3f& tf2, + DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = + nodes1 + root1_id; + if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { + if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); - return (*callback)(static_cast(root1->data), &obj, min_dist); - } - else return false; + return (*callback)(static_cast(root1->data), &obj, + min_dist); + } else + return false; } - if(!tree2->isNodeOccupied(root2)) return false; + if (!tree2->isNodeOccupied(root2)) return false; - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { + if (!tree2->nodeHasChildren(root2) || + (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { AABB aabb2; convertBV(root2_bv, tf2, aabb2); FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + tf2, callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + tf2, callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, + tf2, callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, + tf2, callback, min_dist)) return true; } } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { + } else { + for (unsigned int i = 0; i < 8; ++i) { + if (tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -239,9 +211,9 @@ bool distanceRecurse_(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nod convertBV(child_bv, tf2, aabb2); FCL_REAL d = root1->bv.distance(aabb2); - if(d < min_dist) - { - if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, callback, min_dist)) + if (d < min_dist) { + if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, + callback, min_dist)) return true; } } @@ -251,162 +223,158 @@ bool distanceRecurse_(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nod return false; } - #endif //============================================================================== -bool collisionRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, size_t root2_id, - CollisionCallBackBase * callback) -{ - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = nodes2 + root2_id; - if(root1->isLeaf() && root2->isLeaf()) - { - if(!root1->bv.overlap(root2->bv)) return false; - return (*callback)(static_cast(root1->data), static_cast(root2->data)); +bool collisionRecurse( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, + size_t root1_id, + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, + size_t root2_id, CollisionCallBackBase* callback) { + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = + nodes1 + root1_id; + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = + nodes2 + root2_id; + if (root1->isLeaf() && root2->isLeaf()) { + if (!root1->bv.overlap(root2->bv)) return false; + return (*callback)(static_cast(root1->data), + static_cast(root2->data)); } - if(!root1->bv.overlap(root2->bv)) return false; + if (!root1->bv.overlap(root2->bv)) return false; - if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) - { - if(collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, callback)) + if (root2->isLeaf() || + (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { + if (collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, + callback)) return true; - if(collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, callback)) + if (collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, + callback)) return true; - } - else - { - if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], callback)) + } else { + if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], + callback)) return true; - if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], callback)) + if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], + callback)) return true; } return false; } //============================================================================== -inline HPP_FCL_DLLAPI -bool collisionRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, CollisionCallBackBase * callback) -{ +inline HPP_FCL_DLLAPI bool collisionRecurse( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, + size_t root_id, CollisionObject* query, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; - if(!root->bv.overlap(query->getAABB())) return false; + if (!root->bv.overlap(query->getAABB())) return false; - if(root->isLeaf()) - { + if (root->isLeaf()) { return (*callback)(static_cast(root->data), query); } - size_t select_res = implementation_array::select(query->getAABB(), root->children[0], root->children[1], nodes); + size_t select_res = implementation_array::select( + query->getAABB(), root->children[0], root->children[1], nodes); - if(collisionRecurse(nodes, root->children[select_res], query, callback)) + if (collisionRecurse(nodes, root->children[select_res], query, callback)) return true; - if(collisionRecurse(nodes, root->children[1-select_res], query, callback)) + if (collisionRecurse(nodes, root->children[1 - select_res], query, callback)) return true; return false; } //============================================================================== -bool selfCollisionRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, - size_t root_id, - CollisionCallBackBase * callback) -{ +bool selfCollisionRecurse( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, + size_t root_id, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; - if(root->isLeaf()) return false; + if (root->isLeaf()) return false; - if(selfCollisionRecurse(nodes, root->children[0], callback)) - return true; + if (selfCollisionRecurse(nodes, root->children[0], callback)) return true; - if(selfCollisionRecurse(nodes, root->children[1], callback)) - return true; + if (selfCollisionRecurse(nodes, root->children[1], callback)) return true; - if(collisionRecurse(nodes, root->children[0], nodes, root->children[1], callback)) + if (collisionRecurse(nodes, root->children[0], nodes, root->children[1], + callback)) return true; return false; } //============================================================================== -bool distanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, size_t root2_id, - DistanceCallBackBase * callback, FCL_REAL& min_dist) -{ - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; - DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = nodes2 + root2_id; - if(root1->isLeaf() && root2->isLeaf()) - { +bool distanceRecurse( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, + size_t root1_id, + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, + size_t root2_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = + nodes1 + root1_id; + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = + nodes2 + root2_id; + if (root1->isLeaf() && root2->isLeaf()) { CollisionObject* root1_obj = static_cast(root1->data); CollisionObject* root2_obj = static_cast(root2->data); return (*callback)(root1_obj, root2_obj, min_dist); } - if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) - { + if (root2->isLeaf() || + (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { FCL_REAL d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); FCL_REAL d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, + callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, + callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, + callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, + callback, min_dist)) return true; } } - } - else - { + } else { FCL_REAL d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); FCL_REAL d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], + callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], + callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], + callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], + callback, min_dist)) return true; } } @@ -416,11 +384,12 @@ bool distanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* node } //============================================================================== -bool distanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, DistanceCallBackBase * callback, FCL_REAL& min_dist) -{ +bool distanceRecurse( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, + size_t root_id, CollisionObject* query, DistanceCallBackBase* callback, + FCL_REAL& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; - if(root->isLeaf()) - { + if (root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return (*callback)(root_obj, query, min_dist); } @@ -428,31 +397,24 @@ bool distanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* node FCL_REAL d1 = query->getAABB().distance((nodes + root->children[0])->bv); FCL_REAL d2 = query->getAABB().distance((nodes + root->children[1])->bv); - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(nodes, root->children[1], query, callback, min_dist)) + if (d2 < d1) { + if (d2 < min_dist) { + if (distanceRecurse(nodes, root->children[1], query, callback, min_dist)) return true; } - if(d1 < min_dist) - { - if(distanceRecurse(nodes, root->children[0], query, callback, min_dist)) + if (d1 < min_dist) { + if (distanceRecurse(nodes, root->children[0], query, callback, min_dist)) return true; } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(nodes, root->children[0], query, callback, min_dist)) + } else { + if (d1 < min_dist) { + if (distanceRecurse(nodes, root->children[0], query, callback, min_dist)) return true; } - if(d2 < min_dist) - { - if(distanceRecurse(nodes, root->children[1], query, callback, min_dist)) + if (d2 < min_dist) { + if (distanceRecurse(nodes, root->children[1], query, callback, min_dist)) return true; } } @@ -461,53 +423,63 @@ bool distanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* node } //============================================================================== -bool selfDistanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, DistanceCallBackBase * callback, FCL_REAL& min_dist) -{ +bool selfDistanceRecurse( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, + size_t root_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; - if(root->isLeaf()) return false; + if (root->isLeaf()) return false; - if(selfDistanceRecurse(nodes, root->children[0], callback, min_dist)) + if (selfDistanceRecurse(nodes, root->children[0], callback, min_dist)) return true; - if(selfDistanceRecurse(nodes, root->children[1], callback, min_dist)) + if (selfDistanceRecurse(nodes, root->children[1], callback, min_dist)) return true; - if(distanceRecurse(nodes, root->children[0], nodes, root->children[1], callback, min_dist)) + if (distanceRecurse(nodes, root->children[0], nodes, root->children[1], + callback, min_dist)) return true; return false; } - #if HPP_FCL_HAVE_OCTOMAP //============================================================================== -bool collisionRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, CollisionCallBackBase * callback) -{ - if(tf2.rotation().isIdentity()) - return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), callback); +bool collisionRecurse( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, + size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Transform3f& tf2, + CollisionCallBackBase* callback) { + if (tf2.rotation().isIdentity()) + return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, + tf2.translation(), callback); else - return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, callback); + return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, + callback); } //============================================================================== -bool distanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, DistanceCallBackBase * callback, FCL_REAL& min_dist) -{ - if(tf2.rotation().isIdentity()) - return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), callback, min_dist); +bool distanceRecurse( + DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, + size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, + const AABB& root2_bv, const Transform3f& tf2, + DistanceCallBackBase* callback, FCL_REAL& min_dist) { + if (tf2.rotation().isIdentity()) + return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, + tf2.translation(), callback, min_dist); else - return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, callback, min_dist); + return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, + callback, min_dist); } #endif -} // namespace dynamic_AABB_tree_array +} // namespace dynamic_AABB_tree_array -} // namespace detail +} // namespace detail //============================================================================== -DynamicAABBTreeArrayCollisionManager::DynamicAABBTreeArrayCollisionManager() -{ +DynamicAABBTreeArrayCollisionManager::DynamicAABBTreeArrayCollisionManager() { tree_topdown_balance_threshold = &dtree.bu_threshold; tree_topdown_level = &dtree.topdown_level; max_tree_nonbalanced_level = 10; @@ -524,20 +496,15 @@ DynamicAABBTreeArrayCollisionManager::DynamicAABBTreeArrayCollisionManager() //============================================================================== void DynamicAABBTreeArrayCollisionManager::registerObjects( - const std::vector& other_objs) -{ - if(other_objs.empty()) return; + const std::vector& other_objs) { + if (other_objs.empty()) return; - if(size() > 0) - { + if (size() > 0) { BroadPhaseCollisionManager::registerObjects(other_objs); - } - else - { + } else { DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()]; table.rehash(other_objs.size()); - for(size_t i = 0, size = other_objs.size(); i < size; ++i) - { + for (size_t i = 0, size = other_objs.size(); i < size; ++i) { leaves[i].bv = other_objs[i]->getAABB(); leaves[i].parent = dtree.NULL_NODE; leaves[i].children[1] = dtree.NULL_NODE; @@ -554,36 +521,33 @@ void DynamicAABBTreeArrayCollisionManager::registerObjects( } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::registerObject(CollisionObject* obj) -{ +void DynamicAABBTreeArrayCollisionManager::registerObject( + CollisionObject* obj) { size_t node = dtree.insert(obj->getAABB(), obj); table[obj] = node; } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::unregisterObject(CollisionObject* obj) -{ +void DynamicAABBTreeArrayCollisionManager::unregisterObject( + CollisionObject* obj) { size_t node = table[obj]; table.erase(obj); dtree.remove(node); } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::setup() -{ - if(!setup_) - { +void DynamicAABBTreeArrayCollisionManager::setup() { + if (!setup_) { int num = (int)dtree.size(); - if(num == 0) - { + if (num == 0) { setup_ = true; return; } int height = (int)dtree.getMaxHeight(); - - if((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level) + if ((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0) < + max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else dtree.balanceTopdown(); @@ -593,10 +557,8 @@ void DynamicAABBTreeArrayCollisionManager::setup() } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::update() -{ - for(auto it = table.cbegin(), end = table.cend(); it != end; ++it) - { +void DynamicAABBTreeArrayCollisionManager::update() { + for (auto it = table.cbegin(), end = table.cend(); it != end; ++it) { const CollisionObject* obj = it->first; size_t node = it->second; dtree.getNodes()[node].bv = obj->getAABB(); @@ -609,152 +571,159 @@ void DynamicAABBTreeArrayCollisionManager::update() } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::update_(CollisionObject* updated_obj) -{ +void DynamicAABBTreeArrayCollisionManager::update_( + CollisionObject* updated_obj) { const auto it = table.find(updated_obj); - if(it != table.end()) - { + if (it != table.end()) { size_t node = it->second; - if(!(dtree.getNodes()[node].bv == updated_obj->getAABB())) + if (!(dtree.getNodes()[node].bv == updated_obj->getAABB())) dtree.update(node, updated_obj->getAABB()); } setup_ = false; } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::update(CollisionObject* updated_obj) -{ +void DynamicAABBTreeArrayCollisionManager::update( + CollisionObject* updated_obj) { update_(updated_obj); setup(); } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0, size = updated_objs.size(); i < size; ++i) +void DynamicAABBTreeArrayCollisionManager::update( + const std::vector& updated_objs) { + for (size_t i = 0, size = updated_objs.size(); i < size; ++i) update_(updated_objs[i]); setup(); } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::clear() -{ +void DynamicAABBTreeArrayCollisionManager::clear() { dtree.clear(); table.clear(); } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::getObjects(std::vector& objs) const -{ +void DynamicAABBTreeArrayCollisionManager::getObjects( + std::vector& objs) const { objs.resize(this->size()); - std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); + std::transform( + table.begin(), table.end(), objs.begin(), + std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +void DynamicAABBTreeArrayCollisionManager::collide( + CollisionObject* obj, CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; - switch(obj->collisionGeometry()->getNodeType()) - { + if (size() == 0) return; + switch (obj->collisionGeometry()->getNodeType()) { #if HPP_FCL_HAVE_OCTOMAP - case GEOM_OCTREE: - { - if(!octree_as_geometry_collide) - { - const OcTree* octree = static_cast(obj->collisionGeometry().get()); - detail::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), callback); - } - else - detail::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, callback); - } - break; + case GEOM_OCTREE: { + if (!octree_as_geometry_collide) { + const OcTree* octree = + static_cast(obj->collisionGeometry().get()); + detail::dynamic_AABB_tree_array::collisionRecurse( + dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), + octree->getRootBV(), obj->getTransform(), callback); + } else + detail::dynamic_AABB_tree_array::collisionRecurse( + dtree.getNodes(), dtree.getRoot(), obj, callback); + } break; #endif - default: - detail::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, callback); + default: + detail::dynamic_AABB_tree_array::collisionRecurse( + dtree.getNodes(), dtree.getRoot(), obj, callback); } } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase * callback) const -{ +void DynamicAABBTreeArrayCollisionManager::distance( + CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); - switch(obj->collisionGeometry()->getNodeType()) - { + switch (obj->collisionGeometry()->getNodeType()) { #if HPP_FCL_HAVE_OCTOMAP - case GEOM_OCTREE: - { - if(!octree_as_geometry_distance) - { - const OcTree* octree = static_cast(obj->collisionGeometry().get()); - detail::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), callback, min_dist); - } - else - detail::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist); - } - break; + case GEOM_OCTREE: { + if (!octree_as_geometry_distance) { + const OcTree* octree = + static_cast(obj->collisionGeometry().get()); + detail::dynamic_AABB_tree_array::distanceRecurse( + dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), + octree->getRootBV(), obj->getTransform(), callback, min_dist); + } else + detail::dynamic_AABB_tree_array::distanceRecurse( + dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist); + } break; #endif - default: - detail::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist); + default: + detail::dynamic_AABB_tree_array::distanceRecurse( + dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist); } } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::collide(CollisionCallBackBase * callback) const -{ +void DynamicAABBTreeArrayCollisionManager::collide( + CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; - detail::dynamic_AABB_tree_array::selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), callback); + if (size() == 0) return; + detail::dynamic_AABB_tree_array::selfCollisionRecurse( + dtree.getNodes(), dtree.getRoot(), callback); } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::distance(DistanceCallBackBase * callback) const -{ +void DynamicAABBTreeArrayCollisionManager::distance( + DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); - detail::dynamic_AABB_tree_array::selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), callback, min_dist); + detail::dynamic_AABB_tree_array::selfDistanceRecurse( + dtree.getNodes(), dtree.getRoot(), callback, min_dist); } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase * callback) const -{ +void DynamicAABBTreeArrayCollisionManager::collide( + BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const { callback->init(); - DynamicAABBTreeArrayCollisionManager* other_manager = static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; - detail::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), callback); + DynamicAABBTreeArrayCollisionManager* other_manager = + static_cast(other_manager_); + if ((size() == 0) || (other_manager->size() == 0)) return; + detail::dynamic_AABB_tree_array::collisionRecurse( + dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), + other_manager->dtree.getRoot(), callback); } //============================================================================== -void DynamicAABBTreeArrayCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase * callback) const -{ +void DynamicAABBTreeArrayCollisionManager::distance( + BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const { callback->init(); - DynamicAABBTreeArrayCollisionManager* other_manager = static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + DynamicAABBTreeArrayCollisionManager* other_manager = + static_cast(other_manager_); + if ((size() == 0) || (other_manager->size() == 0)) return; FCL_REAL min_dist = (std::numeric_limits::max)(); - detail::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), callback, min_dist); + detail::dynamic_AABB_tree_array::distanceRecurse( + dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), + other_manager->dtree.getRoot(), callback, min_dist); } //============================================================================== -bool DynamicAABBTreeArrayCollisionManager::empty() const -{ +bool DynamicAABBTreeArrayCollisionManager::empty() const { return dtree.empty(); } //============================================================================== -size_t DynamicAABBTreeArrayCollisionManager::size() const -{ +size_t DynamicAABBTreeArrayCollisionManager::size() const { return dtree.size(); } //============================================================================== const detail::implementation_array::HierarchyTree& -DynamicAABBTreeArrayCollisionManager::getTree() const -{ +DynamicAABBTreeArrayCollisionManager::getTree() const { return dtree; } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index 0a31390ba..aff1c6d25 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -31,20 +31,17 @@ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. - */ + */ /** @author Jia Pan */ #include "hpp/fcl/broadphase/broadphase_interval_tree.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { //============================================================================== -void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) -{ +void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) { // must sorted before setup(); @@ -54,26 +51,22 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) p.value = obj->getAABB().max_[0]; auto end1 = std::upper_bound(start1, endpoints[0].end(), p); - if(start1 < end1) - { + if (start1 < end1) { size_t start_id = (size_t)(start1 - endpoints[0].begin()); size_t end_id = (size_t)(end1 - endpoints[0].begin()); size_t cur_id = (size_t)(start_id); - - for(size_t i = start_id; i < end_id; ++i) - { - if(endpoints[0][(size_t)i].obj != obj) - { - if(i == cur_id) cur_id++; - else - { + + for (size_t i = start_id; i < end_id; ++i) { + if (endpoints[0][(size_t)i].obj != obj) { + if (i == cur_id) + cur_id++; + else { endpoints[0][(size_t)cur_id] = endpoints[0][(size_t)i]; cur_id++; } } } - if(cur_id < end_id) - endpoints[0].resize(endpoints[0].size() - 2); + if (cur_id < end_id) endpoints[0].resize(endpoints[0].size() - 2); } p.value = obj->getAABB().min_[1]; @@ -81,59 +74,49 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) p.value = obj->getAABB().max_[1]; auto end2 = std::upper_bound(start2, endpoints[1].end(), p); - if(start2 < end2) - { + if (start2 < end2) { size_t start_id = (size_t)(start2 - endpoints[1].begin()); size_t end_id = (size_t)(end2 - endpoints[1].begin()); size_t cur_id = (size_t)(start_id); - - for(size_t i = start_id; i < end_id; ++i) - { - if(endpoints[1][i].obj != obj) - { - if(i == cur_id) cur_id++; - else - { + + for (size_t i = start_id; i < end_id; ++i) { + if (endpoints[1][i].obj != obj) { + if (i == cur_id) + cur_id++; + else { endpoints[1][cur_id] = endpoints[1][i]; cur_id++; } } } - if(cur_id < end_id) - endpoints[1].resize(endpoints[1].size() - 2); + if (cur_id < end_id) endpoints[1].resize(endpoints[1].size() - 2); } - p.value = obj->getAABB().min_[2]; auto start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p); p.value = obj->getAABB().max_[2]; auto end3 = std::upper_bound(start3, endpoints[2].end(), p); - if(start3 < end3) - { + if (start3 < end3) { size_t start_id = (size_t)(start3 - endpoints[2].begin()); size_t end_id = (size_t)(end3 - endpoints[2].begin()); size_t cur_id = (size_t)(start_id); - - for(size_t i = start_id; i < end_id; ++i) - { - if(endpoints[2][i].obj != obj) - { - if(i == cur_id) cur_id++; - else - { + + for (size_t i = start_id; i < end_id; ++i) { + if (endpoints[2][i].obj != obj) { + if (i == cur_id) + cur_id++; + else { endpoints[2][cur_id] = endpoints[2][i]; cur_id++; } } } - if(cur_id < end_id) - endpoints[2].resize(endpoints[2].size() - 2); + if (cur_id < end_id) endpoints[2].resize(endpoints[2].size() - 2); } // update the interval tree - if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) - { + if (obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) { SAPInterval* ivl1 = obj_interval_maps[0][obj]; SAPInterval* ivl2 = obj_interval_maps[1][obj]; SAPInterval* ivl3 = obj_interval_maps[2][obj]; @@ -153,21 +136,15 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) } //============================================================================== -IntervalTreeCollisionManager::IntervalTreeCollisionManager() : setup_(false) -{ - for(int i = 0; i < 3; ++i) - interval_trees[i] = nullptr; +IntervalTreeCollisionManager::IntervalTreeCollisionManager() : setup_(false) { + for (int i = 0; i < 3; ++i) interval_trees[i] = nullptr; } //============================================================================== -IntervalTreeCollisionManager::~IntervalTreeCollisionManager() -{ - clear(); -} +IntervalTreeCollisionManager::~IntervalTreeCollisionManager() { clear(); } //============================================================================== -void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) -{ +void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) { EndPoint p, q; p.obj = obj; @@ -192,29 +169,26 @@ void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) } //============================================================================== -void IntervalTreeCollisionManager::setup() -{ - if(!setup_) - { +void IntervalTreeCollisionManager::setup() { + if (!setup_) { std::sort(endpoints[0].begin(), endpoints[0].end()); std::sort(endpoints[1].begin(), endpoints[1].end()); std::sort(endpoints[2].begin(), endpoints[2].end()); - for(int i = 0; i < 3; ++i) - delete interval_trees[i]; + for (int i = 0; i < 3; ++i) delete interval_trees[i]; - for(int i = 0; i < 3; ++i) - interval_trees[i] = new detail::IntervalTree; + for (int i = 0; i < 3; ++i) interval_trees[i] = new detail::IntervalTree; - for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) - { + for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { EndPoint p = endpoints[0][i]; CollisionObject* obj = p.obj; - if(p.minmax == 0) - { - SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj); - SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj); - SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj); + if (p.minmax == 0) { + SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], + obj->getAABB().max_[0], obj); + SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], + obj->getAABB().max_[1], obj); + SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], + obj->getAABB().max_[2], obj); interval_trees[0]->insert(ivl1); interval_trees[1]->insert(ivl2); @@ -231,45 +205,38 @@ void IntervalTreeCollisionManager::setup() } //============================================================================== -void IntervalTreeCollisionManager::update() -{ +void IntervalTreeCollisionManager::update() { setup_ = false; - for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) - { - if(endpoints[0][i].minmax == 0) + for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { + if (endpoints[0][i].minmax == 0) endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0]; else endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0]; } - for(size_t i = 0, size = endpoints[1].size(); i < size; ++i) - { - if(endpoints[1][i].minmax == 0) + for (size_t i = 0, size = endpoints[1].size(); i < size; ++i) { + if (endpoints[1][i].minmax == 0) endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1]; else endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1]; } - for(size_t i = 0, size = endpoints[2].size(); i < size; ++i) - { - if(endpoints[2][i].minmax == 0) + for (size_t i = 0, size = endpoints[2].size(); i < size; ++i) { + if (endpoints[2][i].minmax == 0) endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2]; else endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2]; } setup(); - } //============================================================================== -void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) -{ +void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) { AABB old_aabb; const AABB& new_aabb = updated_obj->getAABB(); - for(int i = 0; i < 3; ++i) - { + for (int i = 0; i < 3; ++i) { const auto it = obj_interval_maps[i].find(updated_obj); interval_trees[i]->deleteNode(it->second); old_aabb.min_[i] = it->second->low; @@ -281,14 +248,11 @@ void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) EndPoint dummy; typename std::vector::iterator it; - for(int i = 0; i < 3; ++i) - { + for (int i = 0; i < 3; ++i) { dummy.value = old_aabb.min_[i]; it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); - for(; it != endpoints[i].end(); ++it) - { - if(it->obj == updated_obj && it->minmax == 0) - { + for (; it != endpoints[i].end(); ++it) { + if (it->obj == updated_obj && it->minmax == 0) { it->value = new_aabb.min_[i]; break; } @@ -296,10 +260,8 @@ void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) dummy.value = old_aabb.max_[i]; it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); - for(; it != endpoints[i].end(); ++it) - { - if(it->obj == updated_obj && it->minmax == 0) - { + for (; it != endpoints[i].end(); ++it) { + if (it->obj == updated_obj && it->minmax == 0) { it->value = new_aabb.max_[i]; break; } @@ -310,124 +272,118 @@ void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) } //============================================================================== -void IntervalTreeCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0; i < updated_objs.size(); ++i) - update(updated_objs[i]); +void IntervalTreeCollisionManager::update( + const std::vector& updated_objs) { + for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); } //============================================================================== -void IntervalTreeCollisionManager::clear() -{ +void IntervalTreeCollisionManager::clear() { endpoints[0].clear(); endpoints[1].clear(); endpoints[2].clear(); - delete interval_trees[0]; interval_trees[0] = nullptr; - delete interval_trees[1]; interval_trees[1] = nullptr; - delete interval_trees[2]; interval_trees[2] = nullptr; - - for(int i = 0; i < 3; ++i) - { - for(auto it = obj_interval_maps[i].cbegin(), end = obj_interval_maps[i].cend(); - it != end; ++it) - { + delete interval_trees[0]; + interval_trees[0] = nullptr; + delete interval_trees[1]; + interval_trees[1] = nullptr; + delete interval_trees[2]; + interval_trees[2] = nullptr; + + for (int i = 0; i < 3; ++i) { + for (auto it = obj_interval_maps[i].cbegin(), + end = obj_interval_maps[i].cend(); + it != end; ++it) { delete it->second; } } - for(int i = 0; i < 3; ++i) - obj_interval_maps[i].clear(); + for (int i = 0; i < 3; ++i) obj_interval_maps[i].clear(); setup_ = false; } //============================================================================== -void IntervalTreeCollisionManager::getObjects(std::vector& objs) const -{ +void IntervalTreeCollisionManager::getObjects( + std::vector& objs) const { objs.resize(endpoints[0].size() / 2); size_t j = 0; - for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) - { - if(endpoints[0][i].minmax == 0) - { - objs[j] = endpoints[0][i].obj; j++; + for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { + if (endpoints[0][i].minmax == 0) { + objs[j] = endpoints[0][i].obj; + j++; } } } //============================================================================== -void IntervalTreeCollisionManager::collide(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +void IntervalTreeCollisionManager::collide( + CollisionObject* obj, CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; collide_(obj, callback); } //============================================================================== -bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, CollisionCallBackBase * callback) const -{ +bool IntervalTreeCollisionManager::collide_( + CollisionObject* obj, CollisionCallBackBase* callback) const { static const unsigned int CUTOFF = 100; std::deque results0, results1, results2; - results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]); - if(results0.size() > CUTOFF) - { - results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]); - if(results1.size() > CUTOFF) - { - results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]); - if(results2.size() > CUTOFF) - { + results0 = + interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]); + if (results0.size() > CUTOFF) { + results1 = interval_trees[1]->query(obj->getAABB().min_[1], + obj->getAABB().max_[1]); + if (results1.size() > CUTOFF) { + results2 = interval_trees[2]->query(obj->getAABB().min_[2], + obj->getAABB().max_[2]); + if (results2.size() > CUTOFF) { size_t d1 = results0.size(); size_t d2 = results1.size(); size_t d3 = results2.size(); - if(d1 >= d2 && d1 >= d3) + if (d1 >= d2 && d1 >= d3) return checkColl(results0.begin(), results0.end(), obj, callback); - else if(d2 >= d1 && d2 >= d3) + else if (d2 >= d1 && d2 >= d3) return checkColl(results1.begin(), results1.end(), obj, callback); else return checkColl(results2.begin(), results2.end(), obj, callback); - } - else + } else return checkColl(results2.begin(), results2.end(), obj, callback); - } - else + } else return checkColl(results1.begin(), results1.end(), obj, callback); - } - else + } else return checkColl(results0.begin(), results0.end(), obj, callback); } //============================================================================== -void IntervalTreeCollisionManager::distance(CollisionObject* obj, DistanceCallBackBase * callback) const -{ +void IntervalTreeCollisionManager::distance( + CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== -bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase * callback, FCL_REAL& min_dist) const -{ +bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, + DistanceCallBackBase* callback, + FCL_REAL& min_dist) const { static const unsigned int CUTOFF = 100; - Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); - if(min_dist < (std::numeric_limits::max)()) - { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + if (min_dist < (std::numeric_limits::max)()) { + Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } int status = 1; FCL_REAL old_min_distance; - while(1) - { + while (1) { bool dist_res = false; old_min_distance = min_dist; @@ -435,62 +391,56 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, DistanceCallB std::deque results0, results1, results2; results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]); - if(results0.size() > CUTOFF) - { + if (results0.size() > CUTOFF) { results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]); - if(results1.size() > CUTOFF) - { + if (results1.size() > CUTOFF) { results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]); - if(results2.size() > CUTOFF) - { + if (results2.size() > CUTOFF) { size_t d1 = results0.size(); size_t d2 = results1.size(); size_t d3 = results2.size(); - if(d1 >= d2 && d1 >= d3) - dist_res = checkDist(results0.begin(), results0.end(), obj, callback, min_dist); - else if(d2 >= d1 && d2 >= d3) - dist_res = checkDist(results1.begin(), results1.end(), obj, callback, min_dist); + if (d1 >= d2 && d1 >= d3) + dist_res = checkDist(results0.begin(), results0.end(), obj, + callback, min_dist); + else if (d2 >= d1 && d2 >= d3) + dist_res = checkDist(results1.begin(), results1.end(), obj, + callback, min_dist); else - dist_res = checkDist(results2.begin(), results2.end(), obj, callback, min_dist); - } - else - dist_res = checkDist(results2.begin(), results2.end(), obj, callback, min_dist); - } - else - dist_res = checkDist(results1.begin(), results1.end(), obj, callback, min_dist); - } - else - dist_res = checkDist(results0.begin(), results0.end(), obj, callback, min_dist); - - if(dist_res) return true; + dist_res = checkDist(results2.begin(), results2.end(), obj, + callback, min_dist); + } else + dist_res = checkDist(results2.begin(), results2.end(), obj, callback, + min_dist); + } else + dist_res = checkDist(results1.begin(), results1.end(), obj, callback, + min_dist); + } else + dist_res = + checkDist(results0.begin(), results0.end(), obj, callback, min_dist); + + if (dist_res) return true; results0.clear(); results1.clear(); results2.clear(); - if(status == 1) - { - if(old_min_distance < (std::numeric_limits::max)()) + if (status == 1) { + if (old_min_distance < (std::numeric_limits::max)()) break; - else - { - if(min_dist < old_min_distance) - { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + else { + if (min_dist < old_min_distance) { + Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; - } - else - { - if(aabb == obj->getAABB()) + } else { + if (aabb == obj->getAABB()) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } - } - else if(status == 0) + } else if (status == 0) break; } @@ -498,34 +448,31 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, DistanceCallB } //============================================================================== -void IntervalTreeCollisionManager::collide(CollisionCallBackBase * callback) const -{ +void IntervalTreeCollisionManager::collide( + CollisionCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; std::set active; std::set > overlap; size_t n = endpoints[0].size(); - FCL_REAL diff_x = endpoints[0][0].value - endpoints[0][n-1].value; - FCL_REAL diff_y = endpoints[1][0].value - endpoints[1][n-1].value; - FCL_REAL diff_z = endpoints[2][0].value - endpoints[2][n-1].value; + FCL_REAL diff_x = endpoints[0][0].value - endpoints[0][n - 1].value; + FCL_REAL diff_y = endpoints[1][0].value - endpoints[1][n - 1].value; + FCL_REAL diff_z = endpoints[2][0].value - endpoints[2][n - 1].value; int axis = 0; - if(diff_y > diff_x && diff_y > diff_z) + if (diff_y > diff_x && diff_y > diff_z) axis = 1; - else if(diff_z > diff_y && diff_z > diff_x) + else if (diff_z > diff_y && diff_z > diff_x) axis = 2; - for(unsigned int i = 0; i < n; ++i) - { + for (unsigned int i = 0; i < n; ++i) { const EndPoint& endpoint = endpoints[axis][i]; CollisionObject* index = endpoint.obj; - if(endpoint.minmax == 0) - { + if (endpoint.minmax == 0) { auto iter = active.begin(); auto end = active.end(); - for(; iter != end; ++iter) - { + for (; iter != end; ++iter) { CollisionObject* active_index = *iter; const AABB& b0 = active_index->getAABB(); const AABB& b1 = index->getAABB(); @@ -533,109 +480,103 @@ void IntervalTreeCollisionManager::collide(CollisionCallBackBase * callback) con int axis2 = (axis + 1) % 3; int axis3 = (axis + 2) % 3; - if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) - { - std::pair >::iterator, bool> insert_res; - if(active_index < index) + if (b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) { + std::pair >::iterator, + bool> + insert_res; + if (active_index < index) insert_res = overlap.insert(std::make_pair(active_index, index)); else insert_res = overlap.insert(std::make_pair(index, active_index)); - if(insert_res.second) - { - if((*callback)(active_index, index)) - return; + if (insert_res.second) { + if ((*callback)(active_index, index)) return; } } } active.insert(index); - } - else + } else active.erase(index); } - } //============================================================================== -void IntervalTreeCollisionManager::distance(DistanceCallBackBase * callback) const -{ +void IntervalTreeCollisionManager::distance( + DistanceCallBackBase* callback) const { callback->init(); - if(size() == 0) return; + if (size() == 0) return; this->enable_tested_set_ = true; this->tested_set.clear(); FCL_REAL min_dist = (std::numeric_limits::max)(); - for(size_t i = 0; i < endpoints[0].size(); ++i) - if(distance_(endpoints[0][i].obj, callback, min_dist)) break; + for (size_t i = 0; i < endpoints[0].size(); ++i) + if (distance_(endpoints[0][i].obj, callback, min_dist)) break; this->enable_tested_set_ = false; this->tested_set.clear(); } //============================================================================== -void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, CollisionCallBackBase * callback) const -{ +void IntervalTreeCollisionManager::collide( + BroadPhaseCollisionManager* other_manager_, + CollisionCallBackBase* callback) const { callback->init(); - IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); + IntervalTreeCollisionManager* other_manager = + static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { collide(callback); return; } - if(this->size() < other_manager->size()) - { - for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) - if(other_manager->collide_(endpoints[0][i].obj, callback)) return; - } - else - { - for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) - if(collide_(other_manager->endpoints[0][i].obj, callback)) return; + if (this->size() < other_manager->size()) { + for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) + if (other_manager->collide_(endpoints[0][i].obj, callback)) return; + } else { + for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) + if (collide_(other_manager->endpoints[0][i].obj, callback)) return; } } //============================================================================== -void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, DistanceCallBackBase * callback) const -{ +void IntervalTreeCollisionManager::distance( + BroadPhaseCollisionManager* other_manager_, + DistanceCallBackBase* callback) const { callback->init(); - IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); + IntervalTreeCollisionManager* other_manager = + static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; + if ((size() == 0) || (other_manager->size() == 0)) return; - if(this == other_manager) - { + if (this == other_manager) { distance(callback); return; } FCL_REAL min_dist = (std::numeric_limits::max)(); - if(this->size() < other_manager->size()) - { - for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) - if(other_manager->distance_(endpoints[0][i].obj, callback, min_dist)) return; - } - else - { - for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) - if(distance_(other_manager->endpoints[0][i].obj, callback, min_dist)) return; + if (this->size() < other_manager->size()) { + for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) + if (other_manager->distance_(endpoints[0][i].obj, callback, min_dist)) + return; + } else { + for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) + if (distance_(other_manager->endpoints[0][i].obj, callback, min_dist)) + return; } } //============================================================================== -bool IntervalTreeCollisionManager::empty() const -{ +bool IntervalTreeCollisionManager::empty() const { return endpoints[0].empty(); } //============================================================================== -size_t IntervalTreeCollisionManager::size() const -{ +size_t IntervalTreeCollisionManager::size() const { return endpoints[0].size() / 2; } @@ -643,18 +584,12 @@ size_t IntervalTreeCollisionManager::size() const bool IntervalTreeCollisionManager::checkColl( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, - CollisionObject* obj, - CollisionCallBackBase * callback) const -{ - while(pos_start < pos_end) - { + CollisionObject* obj, CollisionCallBackBase* callback) const { + while (pos_start < pos_end) { SAPInterval* ivl = static_cast(*pos_start); - if(ivl->obj != obj) - { - if(ivl->obj->getAABB().overlap(obj->getAABB())) - { - if((*callback)(ivl->obj, obj)) - return true; + if (ivl->obj != obj) { + if (ivl->obj->getAABB().overlap(obj->getAABB())) { + if ((*callback)(ivl->obj, obj)) return true; } } @@ -668,31 +603,19 @@ bool IntervalTreeCollisionManager::checkColl( bool IntervalTreeCollisionManager::checkDist( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, - CollisionObject* obj, - DistanceCallBackBase * callback, - FCL_REAL& min_dist) const -{ - while(pos_start < pos_end) - { + CollisionObject* obj, DistanceCallBackBase* callback, + FCL_REAL& min_dist) const { + while (pos_start < pos_end) { SAPInterval* ivl = static_cast(*pos_start); - if(ivl->obj != obj) - { - if(!this->enable_tested_set_) - { - if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) - { - if((*callback)(ivl->obj, obj, min_dist)) - return true; + if (ivl->obj != obj) { + if (!this->enable_tested_set_) { + if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) { + if ((*callback)(ivl->obj, obj, min_dist)) return true; } - } - else - { - if(!this->inTestedSet(ivl->obj, obj)) - { - if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) - { - if((*callback)(ivl->obj, obj, min_dist)) - return true; + } else { + if (!this->inTestedSet(ivl->obj, obj)) { + if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) { + if ((*callback)(ivl->obj, obj, min_dist)) return true; } this->insertTestedSet(ivl->obj, obj); @@ -708,20 +631,19 @@ bool IntervalTreeCollisionManager::checkDist( //============================================================================== bool IntervalTreeCollisionManager::EndPoint::operator<( - const typename IntervalTreeCollisionManager::EndPoint& p) const -{ + const typename IntervalTreeCollisionManager::EndPoint& p) const { return value < p.value; } //============================================================================== -IntervalTreeCollisionManager::SAPInterval::SAPInterval( - FCL_REAL low_, FCL_REAL high_, CollisionObject* obj_) - : detail::SimpleInterval() -{ +IntervalTreeCollisionManager::SAPInterval::SAPInterval(FCL_REAL low_, + FCL_REAL high_, + CollisionObject* obj_) + : detail::SimpleInterval() { this->low = low_; this->high = high_; obj = obj_; } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/broadphase/default_broadphase_callbacks.cpp b/src/broadphase/default_broadphase_callbacks.cpp index a07ad6e07..d76cd28ed 100644 --- a/src/broadphase/default_broadphase_callbacks.cpp +++ b/src/broadphase/default_broadphase_callbacks.cpp @@ -40,35 +40,32 @@ namespace hpp { namespace fcl { -bool defaultCollisionFunction(CollisionObject* o1, - CollisionObject* o2, - void* data) -{ +bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, + void* data) { assert(data != nullptr); auto* collision_data = static_cast(data); const CollisionRequest& request = collision_data->request; CollisionResult& result = collision_data->result; - if(collision_data->done) return true; + if (collision_data->done) return true; collide(o1, o2, request, result); - if (result.isCollision() && result.numContacts() >= request.num_max_contacts) - { + if (result.isCollision() && + result.numContacts() >= request.num_max_contacts) { collision_data->done = true; } return collision_data->done; } -bool CollisionCallBackDefault::collide(CollisionObject* o1, CollisionObject* o2) -{ - return defaultCollisionFunction(o1,o2,&data); +bool CollisionCallBackDefault::collide(CollisionObject* o1, + CollisionObject* o2) { + return defaultCollisionFunction(o1, o2, &data); } -bool defaultDistanceFunction(CollisionObject* o1, - CollisionObject* o2, - void* data, FCL_REAL & dist) { +bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, + void* data, FCL_REAL& dist) { assert(data != nullptr); auto* cdata = static_cast(data); const DistanceRequest& request = cdata->request; @@ -88,43 +85,37 @@ bool defaultDistanceFunction(CollisionObject* o1, return cdata->done; } -bool DistanceCallBackDefault::distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL & dist) -{ - return defaultDistanceFunction(o1,o2,&data,dist); +bool DistanceCallBackDefault::distance(CollisionObject* o1, CollisionObject* o2, + FCL_REAL& dist) { + return defaultDistanceFunction(o1, o2, &data, dist); } CollisionCallBackCollect::CollisionCallBackCollect(const size_t max_size) -: max_size(max_size) -{ + : max_size(max_size) { collision_pairs.resize(max_size); } -bool CollisionCallBackCollect::collide(CollisionObject* o1, CollisionObject* o2) -{ - collision_pairs.push_back(std::make_pair(o1,o2)); +bool CollisionCallBackCollect::collide(CollisionObject* o1, + CollisionObject* o2) { + collision_pairs.push_back(std::make_pair(o1, o2)); return false; } -size_t CollisionCallBackCollect::numCollisionPairs() const -{ +size_t CollisionCallBackCollect::numCollisionPairs() const { return collision_pairs.size(); } -const std::vector & -CollisionCallBackCollect::getCollisionPairs() const -{ +const std::vector& +CollisionCallBackCollect::getCollisionPairs() const { return collision_pairs; } -void CollisionCallBackCollect::init() -{ - collision_pairs.clear(); -} +void CollisionCallBackCollect::init() { collision_pairs.clear(); } -bool CollisionCallBackCollect::exist(const CollisionPair & pair) const -{ - return std::find(collision_pairs.begin(), collision_pairs.end(), pair) != collision_pairs.end(); +bool CollisionCallBackCollect::exist(const CollisionPair& pair) const { + return std::find(collision_pairs.begin(), collision_pairs.end(), pair) != + collision_pairs.end(); } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/broadphase/detail/interval_tree.cpp b/src/broadphase/detail/interval_tree.cpp index ab81cae35..7d5f3ebcd 100644 --- a/src/broadphase/detail/interval_tree.cpp +++ b/src/broadphase/detail/interval_tree.cpp @@ -42,61 +42,55 @@ #include - namespace hpp { namespace fcl { namespace detail { //============================================================================== -IntervalTree::IntervalTree() -{ +IntervalTree::IntervalTree() { nil = new IntervalTreeNode; nil->left = nil->right = nil->parent = nil; nil->red = false; - nil->key = nil->high = nil->max_high = -(std::numeric_limits::max)(); + nil->key = nil->high = nil->max_high = + -(std::numeric_limits::max)(); nil->stored_interval = nullptr; root = new IntervalTreeNode; root->parent = root->left = root->right = nil; - root->key = root->high = root->max_high = (std::numeric_limits::max)(); + root->key = root->high = root->max_high = + (std::numeric_limits::max)(); root->red = false; root->stored_interval = nullptr; /// the following are used for the query function recursion_node_stack_size = 128; - recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size*sizeof(it_recursion_node)); + recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size * + sizeof(it_recursion_node)); recursion_node_stack_top = 1; recursion_node_stack[0].start_node = nullptr; } //============================================================================== -IntervalTree::~IntervalTree() -{ +IntervalTree::~IntervalTree() { IntervalTreeNode* x = root->left; std::deque nodes_to_free; - if(x != nil) - { - if(x->left != nil) - { + if (x != nil) { + if (x->left != nil) { nodes_to_free.push_back(x->left); } - if(x->right != nil) - { + if (x->right != nil) { nodes_to_free.push_back(x->right); } delete x; - while( nodes_to_free.size() > 0) - { + while (nodes_to_free.size() > 0) { x = nodes_to_free.back(); nodes_to_free.pop_back(); - if(x->left != nil) - { + if (x->left != nil) { nodes_to_free.push_back(x->left); } - if(x->right != nil) - { + if (x->right != nil) { nodes_to_free.push_back(x->right); } delete x; @@ -108,18 +102,17 @@ IntervalTree::~IntervalTree() } //============================================================================== -void IntervalTree::leftRotate(IntervalTreeNode* x) -{ +void IntervalTree::leftRotate(IntervalTreeNode* x) { IntervalTreeNode* y; y = x->right; x->right = y->left; - if(y->left != nil) y->left->parent = x; + if (y->left != nil) y->left->parent = x; y->parent = x->parent; - if(x == x->parent->left) + if (x == x->parent->left) x->parent->left = y; else x->parent->right = y; @@ -127,22 +120,22 @@ void IntervalTree::leftRotate(IntervalTreeNode* x) y->left = x; x->parent = y; - x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high)); + x->max_high = + std::max(x->left->max_high, std::max(x->right->max_high, x->high)); y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high)); } //============================================================================== -void IntervalTree::rightRotate(IntervalTreeNode* y) -{ +void IntervalTree::rightRotate(IntervalTreeNode* y) { IntervalTreeNode* x; x = y->left; y->left = x->right; - if(nil != x->right) x->right->parent = y; + if (nil != x->right) x->right->parent = y; x->parent = y->parent; - if(y == y->parent->left) + if (y == y->parent->left) y->parent->left = x; else y->parent->right = x; @@ -150,47 +143,44 @@ void IntervalTree::rightRotate(IntervalTreeNode* y) x->right = y; y->parent = x; - y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high)); + y->max_high = + std::max(y->left->max_high, std::max(y->right->max_high, y->high)); x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high)); } //============================================================================== -void IntervalTree::recursiveInsert(IntervalTreeNode* z) -{ +void IntervalTree::recursiveInsert(IntervalTreeNode* z) { IntervalTreeNode* x; IntervalTreeNode* y; z->left = z->right = nil; y = root; x = root->left; - while(x != nil) - { + while (x != nil) { y = x; - if(x->key > z->key) + if (x->key > z->key) x = x->left; else x = x->right; } z->parent = y; - if((y == root) || (y->key > z->key)) + if ((y == root) || (y->key > z->key)) y->left = z; else y->right = z; } //============================================================================== -void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) -{ - while(x != root) - { - x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high)); +void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) { + while (x != root) { + x->max_high = + std::max(x->high, std::max(x->left->max_high, x->right->max_high)); x = x->parent; } } //============================================================================== -IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) -{ +IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) { IntervalTreeNode* y; IntervalTreeNode* x; IntervalTreeNode* new_node; @@ -200,23 +190,17 @@ IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) fixupMaxHigh(x->parent); new_node = x; x->red = true; - while(x->parent->red) - { + while (x->parent->red) { /// use sentinel instead of checking for root - if(x->parent == x->parent->parent->left) - { + if (x->parent == x->parent->parent->left) { y = x->parent->parent->right; - if(y->red) - { + if (y->red) { x->parent->red = true; y->red = true; x->parent->parent->red = true; x = x->parent->parent; - } - else - { - if(x == x->parent->right) - { + } else { + if (x == x->parent->right) { x = x->parent; leftRotate(x); } @@ -224,21 +208,15 @@ IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) x->parent->parent->red = true; rightRotate(x->parent->parent); } - } - else - { + } else { y = x->parent->parent->left; - if(y->red) - { + if (y->red) { x->parent->red = false; y->red = false; x->parent->parent->red = true; x = x->parent->parent; - } - else - { - if(x == x->parent->left) - { + } else { + if (x == x->parent->left) { x = x->parent; rightRotate(x); } @@ -253,46 +231,34 @@ IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) } //============================================================================== -IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const -{ +IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const { IntervalTreeNode* y; - if(nil != (y = x->right)) - { - while(y->left != nil) - y = y->left; + if (nil != (y = x->right)) { + while (y->left != nil) y = y->left; return y; - } - else - { + } else { y = x->parent; - while(x == y->right) - { + while (x == y->right) { x = y; y = y->parent; } - if(y == root) return nil; + if (y == root) return nil; return y; } } //============================================================================== -IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const -{ +IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const { IntervalTreeNode* y; - if(nil != (y = x->left)) - { - while(y->right != nil) - y = y->right; + if (nil != (y = x->left)) { + while (y->right != nil) y = y->right; return y; - } - else - { + } else { y = x->parent; - while(x == y->left) - { - if(y == root) return nil; + while (x == y->left) { + if (y == root) return nil; x = y; y = y->parent; } @@ -301,49 +267,36 @@ IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const } //============================================================================== -void IntervalTree::recursivePrint(IntervalTreeNode* x) const -{ - if(x != nil) - { +void IntervalTree::recursivePrint(IntervalTreeNode* x) const { + if (x != nil) { recursivePrint(x->left); - x->print(nil,root); + x->print(nil, root); recursivePrint(x->right); } } //============================================================================== -void IntervalTree::print() const -{ - recursivePrint(root->left); -} +void IntervalTree::print() const { recursivePrint(root->left); } //============================================================================== -void IntervalTree::deleteFixup(IntervalTreeNode* x) -{ +void IntervalTree::deleteFixup(IntervalTreeNode* x) { IntervalTreeNode* w; IntervalTreeNode* root_left_node = root->left; - while((!x->red) && (root_left_node != x)) - { - if(x == x->parent->left) - { + while ((!x->red) && (root_left_node != x)) { + if (x == x->parent->left) { w = x->parent->right; - if(w->red) - { + if (w->red) { w->red = false; x->parent->red = true; leftRotate(x->parent); w = x->parent->right; } - if((!w->right->red) && (!w->left->red)) - { + if ((!w->right->red) && (!w->left->red)) { w->red = true; x = x->parent; - } - else - { - if(!w->right->red) - { + } else { + if (!w->right->red) { w->left->red = false; w->red = true; rightRotate(w); @@ -355,26 +308,19 @@ void IntervalTree::deleteFixup(IntervalTreeNode* x) leftRotate(x->parent); x = root_left_node; } - } - else - { + } else { w = x->parent->left; - if(w->red) - { + if (w->red) { w->red = false; x->parent->red = true; rightRotate(x->parent); w = x->parent->left; } - if((!w->right->red) && (!w->left->red)) - { + if ((!w->right->red) && (!w->left->red)) { w->red = true; x = x->parent; - } - else - { - if(!w->left->red) - { + } else { + if (!w->left->red) { w->right->red = false; w->red = true; leftRotate(w); @@ -392,83 +338,67 @@ void IntervalTree::deleteFixup(IntervalTreeNode* x) } //============================================================================== -void IntervalTree::deleteNode(SimpleInterval* ivl) -{ +void IntervalTree::deleteNode(SimpleInterval* ivl) { IntervalTreeNode* node = recursiveSearch(root, ivl); - if(node) - deleteNode(node); + if (node) deleteNode(node); } //============================================================================== -IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const -{ - if(node != nil) - { - if(node->stored_interval == ivl) - return node; +IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, + SimpleInterval* ivl) const { + if (node != nil) { + if (node->stored_interval == ivl) return node; IntervalTreeNode* left = recursiveSearch(node->left, ivl); - if(left != nil) return left; + if (left != nil) return left; IntervalTreeNode* right = recursiveSearch(node->right, ivl); - if(right != nil) return right; + if (right != nil) return right; } return nil; } //============================================================================== -SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) -{ +SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) { IntervalTreeNode* y; IntervalTreeNode* x; SimpleInterval* node_to_delete = z->stored_interval; - y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z); - x= (y->left == nil) ? y->right : y->left; - if(root == (x->parent = y->parent)) - { + y = ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z); + x = (y->left == nil) ? y->right : y->left; + if (root == (x->parent = y->parent)) { root->left = x; - } - else - { - if(y == y->parent->left) - { + } else { + if (y == y->parent->left) { y->parent->left = x; - } - else - { + } else { y->parent->right = x; } } /// @brief y should not be nil in this case /// y is the node to splice out and x is its child - if(y != z) - { + if (y != z) { y->max_high = -(std::numeric_limits::max)(); y->left = z->left; y->right = z->right; y->parent = z->parent; z->left->parent = z->right->parent = y; - if(z == z->parent->left) + if (z == z->parent->left) z->parent->left = y; else z->parent->right = y; fixupMaxHigh(x->parent); - if(!(y->red)) - { + if (!(y->red)) { y->red = z->red; deleteFixup(x); - } - else + } else y->red = z->red; delete z; - } - else - { + } else { fixupMaxHigh(x->parent); - if(!(y->red)) deleteFixup(x); + if (!(y->red)) deleteFixup(x); delete y; } @@ -477,59 +407,50 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) //============================================================================== /// @brief returns 1 if the intervals overlap, and 0 otherwise -bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) -{ - if(a1 <= b1) - { +bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) { + if (a1 <= b1) { return (b1 <= a2); - } - else - { + } else { return (a1 <= b2); } } //============================================================================== -std::deque IntervalTree::query(FCL_REAL low, FCL_REAL high) -{ +std::deque IntervalTree::query(FCL_REAL low, FCL_REAL high) { std::deque result_stack; IntervalTreeNode* x = root->left; bool run = (x != nil); current_parent = 0; - while(run) - { - if(overlap(low,high,x->key,x->high)) - { + while (run) { + if (overlap(low, high, x->key, x->high)) { result_stack.push_back(x->stored_interval); recursion_node_stack[current_parent].try_right_branch = true; } - if(x->left->max_high >= low) - { - if(recursion_node_stack_top == recursion_node_stack_size) - { + if (x->left->max_high >= low) { + if (recursion_node_stack_top == recursion_node_stack_size) { recursion_node_stack_size *= 2; - recursion_node_stack = (it_recursion_node *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node)); - if(recursion_node_stack == nullptr) - abort(); + recursion_node_stack = (it_recursion_node*)realloc( + recursion_node_stack, + recursion_node_stack_size * sizeof(it_recursion_node)); + if (recursion_node_stack == nullptr) abort(); } recursion_node_stack[recursion_node_stack_top].start_node = x; recursion_node_stack[recursion_node_stack_top].try_right_branch = false; - recursion_node_stack[recursion_node_stack_top].parent_index = current_parent; + recursion_node_stack[recursion_node_stack_top].parent_index = + current_parent; current_parent = recursion_node_stack_top++; x = x->left; - } - else + } else x = x->right; run = (x != nil); - while((!run) && (recursion_node_stack_top > 1)) - { - if(recursion_node_stack[--recursion_node_stack_top].try_right_branch) - { - x=recursion_node_stack[recursion_node_stack_top].start_node->right; - current_parent=recursion_node_stack[recursion_node_stack_top].parent_index; + while ((!run) && (recursion_node_stack_top > 1)) { + if (recursion_node_stack[--recursion_node_stack_top].try_right_branch) { + x = recursion_node_stack[recursion_node_stack_top].start_node->right; + current_parent = + recursion_node_stack[recursion_node_stack_top].parent_index; recursion_node_stack[current_parent].try_right_branch = true; run = (x != nil); } @@ -538,8 +459,8 @@ std::deque IntervalTree::query(FCL_REAL low, FCL_REAL high) return result_stack; } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/src/broadphase/detail/interval_tree_node.cpp b/src/broadphase/detail/interval_tree_node.cpp index d6fe96ccc..b9771ae18 100644 --- a/src/broadphase/detail/interval_tree_node.cpp +++ b/src/broadphase/detail/interval_tree_node.cpp @@ -48,44 +48,49 @@ namespace fcl { namespace detail { //============================================================================== -IntervalTreeNode::IntervalTreeNode() -{ +IntervalTreeNode::IntervalTreeNode() { // Do nothing } //============================================================================== IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) - : stored_interval (new_interval), - key(new_interval->low), - high(new_interval->high), - max_high(high) -{ + : stored_interval(new_interval), + key(new_interval->low), + high(new_interval->high), + max_high(high) { // Do nothing } //============================================================================== -IntervalTreeNode::~IntervalTreeNode() -{ +IntervalTreeNode::~IntervalTreeNode() { // Do nothing } //============================================================================== -void IntervalTreeNode::print( - IntervalTreeNode* nil, IntervalTreeNode* root) const -{ +void IntervalTreeNode::print(IntervalTreeNode* nil, + IntervalTreeNode* root) const { stored_interval->print(); std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high; std::cout << " l->key = "; - if(left == nil) std::cout << "nullptr"; else std::cout << left->key; + if (left == nil) + std::cout << "nullptr"; + else + std::cout << left->key; std::cout << " r->key = "; - if(right == nil) std::cout << "nullptr"; else std::cout << right->key; + if (right == nil) + std::cout << "nullptr"; + else + std::cout << right->key; std::cout << " p->key = "; - if(parent == root) std::cout << "nullptr"; else std::cout << parent->key; + if (parent == root) + std::cout << "nullptr"; + else + std::cout << parent->key; std::cout << " red = " << (int)red << std::endl; } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/src/broadphase/detail/morton-inl.h b/src/broadphase/detail/morton-inl.h index 25b261998..53d7d90f7 100644 --- a/src/broadphase/detail/morton-inl.h +++ b/src/broadphase/detail/morton-inl.h @@ -41,33 +41,29 @@ #include "hpp/fcl/broadphase/detail/morton.h" - namespace hpp { -namespace fcl {/// @cond IGNORE +namespace fcl { /// @cond IGNORE namespace detail { //============================================================================== template -uint32_t quantize(S x, uint32_t n) -{ - return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n-1)), uint32_t(0)); +uint32_t quantize(S x, uint32_t n) { + return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0)); } //============================================================================== -template +template morton_functor::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) -{ + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== -template -uint32_t morton_functor::operator()(const Vec3f& point) const -{ +template +uint32_t morton_functor::operator()(const Vec3f& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); @@ -76,20 +72,18 @@ uint32_t morton_functor::operator()(const Vec3f& point) const } //============================================================================== -template +template morton_functor::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) -{ + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== -template -uint64_t morton_functor::operator()(const Vec3f& point) const -{ +template +uint64_t morton_functor::operator()(const Vec3f& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); @@ -98,35 +92,31 @@ uint64_t morton_functor::operator()(const Vec3f& point) const } //============================================================================== -template -constexpr size_t morton_functor::bits() -{ +template +constexpr size_t morton_functor::bits() { return 60; } //============================================================================== -template -constexpr size_t morton_functor::bits() -{ +template +constexpr size_t morton_functor::bits() { return 30; } //============================================================================== -template +template morton_functor>::morton_functor(const AABB& bbox) - : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])) -{ + : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) { // Do nothing } //============================================================================== -template +template std::bitset morton_functor>::operator()( - const Vec3f& point) const -{ + const Vec3f& point) const { S x = (point[0] - base[0]) * inv[0]; S y = (point[1] - base[1]) * inv[1]; S z = (point[2] - base[2]) * inv[2]; @@ -137,29 +127,27 @@ std::bitset morton_functor>::operator()( y *= 2; z *= 2; - for(size_t i = 0; i < bits()/3; ++i) - { + for (size_t i = 0; i < bits() / 3; ++i) { bset[start_bit--] = ((z < 1) ? 0 : 1); bset[start_bit--] = ((y < 1) ? 0 : 1); bset[start_bit--] = ((x < 1) ? 0 : 1); - x = ((x >= 1) ? 2*(x-1) : 2*x); - y = ((y >= 1) ? 2*(y-1) : 2*y); - z = ((z >= 1) ? 2*(z-1) : 2*z); + x = ((x >= 1) ? 2 * (x - 1) : 2 * x); + y = ((y >= 1) ? 2 * (y - 1) : 2 * y); + z = ((z >= 1) ? 2 * (z - 1) : 2 * z); } return bset; } //============================================================================== -template -constexpr size_t morton_functor>::bits() -{ +template +constexpr size_t morton_functor>::bits() { return N; } -} // namespace detail +} // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp #endif diff --git a/src/broadphase/detail/morton.cpp b/src/broadphase/detail/morton.cpp index 896659fc1..54beaa104 100644 --- a/src/broadphase/detail/morton.cpp +++ b/src/broadphase/detail/morton.cpp @@ -38,39 +38,34 @@ #include "hpp/fcl/broadphase/detail/morton-inl.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { /// @cond IGNORE -namespace detail -{ +namespace detail { //============================================================================== -uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z) -{ +uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z) { x = (x | (x << 16)) & 0x030000FF; - x = (x | (x << 8)) & 0x0300F00F; - x = (x | (x << 4)) & 0x030C30C3; - x = (x | (x << 2)) & 0x09249249; + x = (x | (x << 8)) & 0x0300F00F; + x = (x | (x << 4)) & 0x030C30C3; + x = (x | (x << 2)) & 0x09249249; y = (y | (y << 16)) & 0x030000FF; - y = (y | (y << 8)) & 0x0300F00F; - y = (y | (y << 4)) & 0x030C30C3; - y = (y | (y << 2)) & 0x09249249; + y = (y | (y << 8)) & 0x0300F00F; + y = (y | (y << 4)) & 0x030C30C3; + y = (y | (y << 2)) & 0x09249249; z = (z | (z << 16)) & 0x030000FF; - z = (z | (z << 8)) & 0x0300F00F; - z = (z | (z << 4)) & 0x030C30C3; - z = (z | (z << 2)) & 0x09249249; + z = (z | (z << 8)) & 0x0300F00F; + z = (z | (z << 4)) & 0x030C30C3; + z = (z | (z << 2)) & 0x09249249; return x | (y << 1) | (z << 2); } //============================================================================== -uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z) -{ +uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z) { uint32_t lo_x = x & 1023u; uint32_t lo_y = y & 1023u; uint32_t lo_z = z & 1023u; @@ -78,12 +73,11 @@ uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z) uint32_t hi_y = y >> 10u; uint32_t hi_z = z >> 10u; - return - ( uint64_t(morton_code(hi_x, hi_y, hi_z)) << 30) - | uint64_t(morton_code(lo_x, lo_y, lo_z)); + return (uint64_t(morton_code(hi_x, hi_y, hi_z)) << 30) | + uint64_t(morton_code(lo_x, lo_y, lo_z)); } -} // namespace detail +} // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/broadphase/detail/simple_interval.cpp b/src/broadphase/detail/simple_interval.cpp index a9661c5cf..78719156c 100644 --- a/src/broadphase/detail/simple_interval.cpp +++ b/src/broadphase/detail/simple_interval.cpp @@ -46,19 +46,17 @@ namespace fcl { namespace detail { //============================================================================== -SimpleInterval::~SimpleInterval() -{ +SimpleInterval::~SimpleInterval() { // Do nothing } //============================================================================== -void SimpleInterval::print() -{ +void SimpleInterval::print() { // Do nothing } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index a9b29702a..07f7b6631 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -31,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 Jia Pan */ @@ -47,31 +47,36 @@ namespace detail { //============================================================================== SpatialHash::SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) - : cell_size(cell_size_), scene_limit(scene_limit_) -{ - width[0] = static_cast(std::ceil(scene_limit.width() / cell_size)); - width[1] = static_cast(std::ceil(scene_limit.height() / cell_size)); - width[2] = static_cast(std::ceil(scene_limit.depth() / cell_size)); + : cell_size(cell_size_), scene_limit(scene_limit_) { + width[0] = + static_cast(std::ceil(scene_limit.width() / cell_size)); + width[1] = + static_cast(std::ceil(scene_limit.height() / cell_size)); + width[2] = + static_cast(std::ceil(scene_limit.depth() / cell_size)); } //============================================================================== -std::vector SpatialHash::operator()(const AABB& aabb) const -{ - unsigned int min_x = static_cast(std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size)); - unsigned int max_x = static_cast(std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size)); - unsigned int min_y = static_cast(std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size)); - unsigned int max_y = static_cast(std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size)); - unsigned int min_z = static_cast(std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size)); - unsigned int max_z = static_cast(std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size)); +std::vector SpatialHash::operator()(const AABB& aabb) const { + unsigned int min_x = static_cast( + std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size)); + unsigned int max_x = static_cast( + std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size)); + unsigned int min_y = static_cast( + std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size)); + unsigned int max_y = static_cast( + std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size)); + unsigned int min_z = static_cast( + std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size)); + unsigned int max_z = static_cast( + std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size)); - std::vector keys(static_cast((max_x - min_x) * (max_y - min_y) * (max_z - min_z))); + std::vector keys( + static_cast((max_x - min_x) * (max_y - min_y) * (max_z - min_z))); size_t id = 0; - for(unsigned int x = min_x; x < max_x; ++x) - { - for(unsigned int y = min_y; y < max_y; ++y) - { - for(unsigned int z = min_z; z < max_z; ++z) - { + for (unsigned int x = min_x; x < max_x; ++x) { + for (unsigned int y = min_y; y < max_y; ++y) { + for (unsigned int z = min_z; z < max_z; ++z) { keys[id++] = x + y * width[0] + z * width[0] * width[1]; } } @@ -79,8 +84,8 @@ std::vector SpatialHash::operator()(const AABB& aabb) const return keys; } -} // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace detail +} // namespace fcl +} // namespace hpp #endif diff --git a/src/collision.cpp b/src/collision.cpp index 96930f0a5..773a43326 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -35,30 +35,24 @@ /** \author Jia Pan */ - #include #include #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -CollisionFunctionMatrix& getCollisionFunctionLookTable() -{ +CollisionFunctionMatrix& getCollisionFunctionLookTable() { static CollisionFunctionMatrix table; return table; } // reorder collision results in the order the call has been made. -void CollisionResult::swapObjects() -{ - for(std::vector::iterator it = contacts.begin(); - it != contacts.end(); ++it) - { +void CollisionResult::swapObjects() { + for (std::vector::iterator it = contacts.begin(); + it != contacts.end(); ++it) { std::swap(it->o1, it->o2); std::swap(it->b1, it->b2); it->normal *= -1; @@ -66,18 +60,15 @@ void CollisionResult::swapObjects() } std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, - const CollisionRequest& request, CollisionResult& result) -{ - return collide( - o1->collisionGeometry().get(), o1->getTransform(), - o2->collisionGeometry().get(), o2->getTransform(), - request, result); + const CollisionRequest& request, CollisionResult& result) { + return collide(o1->collisionGeometry().get(), o1->getTransform(), + o2->collisionGeometry().get(), o2->getTransform(), request, + result); } std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) -{ + const CollisionRequest& request, CollisionResult& result) { // If securit margin is set to -infinity, return that there is no collision if (request.security_margin == -std::numeric_limits::infinity()) { result.clear(); @@ -92,42 +83,39 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); std::size_t res; - if(request.num_max_contacts == 0) - { - HPP_FCL_THROW_PRETTY("Invalid number of max contacts (current value is 0).",std::invalid_argument); + if (request.num_max_contacts == 0) { + HPP_FCL_THROW_PRETTY("Invalid number of max contacts (current value is 0).", + std::invalid_argument); res = 0; - } - else - { + } else { OBJECT_TYPE object_type1 = o1->getObjectType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); - if(object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) - { - if(!looktable.collision_matrix[node_type2][node_type1]) - { - HPP_FCL_THROW_PRETTY("Collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported.", + if (object_type1 == OT_GEOM && + (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { + if (!looktable.collision_matrix[node_type2][node_type1]) { + HPP_FCL_THROW_PRETTY("Collision function between node type " + << node_type1 << " and node type " + << node_type2 << " is not supported.", std::invalid_argument); res = 0; - } - else - { - res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, &solver, request, result); + } else { + res = looktable.collision_matrix[node_type2][node_type1]( + o2, tf2, o1, tf1, &solver, request, result); result.swapObjects(); } - } - else - { - if(!looktable.collision_matrix[node_type1][node_type2]) - { - HPP_FCL_THROW_PRETTY("Collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported.", + } else { + if (!looktable.collision_matrix[node_type1][node_type2]) { + HPP_FCL_THROW_PRETTY("Collision function between node type " + << node_type1 << " and node type " + << node_type2 << " is not supported.", std::invalid_argument); res = 0; - } - else - res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, &solver, request, result); + } else + res = looktable.collision_matrix[node_type1][node_type2]( + o1, tf1, o2, tf2, &solver, request, result); } } if (solver.enable_cached_guess) { @@ -139,9 +127,8 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, } ComputeCollision::ComputeCollision(const CollisionGeometry* o1, - const CollisionGeometry* o2) - : o1(o1), o2(o2) -{ + const CollisionGeometry* o2) + : o1(o1), o2(o2) { const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); OBJECT_TYPE object_type1 = o1->getObjectType(); @@ -149,14 +136,14 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1, OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type2 = o2->getNodeType(); - swap_geoms = object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD); + swap_geoms = object_type1 == OT_GEOM && + (object_type2 == OT_BVH || object_type2 == OT_HFIELD); - if( ( swap_geoms && !looktable.collision_matrix[node_type2][node_type1]) - || (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2])) - { + if ((swap_geoms && !looktable.collision_matrix[node_type2][node_type1]) || + (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2])) { std::ostringstream oss; - oss << "Warning: collision function between node type " << node_type1 << - " and node type " << node_type2 << " is not supported"; + oss << "Warning: collision function between node type " << node_type1 + << " and node type " << node_type2 << " is not supported"; throw std::invalid_argument(oss.str()); } if (swap_geoms) @@ -165,9 +152,10 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1, func = looktable.collision_matrix[node_type1][node_type2]; } -std::size_t ComputeCollision::run(const Transform3f& tf1, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) const -{ +std::size_t ComputeCollision::run(const Transform3f& tf1, + const Transform3f& tf2, + const CollisionRequest& request, + CollisionResult& result) const { // If securit margin is set to -infinity, return that there is no collision if (request.security_margin == -std::numeric_limits::infinity()) { result.clear(); @@ -178,7 +166,7 @@ std::size_t ComputeCollision::run(const Transform3f& tf1, const Transform3f& tf2 res = func(o2, tf2, o1, tf1, &solver, request, result); result.swapObjects(); } else { - res = func (o1, tf1, o2, tf2, &solver, request, result); + res = func(o1, tf1, o2, tf2, &solver, request, result); } return res; } @@ -195,26 +183,24 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1, solver.cached_guess = request.cached_gjk_guess; solver.support_func_cached_guess = request.cached_support_func_guess; } - + solver.distance_upper_bound = request.distance_upper_bound; - + std::size_t res; - if(request.enable_timings) - { + if (request.enable_timings) { Timer timer; res = run(tf1, tf2, request, result); result.timings = timer.elapsed(); - } - else + } else res = run(tf1, tf2, request, result); - + if (cached) { result.cached_gjk_guess = solver.cached_guess; result.cached_support_func_guess = solver.support_func_cached_guess; } - + return res; } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/collision_data.cpp b/src/collision_data.cpp index 28158ef00..91080fcda 100644 --- a/src/collision_data.cpp +++ b/src/collision_data.cpp @@ -37,21 +37,17 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -bool CollisionRequest::isSatisfied(const CollisionResult& result) const -{ +bool CollisionRequest::isSatisfied(const CollisionResult& result) const { return result.isCollision() && (num_max_contacts <= result.numContacts()); } -bool DistanceRequest::isSatisfied(const DistanceResult& result) const -{ +bool DistanceRequest::isSatisfied(const DistanceResult& result) const { return (result.min_distance <= 0); } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index 2bdc70bf7..02fee42c4 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #include #include @@ -44,22 +43,20 @@ #include #include <../src/traits_traversal.h> -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { #ifdef HPP_FCL_HAS_OCTOMAP -template +template std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); + const GJKSolver* nsolver, const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); - typename TraversalTraitsCollision::CollisionTraversal_t node (request); + typename TraversalTraitsCollision::CollisionTraversal_t node( + request); const TypeA* obj1 = dynamic_cast(o1); const TypeB* obj2 = dynamic_cast(o2); OcTreeSolver otsolver(nsolver); @@ -72,81 +69,80 @@ std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, #endif -template -std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); +template +std::size_t ShapeShapeCollide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); DistanceResult distanceResult; - DistanceRequest distanceRequest (request.enable_contact); - FCL_REAL distance = ShapeShapeDistance - (o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); + DistanceRequest distanceRequest(request.enable_contact); + FCL_REAL distance = ShapeShapeDistance( + o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); size_t num_contacts = 0; if (distance <= 0) { - if (result.numContacts () < request.num_max_contacts) { - const Vec3f& p1 = distanceResult.nearest_points [0]; - const Vec3f& p2 = distanceResult.nearest_points [1]; + if (result.numContacts() < request.num_max_contacts) { + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; - Contact contact (o1, o2, distanceResult.b1, distanceResult.b2, - (p1+p2)/2, - distanceResult.normal, - -distance+request.security_margin); + Contact contact(o1, o2, distanceResult.b1, distanceResult.b2, + (p1 + p2) / 2, distanceResult.normal, + -distance + request.security_margin); - result.addContact (contact); + result.addContact(contact); } num_contacts = result.numContacts(); - } - else if (distance <= request.security_margin) { - if (result.numContacts () < request.num_max_contacts) { - const Vec3f& p1 = distanceResult.nearest_points [0]; - const Vec3f& p2 = distanceResult.nearest_points [1]; - - Contact contact (o1, o2, distanceResult.b1, distanceResult.b2, - .5 * (p1 + p2), - (p2-p1).normalized (), - -distance+request.security_margin); - result.addContact (contact); + } else if (distance <= request.security_margin) { + if (result.numContacts() < request.num_max_contacts) { + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; + + Contact contact(o1, o2, distanceResult.b1, distanceResult.b2, + .5 * (p1 + p2), (p2 - p1).normalized(), + -distance + request.security_margin); + result.addContact(contact); } num_contacts = result.numContacts(); } - result.updateDistanceLowerBound (distance); + result.updateDistanceLowerBound(distance); return num_contacts; } -namespace details -{ - template struct bvh_shape_traits - { - enum { Options = RelativeTransformationIsIdentity }; - }; -#define BVH_SHAPE_DEFAULT_TO_ORIENTED(bv) \ - template struct bvh_shape_traits \ - { enum { Options = 0 }; } - BVH_SHAPE_DEFAULT_TO_ORIENTED(OBB); - BVH_SHAPE_DEFAULT_TO_ORIENTED(RSS); - BVH_SHAPE_DEFAULT_TO_ORIENTED(kIOS); - BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS); +namespace details { +template +struct bvh_shape_traits { + enum { Options = RelativeTransformationIsIdentity }; +}; +#define BVH_SHAPE_DEFAULT_TO_ORIENTED(bv) \ + template \ + struct bvh_shape_traits { \ + enum { Options = 0 }; \ + } +BVH_SHAPE_DEFAULT_TO_ORIENTED(OBB); +BVH_SHAPE_DEFAULT_TO_ORIENTED(RSS); +BVH_SHAPE_DEFAULT_TO_ORIENTED(kIOS); +BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS); #undef BVH_SHAPE_DEFAULT_TO_ORIENTED -} +} // namespace details /// \tparam _Options takes two values. /// - RelativeTransformationIsIdentity if object 1 should be moved /// into the frame of object 2 before computing collisions. /// - 0 if the query should be made with non-aligned object frames. -template::Options> -struct HPP_FCL_LOCAL BVHShapeCollider -{ - static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - if(request.isSatisfied(result)) return result.numContacts(); +template ::Options> +struct HPP_FCL_LOCAL BVHShapeCollider { + static std::size_t collide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); if (_Options & RelativeTransformationIsIdentity) return aligned(o1, tf1, o2, tf2, nsolver, request, result); @@ -154,15 +150,18 @@ struct HPP_FCL_LOCAL BVHShapeCollider return oriented(o1, tf1, o2, tf2, nsolver, request, result); } - static std::size_t aligned(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - if(request.isSatisfied(result)) return result.numContacts(); - - MeshShapeCollisionTraversalNode node (request); - const BVHModel* obj1 = static_cast* >(o1); + static std::size_t aligned(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); + + MeshShapeCollisionTraversalNode + node(request); + const BVHModel* obj1 = static_cast*>(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); @@ -174,22 +173,22 @@ struct HPP_FCL_LOCAL BVHShapeCollider return result.numContacts(); } - static std::size_t oriented(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - if(request.isSatisfied(result)) return result.numContacts(); + static std::size_t oriented(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); - MeshShapeCollisionTraversalNode node (request); - const BVHModel* obj1 = static_cast* >(o1); + MeshShapeCollisionTraversalNode node(request); + const BVHModel* obj1 = static_cast*>(o1); const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); fcl::collide(&node, request, result); return result.numContacts(); } - }; /// @brief Collider functor for HeightField data structure @@ -197,20 +196,20 @@ struct HPP_FCL_LOCAL BVHShapeCollider /// - RelativeTransformationIsIdentity if object 1 should be moved /// into the frame of object 2 before computing collisions. /// - 0 if the query should be made with non-aligned object frames. -template -struct HPP_FCL_LOCAL HeightFieldShapeCollider -{ +template +struct HPP_FCL_LOCAL HeightFieldShapeCollider { typedef HeightField HF; - - static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - if(request.isSatisfied(result)) return result.numContacts(); - - HeightFieldShapeCollisionTraversalNode node (request); - const HF* obj1 = static_cast(o1); + + static std::size_t collide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); + + HeightFieldShapeCollisionTraversalNode node(request); + const HF* obj1 = static_cast(o1); const Shape* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); @@ -219,18 +218,19 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider } }; -namespace details -{ -template -std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); +namespace details { +template +std::size_t orientedMeshCollide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); - OrientedMeshCollisionTraversalNode node (request); - const BVHModel* obj1 = static_cast* >(o1); - const BVHModel* obj2 = static_cast* >(o2); + OrientedMeshCollisionTraversalNode node(request); + const BVHModel* obj1 = static_cast*>(o1); + const BVHModel* obj2 = static_cast*>(o2); initialize(node, *obj1, tf1, *obj2, tf2, result); collide(&node, request, result); @@ -238,23 +238,23 @@ std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& return result.numContacts(); } -} +} // namespace details -template +template std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - MeshCollisionTraversalNode node (request); - const BVHModel* obj1 = static_cast* >(o1); - const BVHModel* obj2 = static_cast* >(o2); + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); + + MeshCollisionTraversalNode node(request); + const BVHModel* obj1 = static_cast*>(o1); + const BVHModel* obj2 = static_cast*>(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); Transform3f tf2_tmp = tf2; - + initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result); fcl::collide(&node, request, result); @@ -264,48 +264,49 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, return result.numContacts(); } -template<> +template <> std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) -{ - return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); + const CollisionRequest& request, + CollisionResult& result) { + return details::orientedMeshCollide( + o1, tf1, o2, tf2, request, result); } -template<> -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) -{ - return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); +template <> +std::size_t BVHCollide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const CollisionRequest& request, + CollisionResult& result) { + return details::orientedMeshCollide( + o1, tf1, o2, tf2, request, result); } - -template<> -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const CollisionRequest& request, CollisionResult& result) -{ - return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); +template <> +std::size_t BVHCollide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const CollisionRequest& request, + CollisionResult& result) { + return details::orientedMeshCollide( + o1, tf1, o2, tf2, request, result); } - -template +template std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* /*nsolver*/, - const CollisionRequest& request, CollisionResult& result) -{ + const CollisionRequest& request, + CollisionResult& result) { return BVHCollide(o1, tf1, o2, tf2, request, result); } - -CollisionFunctionMatrix::CollisionFunctionMatrix() -{ - for(int i = 0; i < NODE_COUNT; ++i) - { - for(int j = 0; j < NODE_COUNT; ++j) - collision_matrix[i][j] = NULL; +CollisionFunctionMatrix::CollisionFunctionMatrix() { + for (int i = 0; i < NODE_COUNT; ++i) { + for (int j = 0; j < NODE_COUNT; ++j) collision_matrix[i][j] = NULL; } collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide; @@ -315,160 +316,271 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide; + collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = + &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide; + collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = + &ShapeShapeCollide; + collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = + &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide; + collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = + &ShapeShapeCollide; + collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = + &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide; + collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = + &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = + &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = + &ShapeShapeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = + &ShapeShapeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = + &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide; + collision_matrix[GEOM_CONE][GEOM_CYLINDER] = + &ShapeShapeCollide; + collision_matrix[GEOM_CONE][GEOM_CONVEX] = + &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide; + collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = + &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_CONE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = + &ShapeShapeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = + &ShapeShapeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = + &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX][GEOM_CONE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = + &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = + &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX][GEOM_PLANE] = + &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = + &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide; + collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = + &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide; + collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = + &ShapeShapeCollide; + collision_matrix[GEOM_PLANE][GEOM_CONVEX] = + &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide; + collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = + &ShapeShapeCollide; + + collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = + &ShapeShapeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = + &ShapeShapeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = + &ShapeShapeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = + &ShapeShapeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = + &ShapeShapeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = + &ShapeShapeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = + &ShapeShapeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = + &ShapeShapeCollide; collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider::collide; + collision_matrix[BV_AABB][GEOM_SPHERE] = + &BVHShapeCollider::collide; + collision_matrix[BV_AABB][GEOM_CAPSULE] = + &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; + collision_matrix[BV_AABB][GEOM_CYLINDER] = + &BVHShapeCollider::collide; + collision_matrix[BV_AABB][GEOM_CONVEX] = + &BVHShapeCollider::collide; + collision_matrix[BV_AABB][GEOM_PLANE] = + &BVHShapeCollider::collide; + collision_matrix[BV_AABB][GEOM_HALFSPACE] = + &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider::collide; + collision_matrix[BV_OBB][GEOM_SPHERE] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBB][GEOM_CAPSULE] = + &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider::collide; + collision_matrix[BV_OBB][GEOM_CYLINDER] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBB][GEOM_CONVEX] = + &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; + collision_matrix[BV_OBB][GEOM_HALFSPACE] = + &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider::collide; + collision_matrix[BV_RSS][GEOM_SPHERE] = + &BVHShapeCollider::collide; + collision_matrix[BV_RSS][GEOM_CAPSULE] = + &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider::collide; + collision_matrix[BV_RSS][GEOM_CYLINDER] = + &BVHShapeCollider::collide; + collision_matrix[BV_RSS][GEOM_CONVEX] = + &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; - - collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider, Box>::collide; - collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; - collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider, Capsule>::collide; - collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider, Cone>::collide; - collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder>::collide; - collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider, ConvexBase>::collide; - collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; - collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; - - collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider, Box>::collide; - collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; - collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider, Capsule>::collide; - collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider, Cone>::collide; - collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder>::collide; - collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider, ConvexBase>::collide; - collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; - collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; - - collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider, Box>::collide; - collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; - collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider, Capsule>::collide; - collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider, Cone>::collide; - collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder>::collide; - collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider, ConvexBase>::collide; - collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; - collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; + collision_matrix[BV_RSS][GEOM_HALFSPACE] = + &BVHShapeCollider::collide; + + collision_matrix[BV_KDOP16][GEOM_BOX] = + &BVHShapeCollider, Box>::collide; + collision_matrix[BV_KDOP16][GEOM_SPHERE] = + &BVHShapeCollider, Sphere>::collide; + collision_matrix[BV_KDOP16][GEOM_CAPSULE] = + &BVHShapeCollider, Capsule>::collide; + collision_matrix[BV_KDOP16][GEOM_CONE] = + &BVHShapeCollider, Cone>::collide; + collision_matrix[BV_KDOP16][GEOM_CYLINDER] = + &BVHShapeCollider, Cylinder>::collide; + collision_matrix[BV_KDOP16][GEOM_CONVEX] = + &BVHShapeCollider, ConvexBase>::collide; + collision_matrix[BV_KDOP16][GEOM_PLANE] = + &BVHShapeCollider, Plane>::collide; + collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = + &BVHShapeCollider, Halfspace>::collide; + + collision_matrix[BV_KDOP18][GEOM_BOX] = + &BVHShapeCollider, Box>::collide; + collision_matrix[BV_KDOP18][GEOM_SPHERE] = + &BVHShapeCollider, Sphere>::collide; + collision_matrix[BV_KDOP18][GEOM_CAPSULE] = + &BVHShapeCollider, Capsule>::collide; + collision_matrix[BV_KDOP18][GEOM_CONE] = + &BVHShapeCollider, Cone>::collide; + collision_matrix[BV_KDOP18][GEOM_CYLINDER] = + &BVHShapeCollider, Cylinder>::collide; + collision_matrix[BV_KDOP18][GEOM_CONVEX] = + &BVHShapeCollider, ConvexBase>::collide; + collision_matrix[BV_KDOP18][GEOM_PLANE] = + &BVHShapeCollider, Plane>::collide; + collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = + &BVHShapeCollider, Halfspace>::collide; + + collision_matrix[BV_KDOP24][GEOM_BOX] = + &BVHShapeCollider, Box>::collide; + collision_matrix[BV_KDOP24][GEOM_SPHERE] = + &BVHShapeCollider, Sphere>::collide; + collision_matrix[BV_KDOP24][GEOM_CAPSULE] = + &BVHShapeCollider, Capsule>::collide; + collision_matrix[BV_KDOP24][GEOM_CONE] = + &BVHShapeCollider, Cone>::collide; + collision_matrix[BV_KDOP24][GEOM_CYLINDER] = + &BVHShapeCollider, Cylinder>::collide; + collision_matrix[BV_KDOP24][GEOM_CONVEX] = + &BVHShapeCollider, ConvexBase>::collide; + collision_matrix[BV_KDOP24][GEOM_PLANE] = + &BVHShapeCollider, Plane>::collide; + collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = + &BVHShapeCollider, Halfspace>::collide; collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider::collide; + collision_matrix[BV_kIOS][GEOM_SPHERE] = + &BVHShapeCollider::collide; + collision_matrix[BV_kIOS][GEOM_CAPSULE] = + &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; - - collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; - - collision_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_AABB][GEOM_CAPSULE] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_AABB][GEOM_CONE] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_AABB][GEOM_CYLINDER] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_AABB][GEOM_CONVEX] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeCollider::collide; - - collision_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_OBBRSS][GEOM_CAPSULE] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_OBBRSS][GEOM_CONE] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_OBBRSS][GEOM_CYLINDER] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_OBBRSS][GEOM_CONVEX] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeCollider::collide; - collision_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeCollider::collide; + collision_matrix[BV_kIOS][GEOM_CYLINDER] = + &BVHShapeCollider::collide; + collision_matrix[BV_kIOS][GEOM_CONVEX] = + &BVHShapeCollider::collide; + collision_matrix[BV_kIOS][GEOM_PLANE] = + &BVHShapeCollider::collide; + collision_matrix[BV_kIOS][GEOM_HALFSPACE] = + &BVHShapeCollider::collide; + + collision_matrix[BV_OBBRSS][GEOM_BOX] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBBRSS][GEOM_SPHERE] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBBRSS][GEOM_CONE] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBBRSS][GEOM_CONVEX] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBBRSS][GEOM_PLANE] = + &BVHShapeCollider::collide; + collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = + &BVHShapeCollider::collide; + + collision_matrix[HF_AABB][GEOM_BOX] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_AABB][GEOM_SPHERE] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_AABB][GEOM_CAPSULE] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_AABB][GEOM_CONE] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_AABB][GEOM_CYLINDER] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_AABB][GEOM_CONVEX] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_AABB][GEOM_PLANE] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_AABB][GEOM_HALFSPACE] = + &HeightFieldShapeCollider::collide; + + collision_matrix[HF_OBBRSS][GEOM_BOX] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_OBBRSS][GEOM_SPHERE] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_OBBRSS][GEOM_CAPSULE] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_OBBRSS][GEOM_CONE] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_OBBRSS][GEOM_CYLINDER] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_OBBRSS][GEOM_CONVEX] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_OBBRSS][GEOM_PLANE] = + &HeightFieldShapeCollider::collide; + collision_matrix[HF_OBBRSS][GEOM_HALFSPACE] = + &HeightFieldShapeCollider::collide; collision_matrix[BV_AABB][BV_AABB] = &BVHCollide; collision_matrix[BV_OBB][BV_OBB] = &BVHCollide; @@ -500,26 +612,33 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_OCTREE][BV_AABB ] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_OBB ] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_RSS ] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_kIOS ] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_KDOP16] = &Collide > >; - collision_matrix[GEOM_OCTREE][BV_KDOP18] = &Collide > >; - collision_matrix[GEOM_OCTREE][BV_KDOP24] = &Collide > >; - - collision_matrix[BV_AABB ][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_OBB ][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_RSS ][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_kIOS ][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_KDOP16][GEOM_OCTREE] = &Collide >, OcTree>; - collision_matrix[BV_KDOP18][GEOM_OCTREE] = &Collide >, OcTree>; - collision_matrix[BV_KDOP24][GEOM_OCTREE] = &Collide >, OcTree>; + collision_matrix[GEOM_OCTREE][BV_AABB] = &Collide >; + collision_matrix[GEOM_OCTREE][BV_OBB] = &Collide >; + collision_matrix[GEOM_OCTREE][BV_RSS] = &Collide >; + collision_matrix[GEOM_OCTREE][BV_OBBRSS] = + &Collide >; + collision_matrix[GEOM_OCTREE][BV_kIOS] = &Collide >; + collision_matrix[GEOM_OCTREE][BV_KDOP16] = + &Collide > >; + collision_matrix[GEOM_OCTREE][BV_KDOP18] = + &Collide > >; + collision_matrix[GEOM_OCTREE][BV_KDOP24] = + &Collide > >; + + collision_matrix[BV_AABB][GEOM_OCTREE] = &Collide, OcTree>; + collision_matrix[BV_OBB][GEOM_OCTREE] = &Collide, OcTree>; + collision_matrix[BV_RSS][GEOM_OCTREE] = &Collide, OcTree>; + collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &Collide, OcTree>; + collision_matrix[BV_kIOS][GEOM_OCTREE] = &Collide, OcTree>; + collision_matrix[BV_KDOP16][GEOM_OCTREE] = + &Collide >, OcTree>; + collision_matrix[BV_KDOP18][GEOM_OCTREE] = + &Collide >, OcTree>; + collision_matrix[BV_KDOP24][GEOM_OCTREE] = + &Collide >, OcTree>; #endif } -//template struct CollisionFunctionMatrix; -} +// template struct CollisionFunctionMatrix; +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/collision_node.cpp b/src/collision_node.cpp index 88865f4a4..0cdd1d6b1 100644 --- a/src/collision_node.cpp +++ b/src/collision_node.cpp @@ -35,40 +35,32 @@ /** \author Jia Pan */ - #include <../src/collision_node.h> #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -void collide(CollisionTraversalNodeBase* node, - const CollisionRequest& request, CollisionResult& result, - BVHFrontList* front_list, - bool recursive) -{ - if(front_list && front_list->size() > 0) - { +void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request, + CollisionResult& result, BVHFrontList* front_list, + bool recursive) { + if (front_list && front_list->size() > 0) { propagateBVHFrontListCollisionRecurse(node, request, result, front_list); - } - else - { - FCL_REAL sqrDistLowerBound=0; + } else { + FCL_REAL sqrDistLowerBound = 0; if (recursive) collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound); else collisionNonRecurse(node, front_list, sqrDistLowerBound); - result.updateDistanceLowerBound (sqrt (sqrDistLowerBound)); + result.updateDistanceLowerBound(sqrt(sqrDistLowerBound)); } } -void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, unsigned int qsize) -{ +void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, + unsigned int qsize) { node->preprocess(); - - if(qsize <= 2) + + if (qsize <= 2) distanceRecurse(node, 0, 0, front_list); else distanceQueueRecurse(node, 0, 0, front_list, qsize); @@ -76,6 +68,6 @@ void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, unsigne node->postprocess(); } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/collision_node.h b/src/collision_node.h index 2ea6d749d..8a72be9cf 100644 --- a/src/collision_node.h +++ b/src/collision_node.h @@ -44,32 +44,34 @@ #include #include -/// @brief collision and distance function on traversal nodes. these functions provide a higher level abstraction for collision functions provided in collision_func_matrix -namespace hpp -{ -namespace fcl -{ - +/// @brief collision and distance function on traversal nodes. these functions +/// provide a higher level abstraction for collision functions provided in +/// collision_func_matrix +namespace hpp { +namespace fcl { /// collision on collision traversal node -/// +/// /// @param node node containing both objects to test, /// @retval squared lower bound to the distance between the objects if they /// do not collide. /// @param front_list list of nodes visited by the query, can be used to /// accelerate computation /// \todo should be HPP_FCL_LOCAL but used in unit test. -HPP_FCL_DLLAPI void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request, - CollisionResult& result, BVHFrontList* front_list = NULL, +HPP_FCL_DLLAPI void collide(CollisionTraversalNodeBase* node, + const CollisionRequest& request, + CollisionResult& result, + BVHFrontList* front_list = NULL, bool recursive = true); -/// @brief distance computation on distance traversal node; can use front list to accelerate -/// \todo should be HPP_FCL_LOCAL but used in unit test. +/// @brief distance computation on distance traversal node; can use front list +/// to accelerate \todo should be HPP_FCL_LOCAL but used in unit test. HPP_FCL_DLLAPI void distance(DistanceTraversalNodeBase* node, - BVHFrontList* front_list = NULL, unsigned int qsize = 2); -} + BVHFrontList* front_list = NULL, + unsigned int qsize = 2); +} // namespace fcl -} // namespace hpp +} // namespace hpp /// @endcond diff --git a/src/collision_object.cpp b/src/collision_object.cpp index 0343a6a68..1b0ec1b71 100644 --- a/src/collision_object.cpp +++ b/src/collision_object.cpp @@ -37,13 +37,11 @@ #include -namespace hpp -{ +namespace hpp { namespace fcl { - bool CollisionGeometry::isUncertain() const - { - return !isOccupied() && !isFree(); - } -} // namespace fcl +bool CollisionGeometry::isUncertain() const { + return !isOccupied() && !isFree(); +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp index f98b784a9..b9acd7bcf 100644 --- a/src/collision_utility.cpp +++ b/src/collision_utility.cpp @@ -18,41 +18,52 @@ #include -namespace hpp -{ -namespace fcl -{ - namespace details { - template - inline CollisionGeometry* extractBVHtpl (const CollisionGeometry* model, const Transform3f& pose, const AABB& aabb) - { - const BVHModel* m = static_cast*>(model); - return BVHExtract(*m, pose, aabb); - } - CollisionGeometry* extractBVH (const CollisionGeometry* model, const Transform3f& pose, const AABB& aabb) - { - switch (model->getNodeType()) { - case BV_AABB : return extractBVHtpl(model, pose, aabb); - case BV_OBB : return extractBVHtpl(model, pose, aabb); - case BV_RSS : return extractBVHtpl(model, pose, aabb); - case BV_kIOS : return extractBVHtpl(model, pose, aabb); - case BV_OBBRSS: return extractBVHtpl(model, pose, aabb); - case BV_KDOP16: return extractBVHtpl >(model, pose, aabb); - case BV_KDOP18: return extractBVHtpl >(model, pose, aabb); - case BV_KDOP24: return extractBVHtpl >(model, pose, aabb); - default: throw std::runtime_error("Unknown type of bounding volume"); - } - } +namespace hpp { +namespace fcl { +namespace details { +template +inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, + const Transform3f& pose, + const AABB& aabb) { + const BVHModel* m = static_cast*>(model); + return BVHExtract(*m, pose, aabb); +} +CollisionGeometry* extractBVH(const CollisionGeometry* model, + const Transform3f& pose, const AABB& aabb) { + switch (model->getNodeType()) { + case BV_AABB: + return extractBVHtpl(model, pose, aabb); + case BV_OBB: + return extractBVHtpl(model, pose, aabb); + case BV_RSS: + return extractBVHtpl(model, pose, aabb); + case BV_kIOS: + return extractBVHtpl(model, pose, aabb); + case BV_OBBRSS: + return extractBVHtpl(model, pose, aabb); + case BV_KDOP16: + return extractBVHtpl >(model, pose, aabb); + case BV_KDOP18: + return extractBVHtpl >(model, pose, aabb); + case BV_KDOP24: + return extractBVHtpl >(model, pose, aabb); + default: + throw std::runtime_error("Unknown type of bounding volume"); } +} +} // namespace details - CollisionGeometry* extract(const CollisionGeometry* model, const Transform3f& pose, const AABB& aabb) - { - switch (model->getObjectType()) { - case OT_BVH: return details::extractBVH (model, pose, aabb); - // case OT_GEOM: return model; - default: throw std::runtime_error("Extraction is not implemented for this type of object"); - } +CollisionGeometry* extract(const CollisionGeometry* model, + const Transform3f& pose, const AABB& aabb) { + switch (model->getObjectType()) { + case OT_BVH: + return details::extractBVH(model, pose, aabb); + // case OT_GEOM: return model; + default: + throw std::runtime_error( + "Extraction is not implemented for this type of object"); } } +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance.cpp b/src/distance.cpp index a180160fd..3cd4bb8d7 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -41,29 +41,24 @@ #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -DistanceFunctionMatrix& getDistanceFunctionLookTable() -{ +DistanceFunctionMatrix& getDistanceFunctionLookTable() { static DistanceFunctionMatrix table; return table; } -FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result) -{ - return distance( - o1->collisionGeometry().get(), o1->getTransform(), - o2->collisionGeometry().get(), o2->getTransform(), - request, result); +FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, + const DistanceRequest& request, DistanceResult& result) { + return distance(o1->collisionGeometry().get(), o1->getTransform(), + o2->collisionGeometry().get(), o2->getTransform(), request, + result); } FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) -{ + const DistanceRequest& request, DistanceResult& result) { GJKSolver solver; solver.enable_cached_guess = request.enable_cached_gjk_guess; if (solver.enable_cached_guess) { @@ -80,35 +75,33 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, FCL_REAL res = (std::numeric_limits::max)(); - if(object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) - { - if(!looktable.distance_matrix[node_type2][node_type1]) - { - std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl; - } - else - { - res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, &solver, request, result); + if (object_type1 == OT_GEOM && + (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { + if (!looktable.distance_matrix[node_type2][node_type1]) { + std::cerr << "Warning: distance function between node type " << node_type1 + << " and node type " << node_type2 << " is not supported" + << std::endl; + } else { + res = looktable.distance_matrix[node_type2][node_type1]( + o2, tf2, o1, tf1, &solver, request, result); // If closest points are requested, switch object 1 and 2 if (request.enable_nearest_points) { - const CollisionGeometry *tmpo = result.o1; - result.o1 = result.o2; - result.o2 = tmpo; - Vec3f tmpn (result.nearest_points [0]); - result.nearest_points [0] = result.nearest_points [1]; - result.nearest_points [1] = tmpn; + const CollisionGeometry* tmpo = result.o1; + result.o1 = result.o2; + result.o2 = tmpo; + Vec3f tmpn(result.nearest_points[0]); + result.nearest_points[0] = result.nearest_points[1]; + result.nearest_points[1] = tmpn; } } - } - else - { - if(!looktable.distance_matrix[node_type1][node_type2]) - { - std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl; - } - else - { - res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, &solver, request, result); + } else { + if (!looktable.distance_matrix[node_type1][node_type2]) { + std::cerr << "Warning: distance function between node type " << node_type1 + << " and node type " << node_type2 << " is not supported" + << std::endl; + } else { + res = looktable.distance_matrix[node_type1][node_type2]( + o1, tf1, o2, tf2, &solver, request, result); } } if (solver.enable_cached_guess) { @@ -120,9 +113,8 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, } ComputeDistance::ComputeDistance(const CollisionGeometry* o1, - const CollisionGeometry* o2) - : o1(o1), o2(o2) -{ + const CollisionGeometry* o2) + : o1(o1), o2(o2) { const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); OBJECT_TYPE object_type1 = o1->getObjectType(); @@ -130,14 +122,14 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1, OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type2 = o2->getNodeType(); - swap_geoms = object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD); + swap_geoms = object_type1 == OT_GEOM && + (object_type2 == OT_BVH || object_type2 == OT_HFIELD); - if( ( swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) - || (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) - { + if ((swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) || + (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) { std::ostringstream oss; - oss << "Warning: distance function between node type " << node_type1 << - " and node type " << node_type2 << " is not supported"; + oss << "Warning: distance function between node type " << node_type1 + << " and node type " << node_type2 << " is not supported"; throw std::invalid_argument(oss.str()); } if (swap_geoms) @@ -146,13 +138,11 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1, func = looktable.distance_matrix[node_type1][node_type2]; } -FCL_REAL ComputeDistance::run(const Transform3f& tf1, - const Transform3f& tf2, +FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request, - DistanceResult& result) const -{ + DistanceResult& result) const { FCL_REAL res; - + if (swap_geoms) { res = func(o2, tf2, o1, tf1, &solver, request, result); if (request.enable_nearest_points) { @@ -160,17 +150,16 @@ FCL_REAL ComputeDistance::run(const Transform3f& tf1, result.nearest_points[0].swap(result.nearest_points[1]); } } else { - res = func (o1, tf1, o2, tf2, &solver, request, result); + res = func(o1, tf1, o2, tf2, &solver, request, result); } - + return res; } FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request, - DistanceResult& result) const -{ + DistanceResult& result) const { bool cached = request.enable_cached_gjk_guess; solver.enable_cached_guess = cached; if (cached) { @@ -179,13 +168,11 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, } FCL_REAL res; - if(request.enable_timings) - { + if (request.enable_timings) { Timer timer; res = run(tf1, tf2, request, result); result.timings = timer.elapsed(); - } - else + } else res = run(tf1, tf2, request, result); if (cached) { @@ -195,5 +182,5 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, return res; } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp index 4898f2b75..ad17bb7df 100644 --- a/src/distance/box_halfspace.cpp +++ b/src/distance/box_halfspace.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Box& s1 = static_cast (*o1); - const Halfspace& s2 = static_cast (*o2); - details::boxHalfspaceIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Box& s1 = static_cast(*o1); + const Halfspace& s2 = static_cast(*o2); + details::boxHalfspaceIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Halfspace& s1 = static_cast (*o1); - const Box& s2 = static_cast (*o2); - details::boxHalfspaceIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Halfspace& s1 = static_cast(*o1); + const Box& s2 = static_cast(*o2); + details::boxHalfspaceIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp index ca2671de9..5c13a5d9e 100644 --- a/src/distance/box_plane.cpp +++ b/src/distance/box_plane.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Box& s1 = static_cast (*o1); - const Plane& s2 = static_cast (*o2); - details::boxPlaneIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Box& s1 = static_cast(*o1); + const Plane& s2 = static_cast(*o2); + details::boxPlaneIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], result.nearest_points[1], + result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Plane& s1 = static_cast (*o1); - const Box& s2 = static_cast (*o2); - details::boxPlaneIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Plane& s1 = static_cast(*o1); + const Box& s2 = static_cast(*o2); + details::boxPlaneIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], result.nearest_points[0], + result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/box_sphere.cpp b/src/distance/box_sphere.cpp index 9baca12a5..1515c82ce 100644 --- a/src/distance/box_sphere.cpp +++ b/src/distance/box_sphere.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Box& s1 = static_cast (*o1); - const Sphere& s2 = static_cast (*o2); - details::boxSphereDistance - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Box& s1 = static_cast(*o1); + const Sphere& s2 = static_cast(*o2); + details::boxSphereDistance(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], result.nearest_points[1], + result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Sphere& s1 = static_cast (*o1); - const Box& s2 = static_cast (*o2); - details::boxSphereDistance - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Sphere& s1 = static_cast(*o1); + const Box& s2 = static_cast(*o2); + details::boxSphereDistance(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], result.nearest_points[0], + result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp index 0b385e7c4..af126ce73 100644 --- a/src/distance/capsule_capsule.cpp +++ b/src/distance/capsule_capsule.cpp @@ -45,141 +45,130 @@ // // One solution would be to make narrow phase solvers derive from an abstract // class and specialize the template for this abstract class. -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; - - /// Clamp num / denom in [0, 1] - FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) - { - assert(denom >= 0.); - if (num <= 0. ) return 0.; - else if (num >= denom) return 1.; - else return num / denom; - } - - /// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd - void clamped_linear(Vec3f& a_sd, const Vec3f& a, - const FCL_REAL& s_n, const FCL_REAL& s_d, const Vec3f& d) - { - assert(s_d >= 0.); - if (s_n <= 0. ) a_sd = a ; - else if (s_n >= s_d) a_sd = a+ d; - else a_sd = a+s_n/s_d*d; - } - - - // Compute the distance between C1 and C2 by computing the distance - // between the two segments supporting the capsules. - // Match algorithm of Real-Time Collision Detection, Christer Ericson - Closest Point of Two Line Segments - template <> - FCL_REAL ShapeShapeDistance (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest& request, - DistanceResult& result) - { - const Capsule* capsule1 = static_cast (o1); - const Capsule* capsule2 = static_cast (o2); - - FCL_REAL EPSILON = std::numeric_limits::epsilon () * 100; - - // We assume that capsules are centered at the origin. - const fcl::Vec3f& c1 = tf1.getTranslation (); - const fcl::Vec3f& c2 = tf2.getTranslation (); - // We assume that capsules are oriented along z-axis. - FCL_REAL halfLength1 = capsule1->halfLength; - FCL_REAL halfLength2 = capsule2->halfLength; - FCL_REAL radius1 = capsule1->radius; - FCL_REAL radius2 = capsule2->radius; - // direction of capsules - // ||d1|| = 2 * halfLength1 - const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2); - const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2); - - // Starting point of the segments - // p1 + d1 is the end point of the segment - const fcl::Vec3f p1 = c1 - d1 / 2; - const fcl::Vec3f p2 = c2 - d2 / 2; - const fcl::Vec3f r = p1-p2; - FCL_REAL a = d1.dot(d1); - FCL_REAL b = d1.dot(d2); - FCL_REAL c = d1.dot(r); - FCL_REAL e = d2.dot(d2); - FCL_REAL f = d2.dot(r); - // S1 is parametrized by the equation p1 + s * d1 - // S2 is parametrized by the equation p2 + t * d2 - - Vec3f w1, w2; - if (a <= EPSILON) { - w1 = p1; - if (e <= EPSILON) - // Check if the segments degenerate into points - w2 = p2; - else - // First segment is degenerated - clamped_linear(w2, p2, f, e, d2); - } - else if (e <= EPSILON) - { - // Second segment is degenerated - clamped_linear(w1, p1, -c, a, d1); +struct GJKSolver; + +/// Clamp num / denom in [0, 1] +FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) { + assert(denom >= 0.); + if (num <= 0.) + return 0.; + else if (num >= denom) + return 1.; + else + return num / denom; +} + +/// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd +void clamped_linear(Vec3f& a_sd, const Vec3f& a, const FCL_REAL& s_n, + const FCL_REAL& s_d, const Vec3f& d) { + assert(s_d >= 0.); + if (s_n <= 0.) + a_sd = a; + else if (s_n >= s_d) + a_sd = a + d; + else + a_sd = a + s_n / s_d * d; +} + +// Compute the distance between C1 and C2 by computing the distance +// between the two segments supporting the capsules. +// Match algorithm of Real-Time Collision Detection, Christer Ericson - Closest +// Point of Two Line Segments +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest& request, DistanceResult& result) { + const Capsule* capsule1 = static_cast(o1); + const Capsule* capsule2 = static_cast(o2); + + FCL_REAL EPSILON = std::numeric_limits::epsilon() * 100; + + // We assume that capsules are centered at the origin. + const fcl::Vec3f& c1 = tf1.getTranslation(); + const fcl::Vec3f& c2 = tf2.getTranslation(); + // We assume that capsules are oriented along z-axis. + FCL_REAL halfLength1 = capsule1->halfLength; + FCL_REAL halfLength2 = capsule2->halfLength; + FCL_REAL radius1 = capsule1->radius; + FCL_REAL radius2 = capsule2->radius; + // direction of capsules + // ||d1|| = 2 * halfLength1 + const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2); + const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2); + + // Starting point of the segments + // p1 + d1 is the end point of the segment + const fcl::Vec3f p1 = c1 - d1 / 2; + const fcl::Vec3f p2 = c2 - d2 / 2; + const fcl::Vec3f r = p1 - p2; + FCL_REAL a = d1.dot(d1); + FCL_REAL b = d1.dot(d2); + FCL_REAL c = d1.dot(r); + FCL_REAL e = d2.dot(d2); + FCL_REAL f = d2.dot(r); + // S1 is parametrized by the equation p1 + s * d1 + // S2 is parametrized by the equation p2 + t * d2 + + Vec3f w1, w2; + if (a <= EPSILON) { + w1 = p1; + if (e <= EPSILON) + // Check if the segments degenerate into points w2 = p2; - } else - { - // Always non-negative, equal 0 if the segments are colinear - FCL_REAL denom = fmax(a*e-b*b, 0); - - FCL_REAL s; - FCL_REAL t; - if (denom > EPSILON) - { - s = clamp((b*f-c*e), denom); - t = b*s + f; - } - else - { - s = 0.; - t = f; - } - - if (t <= 0.0) - { - w2 = p2; - clamped_linear (w1, p1, -c, a, d1); - } - else if (t >= e) - { - clamped_linear (w1, p1, (b - c), a, d1); - w2 = p2 + d2; - } - else - { - w1 = p1 + s*d1; - w2 = p2 + t/e*d2; - } + // First segment is degenerated + clamped_linear(w2, p2, f, e, d2); + } else if (e <= EPSILON) { + // Second segment is degenerated + clamped_linear(w1, p1, -c, a, d1); + w2 = p2; + } else { + // Always non-negative, equal 0 if the segments are colinear + FCL_REAL denom = fmax(a * e - b * b, 0); + + FCL_REAL s; + FCL_REAL t; + if (denom > EPSILON) { + s = clamp((b * f - c * e), denom); + t = b * s + f; + } else { + s = 0.; + t = f; } - // witness points achieving the distance between the two segments - FCL_REAL distance = (w1 - w2).norm(); - const Vec3f normal = (w1 - w2) / distance; - result.normal = normal; - - // capsule spcecific distance computation - distance = distance - (radius1 + radius2); - result.min_distance = distance; - - // witness points for the capsules - if (request.enable_nearest_points) - { - result.nearest_points[0] = w1 - radius1 * normal; - result.nearest_points[1] = w2 + radius2 * normal; + if (t <= 0.0) { + w2 = p2; + clamped_linear(w1, p1, -c, a, d1); + } else if (t >= e) { + clamped_linear(w1, p1, (b - c), a, d1); + w2 = p2 + d2; + } else { + w1 = p1 + s * d1; + w2 = p2 + t / e * d2; } - - return distance; } -} // namespace fcl + // witness points achieving the distance between the two segments + FCL_REAL distance = (w1 - w2).norm(); + const Vec3f normal = (w1 - w2) / distance; + result.normal = normal; + + // capsule spcecific distance computation + distance = distance - (radius1 + radius2); + result.min_distance = distance; + + // witness points for the capsules + if (request.enable_nearest_points) { + result.nearest_points[0] = w1 - radius1 * normal; + result.nearest_points[1] = w2 + radius2 * normal; + } + + return distance; +} + +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp index 27c670aeb..ef64710d9 100644 --- a/src/distance/capsule_halfspace.cpp +++ b/src/distance/capsule_halfspace.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Capsule& s1 = static_cast (*o1); - const Halfspace& s2 = static_cast (*o2); - details::capsuleHalfspaceIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Capsule& s1 = static_cast(*o1); + const Halfspace& s2 = static_cast(*o2); + details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Halfspace& s1 = static_cast (*o1); - const Capsule& s2 = static_cast (*o2); - details::capsuleHalfspaceIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Halfspace& s1 = static_cast(*o1); + const Capsule& s2 = static_cast(*o2); + details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp index 5217692fc..671eb5f36 100644 --- a/src/distance/capsule_plane.cpp +++ b/src/distance/capsule_plane.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Capsule& s1 = static_cast (*o1); - const Plane& s2 = static_cast (*o2); - details::capsulePlaneIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Capsule& s1 = static_cast(*o1); + const Plane& s2 = static_cast(*o2); + details::capsulePlaneIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Plane& s1 = static_cast (*o1); - const Capsule& s2 = static_cast (*o2); - details::capsulePlaneIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Plane& s1 = static_cast(*o1); + const Capsule& s2 = static_cast(*o2); + details::capsulePlaneIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp index 0a5c516a2..2b100f06b 100644 --- a/src/distance/cone_halfspace.cpp +++ b/src/distance/cone_halfspace.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Cone& s1 = static_cast (*o1); - const Halfspace& s2 = static_cast (*o2); - details::coneHalfspaceIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Cone& s1 = static_cast(*o1); + const Halfspace& s2 = static_cast(*o2); + details::coneHalfspaceIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Halfspace& s1 = static_cast (*o1); - const Cone& s2 = static_cast (*o2); - details::coneHalfspaceIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Halfspace& s1 = static_cast(*o1); + const Cone& s2 = static_cast(*o2); + details::coneHalfspaceIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp index 536cc1377..60ee36b44 100644 --- a/src/distance/cone_plane.cpp +++ b/src/distance/cone_plane.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Cone& s1 = static_cast (*o1); - const Plane& s2 = static_cast (*o2); - details::conePlaneIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Cone& s1 = static_cast(*o1); + const Plane& s2 = static_cast(*o2); + details::conePlaneIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Plane& s1 = static_cast (*o1); - const Cone& s2 = static_cast (*o2); - details::conePlaneIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Plane& s1 = static_cast(*o1); + const Cone& s2 = static_cast(*o2); + details::conePlaneIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 36b29655d..897386d52 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -43,37 +43,39 @@ namespace hpp { namespace fcl { template <> -FCL_REAL ShapeShapeDistance -(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) -{ - const ConvexBase& s1 = static_cast (*o1); - const Halfspace& s2 = static_cast (*o2); - details::halfspaceDistance - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const ConvexBase& s1 = static_cast(*o1); + const Halfspace& s2 = static_cast(*o2); + details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], result.nearest_points[0], + result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; result.normal = -result.normal; return result.min_distance; } template <> -FCL_REAL ShapeShapeDistance -(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) -{ - const Halfspace& s1 = static_cast (*o1); - const ConvexBase& s2 = static_cast (*o2); - details::halfspaceDistance - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Halfspace& s1 = static_cast(*o1); + const ConvexBase& s2 = static_cast(*o2); + details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], result.nearest_points[1], + result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; return result.min_distance; } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp index 2daff95a7..c664071c1 100644 --- a/src/distance/cylinder_halfspace.cpp +++ b/src/distance/cylinder_halfspace.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Cylinder& s1 = static_cast (*o1); - const Halfspace& s2 = static_cast (*o2); - details::cylinderHalfspaceIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Cylinder& s1 = static_cast(*o1); + const Halfspace& s2 = static_cast(*o2); + details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Halfspace& s1 = static_cast (*o1); - const Cylinder& s2 = static_cast (*o2); - details::cylinderHalfspaceIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Halfspace& s1 = static_cast(*o1); + const Cylinder& s2 = static_cast(*o2); + details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp index a06deb3ff..5812afe90 100644 --- a/src/distance/cylinder_plane.cpp +++ b/src/distance/cylinder_plane.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Cylinder& s1 = static_cast (*o1); - const Plane& s2 = static_cast (*o2); - details::cylinderPlaneIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Cylinder& s1 = static_cast(*o1); + const Plane& s2 = static_cast(*o2); + details::cylinderPlaneIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Plane& s1 = static_cast (*o1); - const Cylinder& s2 = static_cast (*o2); - details::cylinderPlaneIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Plane& s1 = static_cast(*o1); + const Cylinder& s2 = static_cast(*o2); + details::cylinderPlaneIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp index 8b8e4eb2c..004608ade 100644 --- a/src/distance/sphere_cylinder.cpp +++ b/src/distance/sphere_cylinder.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Sphere& s1 = static_cast (*o1); - const Cylinder& s2 = static_cast (*o2); - details::sphereCylinderDistance - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Sphere& s1 = static_cast(*o1); + const Cylinder& s2 = static_cast(*o2); + details::sphereCylinderDistance(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Cylinder& s1 = static_cast (*o1); - const Sphere& s2 = static_cast (*o2); - details::sphereCylinderDistance - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Cylinder& s1 = static_cast(*o1); + const Sphere& s2 = static_cast(*o2); + details::sphereCylinderDistance(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp index d157e219f..a1ac8fcf6 100644 --- a/src/distance/sphere_halfspace.cpp +++ b/src/distance/sphere_halfspace.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Sphere& s1 = static_cast (*o1); - const Halfspace& s2 = static_cast (*o2); - details::sphereHalfspaceIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Sphere& s1 = static_cast(*o1); + const Halfspace& s2 = static_cast(*o2); + details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Halfspace& s1 = static_cast (*o1); - const Sphere& s2 = static_cast (*o2); - details::sphereHalfspaceIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Halfspace& s1 = static_cast(*o1); + const Sphere& s2 = static_cast(*o2); + details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp index 03a8a85be..1048321b0 100644 --- a/src/distance/sphere_plane.cpp +++ b/src/distance/sphere_plane.cpp @@ -44,43 +44,44 @@ #include #include "../narrowphase/details.h" -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Sphere& s1 = static_cast (*o1); - const Plane& s2 = static_cast (*o2); - details::spherePlaneIntersect - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - return result.min_distance; - } +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Sphere& s1 = static_cast(*o1); + const Plane& s2 = static_cast(*o2); + details::spherePlaneIntersect(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], + result.nearest_points[1], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + return result.min_distance; +} - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - const Plane& s1 = static_cast (*o1); - const Sphere& s2 = static_cast (*o2); - details::spherePlaneIntersect - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; - result.normal = -result.normal; - return result.min_distance; - } -} // namespace fcl +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Plane& s1 = static_cast(*o1); + const Sphere& s2 = static_cast(*o2); + details::spherePlaneIntersect(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], + result.nearest_points[0], result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; + result.normal = -result.normal; + return result.min_distance; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index ed585ae4a..4e99c2098 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -47,93 +47,86 @@ // // One solution would be to make narrow phase solvers derive from an abstract // class and specialize the template for this abstract class. -namespace hpp -{ +namespace hpp { namespace fcl { - struct GJKSolver; +struct GJKSolver; - template <> - FCL_REAL ShapeShapeDistance - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) - { - FCL_REAL epsilon = 1e-7; - const Sphere* s1 = static_cast (o1); - const Sphere* s2 = static_cast (o2); +template <> +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + FCL_REAL epsilon = 1e-7; + const Sphere* s1 = static_cast(o1); + const Sphere* s2 = static_cast(o2); - // We assume that spheres are centered at the origin of their frame. - const fcl::Vec3f& center1 = tf1.getTranslation (); - const fcl::Vec3f& center2 = tf2.getTranslation (); - FCL_REAL r1 = s1->radius; - FCL_REAL r2 = s2->radius; + // We assume that spheres are centered at the origin of their frame. + const fcl::Vec3f& center1 = tf1.getTranslation(); + const fcl::Vec3f& center2 = tf2.getTranslation(); + FCL_REAL r1 = s1->radius; + FCL_REAL r2 = s2->radius; - result.o1 = o1; - result.o2 = o2; - result.b1 = result.b2 = -1; - Vec3f c1c2 = center2 - center1; - FCL_REAL dist = c1c2.norm (); - Vec3f unit (0,0,0); - if(dist > epsilon) - unit = c1c2/dist; - FCL_REAL penetrationDepth; - penetrationDepth = r1 + r2 - dist; - bool collision = (penetrationDepth >= 0); - result.min_distance = -penetrationDepth; - if (collision) { - // Take contact point at the middle of intersection between each sphere - // and segment [c1 c2]. - FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2); - Vec3f contact = center1 + abscissa * unit; - result.nearest_points [0] = result.nearest_points [1] = contact; - return result.min_distance; - } else { - FCL_REAL abs1 (r1), abs2 (dist - r2); - result.nearest_points [0] = center1 + abs1 * unit; - result.nearest_points [1] = center1 + abs2 * unit; - } + result.o1 = o1; + result.o2 = o2; + result.b1 = result.b2 = -1; + Vec3f c1c2 = center2 - center1; + FCL_REAL dist = c1c2.norm(); + Vec3f unit(0, 0, 0); + if (dist > epsilon) unit = c1c2 / dist; + FCL_REAL penetrationDepth; + penetrationDepth = r1 + r2 - dist; + bool collision = (penetrationDepth >= 0); + result.min_distance = -penetrationDepth; + if (collision) { + // Take contact point at the middle of intersection between each sphere + // and segment [c1 c2]. + FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2); + Vec3f contact = center1 + abscissa * unit; + result.nearest_points[0] = result.nearest_points[1] = contact; return result.min_distance; + } else { + FCL_REAL abs1(r1), abs2(dist - r2); + result.nearest_points[0] = center1 + abs1 * unit; + result.nearest_points[1] = center1 + abs2 * unit; } + return result.min_distance; +} - template <> - std::size_t ShapeShapeCollide - (const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const CollisionRequest& request, - CollisionResult& result) - { - FCL_REAL epsilon = 1e-7; - const Sphere* s1 = static_cast (o1); - const Sphere* s2 = static_cast (o2); +template <> +std::size_t ShapeShapeCollide( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionRequest& request, CollisionResult& result) { + FCL_REAL epsilon = 1e-7; + const Sphere* s1 = static_cast(o1); + const Sphere* s2 = static_cast(o2); - // We assume that capsules are centered at the origin. - const fcl::Vec3f& center1 = tf1.getTranslation (); - const fcl::Vec3f& center2 = tf2.getTranslation (); - FCL_REAL r1 = s1->radius; - FCL_REAL r2 = s2->radius; - FCL_REAL margin (request.security_margin); + // We assume that capsules are centered at the origin. + const fcl::Vec3f& center1 = tf1.getTranslation(); + const fcl::Vec3f& center2 = tf2.getTranslation(); + FCL_REAL r1 = s1->radius; + FCL_REAL r2 = s2->radius; + FCL_REAL margin(request.security_margin); - Vec3f c1c2 = center2 - center1; - FCL_REAL dist = c1c2.norm (); - Vec3f unit (0,0,0); - if(dist > epsilon) - unit = c1c2/dist; - // Unlike in distance computation, we consider the security margin. - FCL_REAL penetrationDepth = r1 + r2 + margin - dist; - result.updateDistanceLowerBound (-penetrationDepth+margin); - bool collision = (penetrationDepth >= 0); - if (collision) { - // Take contact point at the middle of intersection between each sphere - // and segment [c1 c2]. - FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2); - Vec3f contactPoint = center1 + abscissa * unit; - Contact contact (o1, o2, -1, -1, contactPoint, unit, penetrationDepth); - result.addContact (contact); - return 1; - } - return 0; + Vec3f c1c2 = center2 - center1; + FCL_REAL dist = c1c2.norm(); + Vec3f unit(0, 0, 0); + if (dist > epsilon) unit = c1c2 / dist; + // Unlike in distance computation, we consider the security margin. + FCL_REAL penetrationDepth = r1 + r2 + margin - dist; + result.updateDistanceLowerBound(-penetrationDepth + margin); + bool collision = (penetrationDepth >= 0); + if (collision) { + // Take contact point at the middle of intersection between each sphere + // and segment [c1 c2]. + FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2); + Vec3f contactPoint = center1 + abscissa * unit; + Contact contact(o1, o2, -1, -1, contactPoint, unit, penetrationDepth); + result.addContact(contact); + return 1; } -} // namespace fcl + return 0; +} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index 2435a9aa2..13514371e 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -43,37 +43,39 @@ namespace hpp { namespace fcl { template <> -FCL_REAL ShapeShapeDistance -(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) -{ - const TriangleP& s1 = static_cast (*o1); - const Halfspace& s2 = static_cast (*o2); - details::halfspaceDistance - (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1], - result.nearest_points [0], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const TriangleP& s1 = static_cast(*o1); + const Halfspace& s2 = static_cast(*o2); + details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance, + result.nearest_points[1], result.nearest_points[0], + result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; result.normal = -result.normal; return result.min_distance; } template <> -FCL_REAL ShapeShapeDistance -(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver*, const DistanceRequest&, - DistanceResult& result) -{ - const Halfspace& s1 = static_cast (*o1); - const TriangleP& s2 = static_cast (*o2); - details::halfspaceDistance - (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0], - result.nearest_points [1], result.normal); - result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1; +FCL_REAL ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { + const Halfspace& s1 = static_cast(*o1); + const TriangleP& s2 = static_cast(*o2); + details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance, + result.nearest_points[0], result.nearest_points[1], + result.normal); + result.o1 = o1; + result.o2 = o2; + result.b1 = -1; + result.b2 = -1; return result.min_distance; } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index e47071902..a91e57ca0 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -42,20 +42,17 @@ #include #include <../src/traits_traversal.h> -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { #ifdef HPP_FCL_HAS_OCTOMAP -template +template FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { + if (request.isSatisfied(result)) return result.min_distance; typename TraversalTraitsDistance::CollisionTraversal_t node; const TypeA* obj1 = static_cast(o1); const TypeB* obj2 = static_cast(o2); @@ -63,19 +60,19 @@ FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); distance(&node); - + return result.min_distance; } #endif -template - FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; +template +FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { + if (request.isSatisfied(result)) return result.min_distance; ShapeDistanceTraversalNode node; const T_SH1* obj1 = static_cast(o1); const T_SH2* obj2 = static_cast(o2); @@ -86,128 +83,129 @@ template return result.min_distance; } -template -struct HPP_FCL_LOCAL BVHShapeDistancer -{ +template +struct HPP_FCL_LOCAL BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { - if(request.isSatisfied(result)) return result.min_distance; + const DistanceRequest& request, + DistanceResult& result) { + if (request.isSatisfied(result)) return result.min_distance; MeshShapeDistanceTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); + const BVHModel* obj1 = static_cast*>(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); fcl::distance(&node); - + delete obj1_tmp; return result.min_distance; } }; -namespace details -{ +namespace details { -template -FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +template +FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; + const DistanceRequest& request, + DistanceResult& result) { + if (request.isSatisfied(result)) return result.min_distance; OrientedMeshShapeDistanceTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); + const BVHModel* obj1 = static_cast*>(o1); const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); fcl::distance(&node); - return result.min_distance; + return result.min_distance; } -} +} // namespace details -template -struct HPP_FCL_LOCAL BVHShapeDistancer -{ +template +struct HPP_FCL_LOCAL BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { - return details::orientedBVHShapeDistance, RSS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result); + const DistanceRequest& request, + DistanceResult& result) { + return details::orientedBVHShapeDistance< + MeshShapeDistanceTraversalNodeRSS, RSS, T_SH>( + o1, tf1, o2, tf2, nsolver, request, result); } }; - -template -struct HPP_FCL_LOCAL BVHShapeDistancer -{ +template +struct HPP_FCL_LOCAL BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { - return details::orientedBVHShapeDistance, kIOS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result); + const DistanceRequest& request, + DistanceResult& result) { + return details::orientedBVHShapeDistance< + MeshShapeDistanceTraversalNodekIOS, kIOS, T_SH>( + o1, tf1, o2, tf2, nsolver, request, result); } }; -template -struct HPP_FCL_LOCAL BVHShapeDistancer -{ +template +struct HPP_FCL_LOCAL BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { - return details::orientedBVHShapeDistance, OBBRSS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result); + const DistanceRequest& request, + DistanceResult& result) { + return details::orientedBVHShapeDistance< + MeshShapeDistanceTraversalNodeOBBRSS, OBBRSS, T_SH>( + o1, tf1, o2, tf2, nsolver, request, result); } }; - - -template -struct HPP_FCL_LOCAL HeightFieldShapeDistancer -{ +template +struct HPP_FCL_LOCAL HeightFieldShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { + const DistanceRequest& request, + DistanceResult& result) { HPP_FCL_UNUSED_VARIABLE(o1); HPP_FCL_UNUSED_VARIABLE(tf1); HPP_FCL_UNUSED_VARIABLE(o2); HPP_FCL_UNUSED_VARIABLE(tf2); HPP_FCL_UNUSED_VARIABLE(nsolver); HPP_FCL_UNUSED_VARIABLE(request); - //TODO(jcarpent) - HPP_FCL_THROW_PRETTY("Distance between a height field and a shape is not implemented", std::invalid_argument); -// if(request.isSatisfied(result)) return result.min_distance; -// HeightFieldShapeDistanceTraversalNode node; -// -// const HeightField* obj1 = static_cast* >(o1); -// const T_SH* obj2 = static_cast(o2); -// -// initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); -// fcl::distance(&node); - + // TODO(jcarpent) + HPP_FCL_THROW_PRETTY( + "Distance between a height field and a shape is not implemented", + std::invalid_argument); + // if(request.isSatisfied(result)) return result.min_distance; + // HeightFieldShapeDistanceTraversalNode node; + // + // const HeightField* obj1 = static_cast* + // >(o1); const T_SH* obj2 = static_cast(o2); + // + // initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); + // fcl::distance(&node); + return result.min_distance; } }; - -template -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; +template +FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const DistanceRequest& request, DistanceResult& result) { + if (request.isSatisfied(result)) return result.min_distance; MeshDistanceTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - const BVHModel* obj2 = static_cast* >(o2); + const BVHModel* obj1 = static_cast*>(o1); + const BVHModel* obj2 = static_cast*>(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); @@ -217,21 +215,22 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const distance(&node); delete obj1_tmp; delete obj2_tmp; - + return result.min_distance; } -namespace details -{ -template -FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; +namespace details { +template +FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) { + if (request.isSatisfied(result)) return result.min_distance; OrientedMeshDistanceTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - const BVHModel* obj2 = static_cast* >(o2); + const BVHModel* obj1 = static_cast*>(o1); + const BVHModel* obj2 = static_cast*>(o2); initialize(node, *obj1, tf1, *obj2, tf2, request, result); distance(&node); @@ -239,49 +238,48 @@ FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, const Transform3f& tf return result.min_distance; } -} +} // namespace details -template<> +template <> FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); + const DistanceRequest& request, + DistanceResult& result) { + return details::orientedMeshDistance( + o1, tf1, o2, tf2, request, result); } -template<> +template <> FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); + const DistanceRequest& request, + DistanceResult& result) { + return details::orientedMeshDistance( + o1, tf1, o2, tf2, request, result); } - -template<> -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); +template <> +FCL_REAL BVHDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) { + return details::orientedMeshDistance( + o1, tf1, o2, tf2, request, result); } - -template +template FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* /*nsolver*/, - const DistanceRequest& request, DistanceResult& result) -{ + const DistanceRequest& request, DistanceResult& result) { return BVHDistance(o1, tf1, o2, tf2, request, result); } -DistanceFunctionMatrix::DistanceFunctionMatrix() -{ - for(int i = 0; i < NODE_COUNT; ++i) - { - for(int j = 0; j < NODE_COUNT; ++j) - distance_matrix[i][j] = NULL; +DistanceFunctionMatrix::DistanceFunctionMatrix() { + for (int i = 0; i < NODE_COUNT; ++i) { + for (int j = 0; j < NODE_COUNT; ++j) distance_matrix[i][j] = NULL; } distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance; @@ -291,166 +289,266 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance; + distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = + &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance; + distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = + &ShapeShapeDistance; + distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = + &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance; + distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = + &ShapeShapeDistance; + distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = + &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance; + distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = + &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance; + distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = + &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance; + distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = + &ShapeShapeDistance; + distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = + &ShapeShapeDistance; + distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = + &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance; + distance_matrix[GEOM_CONE][GEOM_CYLINDER] = + &ShapeShapeDistance; + distance_matrix[GEOM_CONE][GEOM_CONVEX] = + &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance; + distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = + &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance; + distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CYLINDER][GEOM_CONE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = + &ShapeShapeDistance; + distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = + &ShapeShapeDistance; + distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = + &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX][GEOM_CONE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = + &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = + &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX][GEOM_PLANE] = + &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = + &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance; + distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = + &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance; + distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = + &ShapeShapeDistance; + distance_matrix[GEOM_PLANE][GEOM_CONVEX] = + &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance; + distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = + &ShapeShapeDistance; + + distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = + &ShapeShapeDistance; + distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = + &ShapeShapeDistance; + distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = + &ShapeShapeDistance; + distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = + &ShapeShapeDistance; + distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = + &ShapeShapeDistance; + distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = + &ShapeShapeDistance; + distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = + &ShapeShapeDistance; + distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = + &ShapeShapeDistance; /* AABB distance not implemented */ /* distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; + distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CAPSULE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CYLINDER] = + &BVHShapeDistancer::distance; + distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_PLANE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; */ distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer::distance; + distance_matrix[BV_OBB][GEOM_SPHERE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBB][GEOM_CAPSULE] = + &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; + distance_matrix[BV_OBB][GEOM_CYLINDER] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBB][GEOM_CONVEX] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBB][GEOM_PLANE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBB][GEOM_HALFSPACE] = + &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; + distance_matrix[BV_RSS][GEOM_SPHERE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_RSS][GEOM_CAPSULE] = + &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; + distance_matrix[BV_RSS][GEOM_CYLINDER] = + &BVHShapeDistancer::distance; + distance_matrix[BV_RSS][GEOM_CONVEX] = + &BVHShapeDistancer::distance; + distance_matrix[BV_RSS][GEOM_PLANE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_RSS][GEOM_HALFSPACE] = + &BVHShapeDistancer::distance; /* KDOP distance not implemented */ /* - distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer, Box>::distance; - distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer, Sphere>::distance; - distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule>::distance; - distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer, Cone>::distance; - distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder>::distance; - distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer, ConvexBase>::distance; - distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer, Plane>::distance; - distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace>::distance; - - distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer, Box>::distance; - distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer, Sphere>::distance; - distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule>::distance; - distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer, Cone>::distance; - distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder>::distance; - distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer, ConvexBase>::distance; - distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer, Plane>::distance; - distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace>::distance; - - distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer, Box>::distance; - distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer, Sphere>::distance; - distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule>::distance; - distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer, Cone>::distance; - distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder>::distance; - distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer, ConvexBase>::distance; - distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer, Plane>::distance; - distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace>::distance; + distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer, + Box>::distance; distance_matrix[BV_KDOP16][GEOM_SPHERE] = + &BVHShapeDistancer, Sphere>::distance; + distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer, + Capsule>::distance; distance_matrix[BV_KDOP16][GEOM_CONE] = + &BVHShapeDistancer, Cone>::distance; + distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer, + Cylinder>::distance; distance_matrix[BV_KDOP16][GEOM_CONVEX] = + &BVHShapeDistancer, ConvexBase>::distance; + distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer, + Plane>::distance; distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = + &BVHShapeDistancer, Halfspace>::distance; + + distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer, + Box>::distance; distance_matrix[BV_KDOP18][GEOM_SPHERE] = + &BVHShapeDistancer, Sphere>::distance; + distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer, + Capsule>::distance; distance_matrix[BV_KDOP18][GEOM_CONE] = + &BVHShapeDistancer, Cone>::distance; + distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer, + Cylinder>::distance; distance_matrix[BV_KDOP18][GEOM_CONVEX] = + &BVHShapeDistancer, ConvexBase>::distance; + distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer, + Plane>::distance; distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = + &BVHShapeDistancer, Halfspace>::distance; + + distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer, + Box>::distance; distance_matrix[BV_KDOP24][GEOM_SPHERE] = + &BVHShapeDistancer, Sphere>::distance; + distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer, + Capsule>::distance; distance_matrix[BV_KDOP24][GEOM_CONE] = + &BVHShapeDistancer, Cone>::distance; + distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer, + Cylinder>::distance; distance_matrix[BV_KDOP24][GEOM_CONVEX] = + &BVHShapeDistancer, ConvexBase>::distance; + distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer, + Plane>::distance; distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = + &BVHShapeDistancer, Halfspace>::distance; */ distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; - - distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; - - distance_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_AABB][GEOM_CAPSULE] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_AABB][GEOM_CONE] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_AABB][GEOM_CYLINDER] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_AABB][GEOM_CONVEX] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeDistancer::distance; - - distance_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_OBBRSS][GEOM_CAPSULE] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_OBBRSS][GEOM_CONE] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_OBBRSS][GEOM_CYLINDER] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_OBBRSS][GEOM_CONVEX] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeDistancer::distance; - distance_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeDistancer::distance; + distance_matrix[BV_kIOS][GEOM_SPHERE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_kIOS][GEOM_CAPSULE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_kIOS][GEOM_CONE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_kIOS][GEOM_CYLINDER] = + &BVHShapeDistancer::distance; + distance_matrix[BV_kIOS][GEOM_CONVEX] = + &BVHShapeDistancer::distance; + distance_matrix[BV_kIOS][GEOM_PLANE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_kIOS][GEOM_HALFSPACE] = + &BVHShapeDistancer::distance; + + distance_matrix[BV_OBBRSS][GEOM_BOX] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBBRSS][GEOM_SPHERE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBBRSS][GEOM_CONE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBBRSS][GEOM_CONVEX] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBBRSS][GEOM_PLANE] = + &BVHShapeDistancer::distance; + distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = + &BVHShapeDistancer::distance; + + distance_matrix[HF_AABB][GEOM_BOX] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_AABB][GEOM_SPHERE] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_AABB][GEOM_CAPSULE] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_AABB][GEOM_CONE] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_AABB][GEOM_CYLINDER] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_AABB][GEOM_CONVEX] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_AABB][GEOM_PLANE] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_AABB][GEOM_HALFSPACE] = + &HeightFieldShapeDistancer::distance; + + distance_matrix[HF_OBBRSS][GEOM_BOX] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_OBBRSS][GEOM_SPHERE] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_OBBRSS][GEOM_CAPSULE] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_OBBRSS][GEOM_CONE] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_OBBRSS][GEOM_CYLINDER] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_OBBRSS][GEOM_CONVEX] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_OBBRSS][GEOM_PLANE] = + &HeightFieldShapeDistancer::distance; + distance_matrix[HF_OBBRSS][GEOM_HALFSPACE] = + &HeightFieldShapeDistancer::distance; distance_matrix[BV_AABB][BV_AABB] = &BVHDistance; distance_matrix[BV_OBB][BV_OBB] = &BVHDistance; @@ -479,28 +577,33 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Distance; - distance_matrix[GEOM_OCTREE][BV_AABB ] = &Distance >; - distance_matrix[GEOM_OCTREE][BV_OBB ] = &Distance >; - distance_matrix[GEOM_OCTREE][BV_RSS ] = &Distance >; - distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &Distance >; - distance_matrix[GEOM_OCTREE][BV_kIOS ] = &Distance >; - distance_matrix[GEOM_OCTREE][BV_KDOP16] = &Distance > >; - distance_matrix[GEOM_OCTREE][BV_KDOP18] = &Distance > >; - distance_matrix[GEOM_OCTREE][BV_KDOP24] = &Distance > >; - - distance_matrix[BV_AABB][GEOM_OCTREE ] = &Distance, OcTree>; - distance_matrix[BV_OBB][GEOM_OCTREE ] = &Distance, OcTree>; - distance_matrix[BV_RSS][GEOM_OCTREE ] = &Distance, OcTree>; - distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &Distance, OcTree>; - distance_matrix[BV_kIOS][GEOM_OCTREE ] = &Distance, OcTree>; - distance_matrix[BV_KDOP16][GEOM_OCTREE] = &Distance >, OcTree>; - distance_matrix[BV_KDOP18][GEOM_OCTREE] = &Distance >, OcTree>; - distance_matrix[BV_KDOP24][GEOM_OCTREE] = &Distance >, OcTree>; + distance_matrix[GEOM_OCTREE][BV_AABB] = &Distance >; + distance_matrix[GEOM_OCTREE][BV_OBB] = &Distance >; + distance_matrix[GEOM_OCTREE][BV_RSS] = &Distance >; + distance_matrix[GEOM_OCTREE][BV_OBBRSS] = + &Distance >; + distance_matrix[GEOM_OCTREE][BV_kIOS] = &Distance >; + distance_matrix[GEOM_OCTREE][BV_KDOP16] = + &Distance > >; + distance_matrix[GEOM_OCTREE][BV_KDOP18] = + &Distance > >; + distance_matrix[GEOM_OCTREE][BV_KDOP24] = + &Distance > >; + + distance_matrix[BV_AABB][GEOM_OCTREE] = &Distance, OcTree>; + distance_matrix[BV_OBB][GEOM_OCTREE] = &Distance, OcTree>; + distance_matrix[BV_RSS][GEOM_OCTREE] = &Distance, OcTree>; + distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &Distance, OcTree>; + distance_matrix[BV_kIOS][GEOM_OCTREE] = &Distance, OcTree>; + distance_matrix[BV_KDOP16][GEOM_OCTREE] = + &Distance >, OcTree>; + distance_matrix[BV_KDOP18][GEOM_OCTREE] = + &Distance >, OcTree>; + distance_matrix[BV_KDOP24][GEOM_OCTREE] = + &Distance >, OcTree>; #endif - - -} -//template struct DistanceFunctionMatrix; } +// template struct DistanceFunctionMatrix; +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/hfield.cpp b/src/hfield.cpp index 4b4ddbd31..ba771ec65 100644 --- a/src/hfield.cpp +++ b/src/hfield.cpp @@ -45,68 +45,58 @@ #include #include -namespace hpp -{ -namespace fcl -{ - -template<> -NODE_TYPE HeightField::getNodeType() const -{ +namespace hpp { +namespace fcl { + +template <> +NODE_TYPE HeightField::getNodeType() const { return HF_AABB; } -template<> -NODE_TYPE HeightField::getNodeType() const -{ - return BV_UNKNOWN; // HF_OBB; +template <> +NODE_TYPE HeightField::getNodeType() const { + return BV_UNKNOWN; // HF_OBB; } -template<> -NODE_TYPE HeightField::getNodeType() const -{ - return BV_UNKNOWN; // HF_RSS; +template <> +NODE_TYPE HeightField::getNodeType() const { + return BV_UNKNOWN; // HF_RSS; } -template<> -NODE_TYPE HeightField::getNodeType() const -{ - return BV_UNKNOWN; // BV_kIOS; +template <> +NODE_TYPE HeightField::getNodeType() const { + return BV_UNKNOWN; // BV_kIOS; } -template<> -NODE_TYPE HeightField::getNodeType() const -{ +template <> +NODE_TYPE HeightField::getNodeType() const { return HF_OBBRSS; } -template<> -NODE_TYPE HeightField >::getNodeType() const -{ - return BV_UNKNOWN; // BV_KDOP16; +template <> +NODE_TYPE HeightField >::getNodeType() const { + return BV_UNKNOWN; // BV_KDOP16; } -template<> -NODE_TYPE HeightField >::getNodeType() const -{ - return BV_UNKNOWN; // BV_KDOP18; +template <> +NODE_TYPE HeightField >::getNodeType() const { + return BV_UNKNOWN; // BV_KDOP18; } -template<> -NODE_TYPE HeightField >::getNodeType() const -{ - return BV_UNKNOWN; // BV_KDOP24; +template <> +NODE_TYPE HeightField >::getNodeType() const { + return BV_UNKNOWN; // BV_KDOP24; } -//template class HeightField >; -//template class HeightField >; -//template class HeightField >; +// template class HeightField >; +// template class HeightField >; +// template class HeightField >; template class HeightField; template class HeightField; template class HeightField; -//template class HeightField; +// template class HeightField; template class HeightField; -} // namespace fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/intersect.cpp b/src/intersect.cpp index 1cb4fd086..3667523c9 100644 --- a/src/intersect.cpp +++ b/src/intersect.cpp @@ -39,21 +39,17 @@ #include #include #include -#include // isnan. +#include // isnan. #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -bool Intersect::buildTrianglePlane -(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t) -{ +bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, + const Vec3f& v3, Vec3f* n, FCL_REAL* t) { Vec3f n_ = (v2 - v1).cross(v3 - v1); FCL_REAL norm2 = n_.squaredNorm(); - if (norm2 > 0) - { + if (norm2 > 0) { *n = n_ / sqrt(norm2); *t = n->dot(v1); return true; @@ -61,9 +57,9 @@ bool Intersect::buildTrianglePlane return false; } -void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B, - Vec3f& VEC, Vec3f& X, Vec3f& Y) -{ +void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, + const Vec3f& B, Vec3f& VEC, Vec3f& X, + Vec3f& Y) { Vec3f T; FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; Vec3f TMP; @@ -83,102 +79,82 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, // compute t for the closest point on ray P,A to // ray Q,B - FCL_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; + FCL_REAL denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B; - t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; + t = (A_dot_T * B_dot_B - B_dot_T * A_dot_B) / denom; // clamp result so t is on the segment P,A - if((t < 0) || boost::math::isnan(t)) t = 0; else if(t > 1) t = 1; + if ((t < 0) || boost::math::isnan(t)) + t = 0; + else if (t > 1) + t = 1; // find u for point on ray Q,B closest to point at t - u = (t*A_dot_B - B_dot_T) / B_dot_B; + u = (t * A_dot_B - B_dot_T) / B_dot_B; // if u is on segment Q,B, t and u correspond to // closest points, otherwise, clamp u, recompute and // clamp t - if((u <= 0) || boost::math::isnan(u)) - { + if ((u <= 0) || boost::math::isnan(u)) { Y = Q; t = A_dot_T / A_dot_A; - if((t <= 0) || boost::math::isnan(t)) - { + if ((t <= 0) || boost::math::isnan(t)) { X = P; VEC = Q - P; - } - else if(t >= 1) - { + } else if (t >= 1) { X = P + A; VEC = Q - X; - } - else - { + } else { X = P + A * t; TMP = T.cross(A); VEC = A.cross(TMP); } - } - else if (u >= 1) - { + } else if (u >= 1) { Y = Q + B; t = (A_dot_B + A_dot_T) / A_dot_A; - if((t <= 0) || boost::math::isnan(t)) - { + if ((t <= 0) || boost::math::isnan(t)) { X = P; VEC = Y - P; - } - else if(t >= 1) - { + } else if (t >= 1) { X = P + A; VEC = Y - X; - } - else - { + } else { X = P + A * t; T = Y - P; TMP = T.cross(A); - VEC= A.cross(TMP); + VEC = A.cross(TMP); } - } - else - { + } else { Y = Q + B * u; - if((t <= 0) || boost::math::isnan(t)) - { + if ((t <= 0) || boost::math::isnan(t)) { X = P; TMP = T.cross(B); VEC = B.cross(TMP); - } - else if(t >= 1) - { + } else if (t >= 1) { X = P + A; T = Q - X; TMP = T.cross(B); VEC = B.cross(TMP); - } - else - { + } else { X = P + A * t; VEC = A.cross(B); - if(VEC.dot(T) < 0) - { + if (VEC.dot(T) < 0) { VEC = VEC * (-1); } } } } - -FCL_REAL TriangleDistance::sqrTriDistance -(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q) -{ +FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + Vec3f& P, Vec3f& Q) { // Compute vectors along the 6 sides Vec3f Sv[3]; @@ -205,12 +181,10 @@ FCL_REAL TriangleDistance::sqrTriDistance FCL_REAL mindd; int shown_disjoint = 0; - mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high + mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high - for(int i = 0; i < 3; ++i) - { - for(int j = 0; j < 3; ++j) - { + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { // Find closest points on edges i & j, plus the // vector (and distance squared) between these points segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); @@ -221,24 +195,23 @@ FCL_REAL TriangleDistance::sqrTriDistance // Verify this closest point pair only if the distance // squared is less than the minimum found thus far. - if(dd <= mindd) - { + if (dd <= mindd) { minP = P; minQ = Q; mindd = dd; - Z = S[(i+2)%3] - P; + Z = S[(i + 2) % 3] - P; FCL_REAL a = Z.dot(VEC); - Z = T[(j+2)%3] - Q; + Z = T[(j + 2) % 3] - Q; FCL_REAL b = Z.dot(VEC); - if((a <= 0) && (b >= 0)) return dd; + if ((a <= 0) && (b >= 0)) return dd; FCL_REAL p = V.dot(VEC); - if(a < 0) a = 0; - if(b > 0) b = 0; - if((p - a + b) > 0) shown_disjoint = 1; + if (a < 0) a = 0; + if (b > 0) b = 0; + if ((p - a + b) > 0) shown_disjoint = 1; } } } @@ -262,13 +235,12 @@ FCL_REAL TriangleDistance::sqrTriDistance Vec3f Sn; FCL_REAL Snl; - Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle - Snl = Sn.dot(Sn); // Compute square of length of normal + Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle + Snl = Sn.dot(Sn); // Compute square of length of normal // If cross product is long enough, - if(Snl > 1e-15) - { + if (Snl > 1e-15) { // Get projection lengths of T points Vec3f Tp; @@ -286,21 +258,23 @@ FCL_REAL TriangleDistance::sqrTriDistance // find point with smallest projection int point = -1; - if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) - { - if(Tp[0] < Tp[1]) point = 0; else point = 1; - if(Tp[2] < Tp[point]) point = 2; - } - else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) - { - if(Tp[0] > Tp[1]) point = 0; else point = 1; - if(Tp[2] > Tp[point]) point = 2; + if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) { + if (Tp[0] < Tp[1]) + point = 0; + else + point = 1; + if (Tp[2] < Tp[point]) point = 2; + } else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) { + if (Tp[0] > Tp[1]) + point = 0; + else + point = 1; + if (Tp[2] > Tp[point]) point = 2; } // If Sn is a separating direction, - if(point >= 0) - { + if (point >= 0) { shown_disjoint = 1; // Test whether the point found, when projected onto the @@ -308,16 +282,13 @@ FCL_REAL TriangleDistance::sqrTriDistance V = T[point] - S[0]; Z = Sn.cross(Sv[0]); - if(V.dot(Z) > 0) - { + if (V.dot(Z) > 0) { V = T[point] - S[1]; Z = Sn.cross(Sv[1]); - if(V.dot(Z) > 0) - { + if (V.dot(Z) > 0) { V = T[point] - S[2]; Z = Sn.cross(Sv[2]); - if(V.dot(Z) > 0) - { + if (V.dot(Z) > 0) { // T[point] passed the test - it's a closest point for // the T triangle; the other point is on the face of S P = T[point] + Sn * (Tp[point] / Snl); @@ -335,8 +306,7 @@ FCL_REAL TriangleDistance::sqrTriDistance Tn = Tv[0].cross(Tv[1]); Tnl = Tn.dot(Tn); - if(Tnl > 1e-15) - { + if (Tnl > 1e-15) { Vec3f Sp; V = T[0] - S[0]; @@ -349,33 +319,32 @@ FCL_REAL TriangleDistance::sqrTriDistance Sp[2] = V.dot(Tn); int point = -1; - if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) - { - if(Sp[0] < Sp[1]) point = 0; else point = 1; - if(Sp[2] < Sp[point]) point = 2; - } - else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) - { - if(Sp[0] > Sp[1]) point = 0; else point = 1; - if(Sp[2] > Sp[point]) point = 2; + if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) { + if (Sp[0] < Sp[1]) + point = 0; + else + point = 1; + if (Sp[2] < Sp[point]) point = 2; + } else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) { + if (Sp[0] > Sp[1]) + point = 0; + else + point = 1; + if (Sp[2] > Sp[point]) point = 2; } - if(point >= 0) - { + if (point >= 0) { shown_disjoint = 1; V = S[point] - T[0]; Z = Tn.cross(Tv[0]); - if(V.dot(Z) > 0) - { + if (V.dot(Z) > 0) { V = S[point] - T[1]; Z = Tn.cross(Tv[1]); - if(V.dot(Z) > 0) - { + if (V.dot(Z) > 0) { V = S[point] - T[2]; Z = Tn.cross(Tv[2]); - if(V.dot(Z) > 0) - { + if (V.dot(Z) > 0) { P = S[point]; Q = S[point] + Tn * (Sp[point] / Tnl); return (P - Q).squaredNorm(); @@ -390,177 +359,187 @@ FCL_REAL TriangleDistance::sqrTriDistance // we assume case 3 or 4, otherwise we conclude case 2, // that the triangles overlap. - if(shown_disjoint) - { + if (shown_disjoint) { P = minP; Q = minQ; return mindd; - } - else return 0; + } else + return 0; } - -FCL_REAL TriangleDistance::sqrTriDistance -(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, - const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, Vec3f& P, Vec3f& Q) -{ +FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + Vec3f& P, Vec3f& Q) { Vec3f S[3]; Vec3f T[3]; - S[0] = S1; S[1] = S2; S[2] = S3; - T[0] = T1; T[1] = T2; T[2] = T3; - - return sqrTriDistance (S, T, P, Q); + S[0] = S1; + S[1] = S2; + S[2] = S3; + T[0] = T1; + T[1] = T2; + T[2] = T3; + + return sqrTriDistance(S, T, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance (const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q) -{ +FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Matrix3f& R, const Vec3f& Tl, + Vec3f& P, Vec3f& Q) { Vec3f T_transformed[3]; T_transformed[0] = R * T[0] + Tl; T_transformed[1] = R * T[1] + Tl; T_transformed[2] = R * T[2] + Tl; - return sqrTriDistance (S, T_transformed, P, Q); + return sqrTriDistance(S, T_transformed, P, Q); } - -FCL_REAL TriangleDistance::sqrTriDistance (const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, - Vec3f& P, Vec3f& Q) -{ +FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Transform3f& tf, Vec3f& P, + Vec3f& Q) { Vec3f T_transformed[3]; T_transformed[0] = tf.transform(T[0]); T_transformed[1] = tf.transform(T[1]); T_transformed[2] = tf.transform(T[2]); - return sqrTriDistance (S, T_transformed, P, Q); + return sqrTriDistance(S, T_transformed, P, Q); } - -FCL_REAL TriangleDistance::sqrTriDistance -(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, - const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q) -{ +FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Matrix3f& R, const Vec3f& Tl, + Vec3f& P, Vec3f& Q) { Vec3f T1_transformed = R * T1 + Tl; Vec3f T2_transformed = R * T2 + Tl; Vec3f T3_transformed = R * T3 + Tl; - return sqrTriDistance (S1, S2, S3, T1_transformed, T2_transformed, - T3_transformed, P, Q); + return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, + T3_transformed, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance -(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, - const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, - Vec3f& P, Vec3f& Q) -{ +FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Transform3f& tf, Vec3f& P, + Vec3f& Q) { Vec3f T1_transformed = tf.transform(T1); Vec3f T2_transformed = tf.transform(T2); Vec3f T3_transformed = tf.transform(T3); - return sqrTriDistance (S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); + return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, + T3_transformed, P, Q); } - - - - -Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, const Vec3f& p) -{ +Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, + const Vec3f& p) { ProjectResult res; const Vec3f d = b - a; const FCL_REAL l = d.squaredNorm(); - if(l > 0) - { + if (l > 0) { const FCL_REAL t = (p - a).dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } + if (t >= l) { + res.sqr_distance = (p - b).squaredNorm(); + res.encode = 2; /* 0x10 */ + } else if (t <= 0) { + res.sqr_distance = (p - a).squaredNorm(); + res.encode = 1; /* 0x01 */ + } else { + res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); + res.encode = 3; /* 0x00 */ + } } return res; } -Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& p) -{ +Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, + const Vec3f& c, + const Vec3f& p) { ProjectResult res; - + static const size_t nexti[3] = {1, 2, 0}; const Vec3f* vt[] = {&a, &b, &c}; const Vec3f dl[] = {a - b, b - c, c - a}; const Vec3f& n = dl[0].cross(dl[1]); const FCL_REAL l = n.squaredNorm(); - - if(l > 0) - { + + if (l > 0) { FCL_REAL mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + for (size_t i = 0; i < 3; ++i) { + if ((*vt[i] - p).dot(dl[i].cross(n)) > + 0) // origin is to the outside part of the triangle edge, then the + // optimal can only be on the edge { size_t j = nexti[i]; ProjectResult res_line = projectLine(*vt[i], *vt[j], p); - if(mindist < 0 || res_line.sqr_distance < mindist) - { + if (mindist < 0 || res_line.sqr_distance < mindist) { mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1<(((res_line.encode & 1) ? 1 << i : 0) + + ((res_line.encode & 2) ? 1 << j : 0)); res.parameterization[i] = res_line.parameterization[0]; res.parameterization[j] = res_line.parameterization[1]; res.parameterization[nexti[j]] = 0; } } } - - if(mindist < 0) // the origin project is within the triangle + + if (mindist < 0) // the origin project is within the triangle { FCL_REAL d = (a - p).dot(n); FCL_REAL s = sqrt(l); Vec3f p_to_project = n * (d / l); mindist = p_to_project.squaredNorm(); - res.encode = 7; // m = 0x111 - res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s; - res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s; - res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - p - p_to_project).norm() / s; + res.parameterization[2] = + 1 - res.parameterization[0] - res.parameterization[1]; } res.sqr_distance = mindist; } - return res; - + return res; } -Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, const Vec3f& p) -{ +Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, + const Vec3f& b, + const Vec3f& c, + const Vec3f& d, + const Vec3f& p) { ProjectResult res; static const size_t nexti[] = {1, 2, 0}; const Vec3f* vt[] = {&a, &b, &c, &d}; - const Vec3f dl[3] = {a-d, b-d, c-d}; - FCL_REAL vl = triple(dl[0], dl[1], dl[2]); - bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0; - if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + const Vec3f dl[3] = {a - d, b - d, c - d}; + FCL_REAL vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0; + if (ng && + std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng + // is false, then the last vertex in the tetrahedron + // does not grow toward the origin (in fact origin is + // on the other side of the abc face) { FCL_REAL mindist = -1; - for(size_t i = 0; i < 3; ++i) - { + for (size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; - FCL_REAL s = vl * (d-p).dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + FCL_REAL s = vl * (d - p).dot(dl[i].cross(dl[j])); + if (s > 0) // the origin is to the outside part of a triangle face, then + // the optimal can only be on the triangle face { ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { + if (mindist < 0 || res_triangle.sqr_distance < mindist) { mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1<((res_triangle.encode & 1 ? 1 << i : 0) + + (res_triangle.encode & 2 ? 1 << j : 0) + + (res_triangle.encode & 4 ? 8 : 0)); res.parameterization[i] = res_triangle.parameterization[0]; res.parameterization[j] = res_triangle.parameterization[1]; res.parameterization[nexti[j]] = 0; @@ -569,120 +548,135 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, const Vec3f& b } } - if(mindist < 0) - { + if (mindist < 0) { mindist = 0; res.encode = 15; res.parameterization[0] = triple(c - p, b - p, d - p) / vl; res.parameterization[1] = triple(a - p, c - p, d - p) / vl; res.parameterization[2] = triple(b - p, a - p, d - p) / vl; - res.parameterization[3] = 1 - (res.parameterization[0] + res.parameterization[1] + res.parameterization[2]); + res.parameterization[3] = + 1 - (res.parameterization[0] + res.parameterization[1] + + res.parameterization[2]); } res.sqr_distance = mindist; - } - else if(!ng) - { + } else if (!ng) { res = projectTriangle(a, b, c, p); res.parameterization[3] = 0; } return res; } -Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, const Vec3f& b) -{ +Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, + const Vec3f& b) { ProjectResult res; const Vec3f d = b - a; const FCL_REAL l = d.squaredNorm(); - if(l > 0) - { - const FCL_REAL t = - a.dot(d); + if (l > 0) { + const FCL_REAL t = -a.dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } + if (t >= l) { + res.sqr_distance = b.squaredNorm(); + res.encode = 2; /* 0x10 */ + } else if (t <= 0) { + res.sqr_distance = a.squaredNorm(); + res.encode = 1; /* 0x01 */ + } else { + res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); + res.encode = 3; /* 0x00 */ + } } return res; } -Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c) -{ +Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, + const Vec3f& b, + const Vec3f& c) { ProjectResult res; - + static const size_t nexti[3] = {1, 2, 0}; const Vec3f* vt[] = {&a, &b, &c}; const Vec3f dl[] = {a - b, b - c, c - a}; const Vec3f& n = dl[0].cross(dl[1]); const FCL_REAL l = n.squaredNorm(); - - if(l > 0) - { + + if (l > 0) { FCL_REAL mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + for (size_t i = 0; i < 3; ++i) { + if (vt[i]->dot(dl[i].cross(n)) > + 0) // origin is to the outside part of the triangle edge, then the + // optimal can only be on the edge { size_t j = nexti[i]; ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); - if(mindist < 0 || res_line.sqr_distance < mindist) - { + if (mindist < 0 || res_line.sqr_distance < mindist) { mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1<(((res_line.encode & 1) ? 1 << i : 0) + + ((res_line.encode & 2) ? 1 << j : 0)); res.parameterization[i] = res_line.parameterization[0]; res.parameterization[j] = res_line.parameterization[1]; res.parameterization[nexti[j]] = 0; } } } - - if(mindist < 0) // the origin project is within the triangle + + if (mindist < 0) // the origin project is within the triangle { FCL_REAL d = a.dot(n); FCL_REAL s = sqrt(l); Vec3f o_to_project = n * (d / l); mindist = o_to_project.squaredNorm(); - res.encode = 7; // m = 0x111 + res.encode = 7; // m = 0x111 res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; - res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + res.parameterization[2] = + 1 - res.parameterization[0] - res.parameterization[1]; } res.sqr_distance = mindist; } - return res; - + return res; } -Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d) -{ +Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, + const Vec3f& b, + const Vec3f& c, + const Vec3f& d) { ProjectResult res; static const size_t nexti[] = {1, 2, 0}; const Vec3f* vt[] = {&a, &b, &c, &d}; - const Vec3f dl[3] = {a-d, b-d, c-d}; - FCL_REAL vl = triple(dl[0], dl[1], dl[2]); - bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0; - if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + const Vec3f dl[3] = {a - d, b - d, c - d}; + FCL_REAL vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0; + if (ng && + std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng + // is false, then the last vertex in the tetrahedron + // does not grow toward the origin (in fact origin is + // on the other side of the abc face) { FCL_REAL mindist = -1; - for(size_t i = 0; i < 3; ++i) - { + for (size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; FCL_REAL s = vl * d.dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + if (s > 0) // the origin is to the outside part of a triangle face, then + // the optimal can only be on the triangle face { ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { + if (mindist < 0 || res_triangle.sqr_distance < mindist) { mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1<((res_triangle.encode & 1 ? 1 << i : 0) + + (res_triangle.encode & 2 ? 1 << j : 0) + + (res_triangle.encode & 4 ? 8 : 0)); res.parameterization[i] = res_triangle.parameterization[0]; res.parameterization[j] = res_triangle.parameterization[1]; res.parameterization[nexti[j]] = 0; @@ -691,27 +685,25 @@ Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, const Ve } } - if(mindist < 0) - { + if (mindist < 0) { mindist = 0; res.encode = 15; res.parameterization[0] = triple(c, b, d) / vl; res.parameterization[1] = triple(a, c, d) / vl; res.parameterization[2] = triple(b, a, d) / vl; - res.parameterization[3] = 1 - (res.parameterization[0] + res.parameterization[1] + res.parameterization[2]); + res.parameterization[3] = + 1 - (res.parameterization[0] + res.parameterization[1] + + res.parameterization[2]); } res.sqr_distance = mindist; - } - else if(!ng) - { + } else if (!ng) { res = projectTriangleOrigin(a, b, c); res.parameterization[3] = 0; } return res; } +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp diff --git a/src/math/transform.cpp b/src/math/transform.cpp index 1e8c15bf8..550110c19 100644 --- a/src/math/transform.cpp +++ b/src/math/transform.cpp @@ -35,30 +35,23 @@ /** \author Jia Pan */ - #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { void relativeTransform(const Transform3f& tf1, const Transform3f& tf2, - Transform3f& tf) -{ - tf = tf1.inverseTimes (tf2); + Transform3f& tf) { + tf = tf1.inverseTimes(tf2); } void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2, - Transform3f& tf) -{ - Matrix3f R (tf2.getRotation() * tf1.getRotation().transpose()); + Transform3f& tf) { + Matrix3f R(tf2.getRotation() * tf1.getRotation().transpose()); tf = Transform3f(R, tf2.getTranslation() - R * tf1.getTranslation()); } +} // namespace fcl - -} - -} // namespace hpp +} // namespace hpp diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp index 5ff1e9143..634beba1f 100644 --- a/src/mesh_loader/assimp.cpp +++ b/src/mesh_loader/assimp.cpp @@ -33,16 +33,18 @@ */ #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) - #define nullptr NULL +#define nullptr NULL #endif #include -// Assimp >= 5.0 is forcing the use of C++11 keywords. A fix has been submitted https://github.com/assimp/assimp/pull/2758. -// The next lines fixes the bug for current version of hpp-fcl. +// Assimp >= 5.0 is forcing the use of C++11 keywords. A fix has been submitted +// https://github.com/assimp/assimp/pull/2758. The next lines fixes the bug for +// current version of hpp-fcl. #include -#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) && defined(AI_NO_EXCEPT) - #undef AI_NO_EXCEPT - #define AI_NO_EXCEPT +#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) && \ + defined(AI_NO_EXCEPT) +#undef AI_NO_EXCEPT +#define AI_NO_EXCEPT #endif #include @@ -52,62 +54,47 @@ #include #include -namespace hpp -{ -namespace fcl -{ - -namespace internal -{ +namespace hpp { +namespace fcl { -Loader::Loader () : importer (new Assimp::Importer()) -{ +namespace internal { + +Loader::Loader() : importer(new Assimp::Importer()) { // set list of ignored parameters (parameters used for rendering) - importer->SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, - aiComponent_TANGENTS_AND_BITANGENTS| - aiComponent_COLORS | - aiComponent_BONEWEIGHTS | - aiComponent_ANIMATIONS | - aiComponent_LIGHTS | - aiComponent_CAMERAS| - aiComponent_TEXTURES | - aiComponent_TEXCOORDS | - aiComponent_MATERIALS | - aiComponent_NORMALS - ); - + importer->SetPropertyInteger( + AI_CONFIG_PP_RVC_FLAGS, + aiComponent_TANGENTS_AND_BITANGENTS | aiComponent_COLORS | + aiComponent_BONEWEIGHTS | aiComponent_ANIMATIONS | + aiComponent_LIGHTS | aiComponent_CAMERAS | aiComponent_TEXTURES | + aiComponent_TEXCOORDS | aiComponent_MATERIALS | aiComponent_NORMALS); + // remove LINES and POINTS importer->SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT); - } -Loader::~Loader () -{ - if (importer) delete importer; +Loader::~Loader() { + if (importer) delete importer; } -void Loader::load (const std::string & resource_path) -{ - scene = importer->ReadFile(resource_path.c_str(), - aiProcess_SortByPType | - aiProcess_Triangulate | - aiProcess_RemoveComponent | - aiProcess_ImproveCacheLocality | - aiProcess_FindDegenerates | - aiProcess_JoinIdenticalVertices - ); - - if (!scene) - { - const std::string exception_message (std::string ("Could not load resource ") + resource_path + std::string("\n") + - importer->GetErrorString () + std::string("\n") + - "Hint: the mesh directory may be wrong."); +void Loader::load(const std::string& resource_path) { + scene = importer->ReadFile( + resource_path.c_str(), + aiProcess_SortByPType | aiProcess_Triangulate | + aiProcess_RemoveComponent | aiProcess_ImproveCacheLocality | + aiProcess_FindDegenerates | aiProcess_JoinIdenticalVertices); + + if (!scene) { + const std::string exception_message( + std::string("Could not load resource ") + resource_path + + std::string("\n") + importer->GetErrorString() + std::string("\n") + + "Hint: the mesh directory may be wrong."); throw std::invalid_argument(exception_message); } if (!scene->HasMeshes()) - throw std::invalid_argument (std::string ("No meshes found in file ")+resource_path); + throw std::invalid_argument(std::string("No meshes found in file ") + + resource_path); } /** @@ -119,72 +106,60 @@ void Loader::load (const std::string & resource_path) * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ -unsigned recurseBuildMesh ( - const fcl::Vec3f & scale, - const aiScene* scene, - const aiNode* node, - unsigned vertices_offset, - TriangleAndVertices & tv) -{ +unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, + const aiNode* node, unsigned vertices_offset, + TriangleAndVertices& tv) { if (!node) return 0; - + aiMatrix4x4 transform = node->mTransformation; - aiNode *pnode = node->mParent; - while (pnode) - { + aiNode* pnode = node->mParent; + while (pnode) { // Don't convert to y-up orientation, which is what the root node in // Assimp does - if (pnode->mParent != NULL) - { + if (pnode->mParent != NULL) { transform = pnode->mTransformation * transform; } pnode = pnode->mParent; } - + unsigned nbVertices = 0; - for (uint32_t i = 0; i < node->mNumMeshes; i++) - { + for (uint32_t i = 0; i < node->mNumMeshes; i++) { aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; - + // Add the vertices - for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) - { + for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) { aiVector3D p = input_mesh->mVertices[j]; p *= transform; - tv.vertices_.push_back (fcl::Vec3f (p.x * scale[0], - p.y * scale[1], - p.z * scale[2])); + tv.vertices_.push_back( + fcl::Vec3f(p.x * scale[0], p.y * scale[1], p.z * scale[2])); } - + // add the indices - for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) - { + for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) { aiFace& face = input_mesh->mFaces[j]; assert(face.mNumIndices == 3 && "The size of the face is not valid."); - tv.triangles_.push_back (fcl::Triangle(vertices_offset + face.mIndices[0], - vertices_offset + face.mIndices[1], - vertices_offset + face.mIndices[2])); + tv.triangles_.push_back( + fcl::Triangle(vertices_offset + face.mIndices[0], + vertices_offset + face.mIndices[1], + vertices_offset + face.mIndices[2])); } nbVertices += input_mesh->mNumVertices; } - - for (uint32_t i=0; i < node->mNumChildren; ++i) - { - nbVertices += recurseBuildMesh(scale, scene, node->mChildren[i], nbVertices, tv); + + for (uint32_t i = 0; i < node->mNumChildren; ++i) { + nbVertices += + recurseBuildMesh(scale, scene, node->mChildren[i], nbVertices, tv); } return nbVertices; } -void buildMesh (const fcl::Vec3f & scale, - const aiScene* scene, - unsigned vertices_offset, - TriangleAndVertices & tv) -{ - recurseBuildMesh (scale, scene, scene->mRootNode, vertices_offset, tv); +void buildMesh(const fcl::Vec3f& scale, const aiScene* scene, + unsigned vertices_offset, TriangleAndVertices& tv) { + recurseBuildMesh(scale, scene, scene->mRootNode, vertices_offset, tv); } -} // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace internal +} // namespace fcl +} // namespace hpp diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp index 02424f0eb..b874c54ec 100644 --- a/src/mesh_loader/loader.cpp +++ b/src/mesh_loader/loader.cpp @@ -38,72 +38,77 @@ #include #ifdef HPP_FCL_HAS_OCTOMAP -# include +#include #endif #include -namespace hpp -{ +namespace hpp { namespace fcl { - bool CachedMeshLoader::Key::operator< (const CachedMeshLoader::Key& b) const - { - const CachedMeshLoader::Key& a = *this; - for (int i = 0; i < 3; ++i) { - if (a.scale[i] < b.scale[i]) return true; - else if (a.scale[i] > b.scale[i]) return false; - } - return std::less() (a.filename, b.filename); +bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const { + const CachedMeshLoader::Key& a = *this; + for (int i = 0; i < 3; ++i) { + if (a.scale[i] < b.scale[i]) + return true; + else if (a.scale[i] > b.scale[i]) + return false; } + return std::less()(a.filename, b.filename); +} - template - BVHModelPtr_t _load (const std::string& filename, const Vec3f& scale) - { - shared_ptr < BVHModel > polyhedron (new BVHModel); - loadPolyhedronFromResource (filename, scale, polyhedron); - return polyhedron; - } +template +BVHModelPtr_t _load(const std::string& filename, const Vec3f& scale) { + shared_ptr > polyhedron(new BVHModel); + loadPolyhedronFromResource(filename, scale, polyhedron); + return polyhedron; +} - BVHModelPtr_t MeshLoader::load (const std::string& filename, - const Vec3f& scale) - { - switch (bvType_) { - case BV_AABB : return _load (filename, scale); - case BV_OBB : return _load (filename, scale); - case BV_RSS : return _load (filename, scale); - case BV_kIOS : return _load (filename, scale); - case BV_OBBRSS: return _load (filename, scale); - case BV_KDOP16: return _load > (filename, scale); - case BV_KDOP18: return _load > (filename, scale); - case BV_KDOP24: return _load > (filename, scale); - default: - throw std::invalid_argument("Unhandled bouding volume type."); - } +BVHModelPtr_t MeshLoader::load(const std::string& filename, + const Vec3f& scale) { + switch (bvType_) { + case BV_AABB: + return _load(filename, scale); + case BV_OBB: + return _load(filename, scale); + case BV_RSS: + return _load(filename, scale); + case BV_kIOS: + return _load(filename, scale); + case BV_OBBRSS: + return _load(filename, scale); + case BV_KDOP16: + return _load >(filename, scale); + case BV_KDOP18: + return _load >(filename, scale); + case BV_KDOP24: + return _load >(filename, scale); + default: + throw std::invalid_argument("Unhandled bouding volume type."); } +} - CollisionGeometryPtr_t MeshLoader::loadOctree (const std::string& filename) - { +CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) { #ifdef HPP_FCL_HAS_OCTOMAP - shared_ptr octree (new octomap::OcTree (filename)); - return CollisionGeometryPtr_t (new hpp::fcl::OcTree (octree)); + shared_ptr octree(new octomap::OcTree(filename)); + return CollisionGeometryPtr_t(new hpp::fcl::OcTree(octree)); #else - throw std::logic_error("hpp-fcl compiled without OctoMap. Cannot create OcTrees."); + throw std::logic_error( + "hpp-fcl compiled without OctoMap. Cannot create OcTrees."); #endif - } +} - BVHModelPtr_t CachedMeshLoader::load (const std::string& filename, - const Vec3f& scale) - { - Key key (filename, scale); - Cache_t::const_iterator _cached = cache_.find (key); - if (_cached == cache_.end()) { - BVHModelPtr_t geom = MeshLoader::load (filename, scale); - cache_.insert (std::make_pair(key, geom)); - return geom; - } else { - return _cached->second; - } +BVHModelPtr_t CachedMeshLoader::load(const std::string& filename, + const Vec3f& scale) { + Key key(filename, scale); + Cache_t::const_iterator _cached = cache_.find(key); + if (_cached == cache_.end()) { + BVHModelPtr_t geom = MeshLoader::load(filename, scale); + cache_.insert(std::make_pair(key, geom)); + return geom; + } else { + return _cached->second; } } +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index cddb6876b..fd73eef95 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -36,2648 +36,2479 @@ /** \author Jia Pan, Florent Lamiraux */ #ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H -# define HPP_FCL_SRC_NARROWPHASE_DETAILS_H +#define HPP_FCL_SRC_NARROWPHASE_DETAILS_H #include #include -namespace hpp -{ +namespace hpp { namespace fcl { - namespace details - { - // Compute the point on a line segment that is the closest point on the - // segment to to another point. The code is inspired by the explanation - // given by Dan Sunday's page: - // http://geomalgorithms.com/a02-_lines.html - static inline void lineSegmentPointClosestToPoint (const Vec3f &p, const Vec3f &s1, const Vec3f &s2, Vec3f &sp) { - Vec3f v = s2 - s1; - Vec3f w = p - s1; - - FCL_REAL c1 = w.dot(v); - FCL_REAL c2 = v.dot(v); - - if (c1 <= 0) { - sp = s1; - } else if (c2 <= c1) { - sp = s2; +namespace details { +// Compute the point on a line segment that is the closest point on the +// segment to to another point. The code is inspired by the explanation +// given by Dan Sunday's page: +// http://geomalgorithms.com/a02-_lines.html +static inline void lineSegmentPointClosestToPoint(const Vec3f& p, + const Vec3f& s1, + const Vec3f& s2, Vec3f& sp) { + Vec3f v = s2 - s1; + Vec3f w = p - s1; + + FCL_REAL c1 = w.dot(v); + FCL_REAL c2 = v.dot(v); + + if (c1 <= 0) { + sp = s1; + } else if (c2 <= c1) { + sp = s2; + } else { + FCL_REAL b = c1 / c2; + Vec3f Pb = s1 + v * b; + sp = Pb; + } +} + +inline bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1, + const Capsule& s2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f* contact_points, + Vec3f* normal_) { + Vec3f pos1( + tf2.transform(Vec3f(0., 0., s2.halfLength))); // from distance function + Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength))); + Vec3f s_c = tf1.getTranslation(); + + Vec3f segment_point; + + lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point); + Vec3f diff = s_c - segment_point; + + FCL_REAL diffN = diff.norm(); + distance = diffN - s1.radius - s2.radius; + + if (distance > 0) return false; + + // Vec3f normal (-diff.normalized()); + + if (normal_) *normal_ = -diff / diffN; + + if (contact_points) { + *contact_points = segment_point + diff * s2.radius; + } + + return true; +} + +inline bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, + const Capsule& s2, const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength))); + Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength))); + Vec3f s_c = tf1.getTranslation(); + + Vec3f segment_point; + + lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point); + normal = segment_point - s_c; + FCL_REAL norm(normal.norm()); + dist = norm - s1.radius - s2.radius; + + static const FCL_REAL eps(std::numeric_limits::epsilon()); + if (norm > eps) { + normal.normalize(); + } else { + normal << 1, 0, 0; + } + p1 = s_c + normal * s1.radius; + p2 = segment_point - normal * s2.radius; + + if (dist <= 0) { + p1 = p2 = .5 * (p1 + p2); + return false; + } + return true; +} + +inline bool sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, + const Cylinder& s2, const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + static const FCL_REAL eps(sqrt(std::numeric_limits::epsilon())); + FCL_REAL r1(s1.radius); + FCL_REAL r2(s2.radius); + FCL_REAL lz2(s2.halfLength); + // boundaries of the cylinder axis + Vec3f A(tf2.transform(Vec3f(0, 0, -lz2))); + Vec3f B(tf2.transform(Vec3f(0, 0, lz2))); + // Position of the center of the sphere + Vec3f S(tf1.getTranslation()); + // axis of the cylinder + Vec3f u(tf2.getRotation().col(2)); + /// @todo a tiny performance improvement could be achieved using the abscissa + /// with S as the origin + assert((B - A - (s2.halfLength * 2) * u).norm() < eps); + Vec3f AS(S - A); + // abscissa of S on cylinder axis with A as the origin + FCL_REAL s(u.dot(AS)); + Vec3f P(A + s * u); + Vec3f PS(S - P); + FCL_REAL dPS = PS.norm(); + // Normal to cylinder axis such that plane (A, u, v) contains sphere + // center + Vec3f v(0, 0, 0); + if (dPS > eps) { + // S is not on cylinder axis + v = (1 / dPS) * PS; + } + if (s <= 0) { + if (dPS <= r2) { + // closest point on cylinder is on cylinder disc basis + dist = -s - r1; + p1 = S + r1 * u; + p2 = A + dPS * v; + normal = u; + } else { + // closest point on cylinder is on cylinder circle basis + p2 = A + r2 * v; + Vec3f Sp2(p2 - S); + FCL_REAL dSp2 = Sp2.norm(); + if (dSp2 > eps) { + normal = (1 / dSp2) * Sp2; + p1 = S + r1 * normal; + dist = dSp2 - r1; + assert(fabs(dist) - (p1 - p2).norm() < eps); } else { - FCL_REAL b = c1/c2; - Vec3f Pb = s1 + v * b; - sp = Pb; + // Center of sphere is on cylinder boundary + normal = .5 * (A + B) - p2; + normal.normalize(); + p1 = p2; + dist = -r1; } } - - inline bool sphereCapsuleIntersect - (const Sphere& s1, const Transform3f& tf1, - const Capsule& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f* contact_points, Vec3f* normal_) - { - Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength))); // from distance function - Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength))); - Vec3f s_c = tf1.getTranslation (); - - Vec3f segment_point; - - lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point); - Vec3f diff = s_c - segment_point; - - FCL_REAL diffN = diff.norm(); - distance = diffN - s1.radius - s2.radius; - - if (distance > 0) - return false; - - // Vec3f normal (-diff.normalized()); - - if (normal_) - *normal_ = -diff / diffN; - - if (contact_points) { - *contact_points = segment_point + diff * s2.radius; - } - - return true; + } else if (s <= (s2.halfLength * 2)) { + // 0 < s <= s2.lz + normal = -v; + dist = dPS - r1 - r2; + if (dPS <= r2) { + // Sphere center is inside cylinder + p1 = p2 = S; + } else { + p2 = P + r2 * v; + p1 = S - r1 * v; } - - inline bool sphereCapsuleDistance - (const Sphere& s1, const Transform3f& tf1, - const Capsule& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) - { - Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength))); - Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength))); - Vec3f s_c = tf1.getTranslation (); - - Vec3f segment_point; - - lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point); - normal = segment_point - s_c; - FCL_REAL norm (normal.norm()); - dist = norm - s1.radius - s2.radius; - - static const FCL_REAL eps (std::numeric_limits::epsilon()); - if (norm > eps) { - normal.normalize(); + } else { + // lz < s + if (dPS <= r2) { + // closest point on cylinder is on cylinder disc basis + dist = s - (s2.halfLength * 2) - r1; + p1 = S - r1 * u; + p2 = B + dPS * v; + normal = -u; + } else { + // closest point on cylinder is on cylinder circle basis + p2 = B + r2 * v; + Vec3f Sp2(p2 - S); + FCL_REAL dSp2 = Sp2.norm(); + if (dSp2 > eps) { + normal = (1 / dSp2) * Sp2; + p1 = S + r1 * normal; + dist = dSp2 - r1; + assert(fabs(dist) - (p1 - p2).norm() < eps); } else { - normal << 1,0,0; - } - p1 = s_c + normal * s1.radius; - p2 = segment_point - normal * s2.radius; - - if(dist <= 0) { - p1 = p2 = .5 * (p1+p2); - return false; + // Center of sphere is on cylinder boundary + normal = .5 * (A + B) - p2; + normal.normalize(); + p1 = p2; + dist = -r1; } - return true; } - - inline bool sphereCylinderDistance - (const Sphere& s1, const Transform3f& tf1, - const Cylinder& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) - { - static const FCL_REAL eps (sqrt (std::numeric_limits ::epsilon ())); - FCL_REAL r1 (s1.radius); - FCL_REAL r2 (s2.radius); - FCL_REAL lz2 (s2.halfLength); - // boundaries of the cylinder axis - Vec3f A (tf2.transform (Vec3f (0, 0, -lz2))); - Vec3f B (tf2.transform (Vec3f (0, 0, lz2))); - // Position of the center of the sphere - Vec3f S (tf1.getTranslation ()); - // axis of the cylinder - Vec3f u (tf2.getRotation ().col (2)); - /// @todo a tiny performance improvement could be achieved using the abscissa with S as the origin - assert ((B - A - (s2.halfLength * 2) * u).norm () < eps); - Vec3f AS (S - A); - // abscissa of S on cylinder axis with A as the origin - FCL_REAL s (u.dot (AS)); - Vec3f P (A + s*u); - Vec3f PS (S - P); - FCL_REAL dPS = PS.norm (); - // Normal to cylinder axis such that plane (A, u, v) contains sphere - // center - Vec3f v (0, 0, 0); - if (dPS > eps) { - // S is not on cylinder axis - v = (1/dPS) * PS; - } - if (s <= 0) { - if (dPS <= r2) { - // closest point on cylinder is on cylinder disc basis - dist = -s - r1; p1 = S + r1 * u; p2 = A + dPS * v; normal = u; - } else { - // closest point on cylinder is on cylinder circle basis - p2 = A + r2 * v; - Vec3f Sp2 (p2 - S); - FCL_REAL dSp2 = Sp2.norm (); - if (dSp2 > eps) { - normal = (1/dSp2) * Sp2; - p1 = S + r1 * normal; - dist = dSp2 - r1; - assert (fabs (dist) - (p1-p2).norm () < eps); + } + if (dist < 0) { + p1 = p2 = .5 * (p1 + p2); + } + return (dist > 0); +} + +inline bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, + const Sphere& s2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f* contact_points, + Vec3f* normal) { + const Vec3f diff = tf2.getTranslation() - tf1.getTranslation(); + FCL_REAL len = diff.norm(); + distance = len - s1.radius - s2.radius; + if (distance > 0) return false; + + // 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) + *normal = diff / len; + else + *normal = diff; + } + + if (contact_points) + *contact_points = + tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius); + + return true; +} + +inline bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, + const Sphere& s2, const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + const Vec3f& o1 = tf1.getTranslation(); + const Vec3f& o2 = tf2.getTranslation(); + Vec3f diff = o1 - o2; + FCL_REAL len = diff.norm(); + normal = -diff / len; + dist = len - s1.radius - s2.radius; + + p1.noalias() = o1 + normal * s1.radius; + p2.noalias() = o2 - normal * s2.radius; + + return (dist >= 0); +} + +/** @brief the minimum distance from a point to a line */ +inline FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to, + const Vec3f& p, Vec3f& nearest) { + Vec3f diff = p - from; + Vec3f v = to - from; + FCL_REAL t = v.dot(diff); + + if (t > 0) { + FCL_REAL dotVV = v.squaredNorm(); + if (t < dotVV) { + t /= dotVV; + diff -= v * t; + } else { + t = 1; + diff -= v; + } + } else + t = 0; + + nearest.noalias() = from + v * t; + return diff.squaredNorm(); +} + +/// @brief Whether a point's projection is in a triangle +inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, + const Vec3f& normal, const Vec3f& p) { + Vec3f edge1(p2 - p1); + Vec3f edge2(p3 - p2); + Vec3f edge3(p1 - p3); + + Vec3f p1_to_p(p - p1); + Vec3f p2_to_p(p - p2); + Vec3f p3_to_p(p - p3); + + Vec3f edge1_normal(edge1.cross(normal)); + Vec3f edge2_normal(edge2.cross(normal)); + Vec3f edge3_normal(edge3.cross(normal)); + + FCL_REAL r1, r2, r3; + r1 = edge1_normal.dot(p1_to_p); + r2 = edge2_normal.dot(p2_to_p); + r3 = edge3_normal.dot(p3_to_p); + if ((r1 > 0 && r2 > 0 && r3 > 0) || (r1 <= 0 && r2 <= 0 && r3 <= 0)) + return true; + return false; +} + +// Intersection between sphere and triangle +// Sphere is in position tf1, Triangle is expressed in global frame +inline bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf1, + const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, FCL_REAL& distance, + Vec3f& p1, Vec3f& p2, Vec3f& normal_) { + Vec3f normal = (P2 - P1).cross(P3 - P1); + normal.normalize(); + const Vec3f& center = tf1.getTranslation(); + const FCL_REAL& radius = s.radius; + assert(radius >= 0); + Vec3f p1_to_center = center - P1; + FCL_REAL distance_from_plane = p1_to_center.dot(normal); + Vec3f closest_point( + Vec3f::Constant(std::numeric_limits::quiet_NaN())); + FCL_REAL min_distance_sqr, distance_sqr; + + if (distance_from_plane < 0) { + distance_from_plane *= -1; + normal *= -1; + } + + if (projectInTriangle(P1, P2, P3, normal, center)) { + closest_point = center - normal * distance_from_plane; + min_distance_sqr = distance_from_plane; + } else { + // Compute distance to each each and take minimal distance + Vec3f nearest_on_edge; + min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point); + + distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); + if (distance_sqr < min_distance_sqr) { + min_distance_sqr = distance_sqr; + closest_point = nearest_on_edge; + } + distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge); + if (distance_sqr < min_distance_sqr) { + min_distance_sqr = distance_sqr; + closest_point = nearest_on_edge; + } + } + + if (min_distance_sqr < radius * radius) { + // Collision + normal_ = (closest_point - center).normalized(); + p1 = p2 = closest_point; + distance = sqrt(min_distance_sqr) - radius; + assert(distance < 0); + return true; + } else { + normal_ = (closest_point - center).normalized(); + p1 = center + normal_ * radius; + p2 = closest_point; + distance = sqrt(min_distance_sqr) - radius; + assert(distance >= 0); + return false; + } +} + +inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf, + const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, FCL_REAL* dist) { + // from geometric tools, very different from the collision code. + + const Vec3f& center = tf.getTranslation(); + FCL_REAL radius = sp.radius; + Vec3f diff = P1 - center; + Vec3f edge0 = P2 - P1; + Vec3f edge1 = P3 - P1; + FCL_REAL a00 = edge0.squaredNorm(); + FCL_REAL a01 = edge0.dot(edge1); + FCL_REAL a11 = edge1.squaredNorm(); + FCL_REAL b0 = diff.dot(edge0); + FCL_REAL b1 = diff.dot(edge1); + FCL_REAL c = diff.squaredNorm(); + FCL_REAL det = fabs(a00 * a11 - a01 * a01); + FCL_REAL s = a01 * b1 - a11 * b0; + FCL_REAL t = a01 * b0 - a00 * b1; + + FCL_REAL sqr_dist; + + if (s + t <= det) { + if (s < 0) { + if (t < 0) // region 4 + { + if (b0 < 0) { + t = 0; + if (-b0 >= a00) { + s = 1; + sqr_dist = a00 + 2 * b0 + c; } else { - // Center of sphere is on cylinder boundary - normal = .5 * (A + B) - p2; normal.normalize (); - p1 = p2; - dist = -r1; + s = -b0 / a00; + sqr_dist = b0 * s + c; } - } - } else if (s <= (s2.halfLength * 2)) { - // 0 < s <= s2.lz - normal = -v; - dist = dPS - r1 - r2; - if (dPS <= r2) { - // Sphere center is inside cylinder - p1 = p2 = S; - } else { - p2 = P + r2*v; p1 = S - r1*v; - } - } else { - // lz < s - if (dPS <= r2) { - // closest point on cylinder is on cylinder disc basis - dist = s - (s2.halfLength * 2) - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u; } else { - // closest point on cylinder is on cylinder circle basis - p2 = B + r2 * v; - Vec3f Sp2 (p2 - S); - FCL_REAL dSp2 = Sp2.norm (); - if (dSp2 > eps) { - normal = (1/dSp2) * Sp2; - p1 = S + r1 * normal; - dist = dSp2 - r1; - assert (fabs (dist) - (p1-p2).norm () < eps); + s = 0; + if (b1 >= 0) { + t = 0; + sqr_dist = c; + } else if (-b1 >= a11) { + t = 1; + sqr_dist = a11 + 2 * b1 + c; } else { - // Center of sphere is on cylinder boundary - normal = .5 * (A + B) - p2; normal.normalize (); - p1 = p2; - dist = -r1; + t = -b1 / a11; + sqr_dist = b1 * t + c; } } + } else // region 3 + { + s = 0; + if (b1 >= 0) { + t = 0; + sqr_dist = c; + } else if (-b1 >= a11) { + t = 1; + sqr_dist = a11 + 2 * b1 + c; + } else { + t = -b1 / a11; + sqr_dist = b1 * t + c; + } } - if (dist < 0) { - p1 = p2 = .5 * (p1 + p2); - } - return (dist > 0); - } - - inline bool sphereSphereIntersect - (const Sphere& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f* contact_points, Vec3f* normal) + } else if (t < 0) // region 5 { - const Vec3f diff = tf2.getTranslation() - tf1.getTranslation(); - FCL_REAL len = diff.norm(); - distance = len - s1.radius - s2.radius; - if(distance > 0) - return false; - - // 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) - *normal = diff / len; - else - *normal = diff; - } - - if(contact_points) - *contact_points = tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius); - - return true; - } - - - inline bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, - Vec3f& normal) + t = 0; + if (b0 >= 0) { + s = 0; + sqr_dist = c; + } else if (-b0 >= a00) { + s = 1; + sqr_dist = a00 + 2 * b0 + c; + } else { + s = -b0 / a00; + sqr_dist = b0 * s + c; + } + } else // region 0 { - const Vec3f & o1 = tf1.getTranslation(); - const Vec3f & o2 = tf2.getTranslation(); - Vec3f diff = o1 - o2; - FCL_REAL len = diff.norm(); - normal = -diff/len; - dist = len - s1.radius - s2.radius; - - p1.noalias() = o1 + normal * s1.radius; - p2.noalias() = o2 - normal * s2.radius; - - return (dist >=0); + // minimum at interior point + FCL_REAL inv_det = (1) / det; + s *= inv_det; + t *= inv_det; + sqr_dist = s * (a00 * s + a01 * t + 2 * b0) + + t * (a01 * s + a11 * t + 2 * b1) + c; } + } else { + FCL_REAL tmp0, tmp1, numer, denom; - /** @brief the minimum distance from a point to a line */ - inline FCL_REAL segmentSqrDistance - (const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest) + if (s < 0) // region 2 { - Vec3f diff = p - from; - Vec3f v = to - from; - FCL_REAL t = v.dot(diff); - - if(t > 0) - { - FCL_REAL dotVV = v.squaredNorm(); - if(t < dotVV) - { - t /= dotVV; - diff -= v * t; - } - else - { - t = 1; - diff -= v; - } + tmp0 = a01 + b0; + tmp1 = a11 + b1; + if (tmp1 > tmp0) { + numer = tmp1 - tmp0; + denom = a00 - 2 * a01 + a11; + if (numer >= denom) { + s = 1; + t = 0; + sqr_dist = a00 + 2 * b0 + c; + } else { + s = numer / denom; + t = 1 - s; + sqr_dist = s * (a00 * s + a01 * t + 2 * b0) + + t * (a01 * s + a11 * t + 2 * b1) + c; } - else - t = 0; - - nearest.noalias() = from + v * t; - return diff.squaredNorm(); - } - - /// @brief Whether a point's projection is in a triangle - inline bool projectInTriangle (const Vec3f& p1, const Vec3f& p2, - const Vec3f& p3, const Vec3f& normal, - const Vec3f& p) - { - Vec3f edge1(p2 - p1); - Vec3f edge2(p3 - p2); - Vec3f edge3(p1 - p3); - - Vec3f p1_to_p(p - p1); - Vec3f p2_to_p(p - p2); - Vec3f p3_to_p(p - p3); - - Vec3f edge1_normal(edge1.cross(normal)); - Vec3f edge2_normal(edge2.cross(normal)); - Vec3f edge3_normal(edge3.cross(normal)); - - FCL_REAL r1, r2, r3; - r1 = edge1_normal.dot(p1_to_p); - r2 = edge2_normal.dot(p2_to_p); - r3 = edge3_normal.dot(p3_to_p); - if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || - ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) - return true; - return false; - } - - // Intersection between sphere and triangle - // Sphere is in position tf1, Triangle is expressed in global frame - inline bool sphereTriangleIntersect - (const Sphere& s, const Transform3f& tf1, - const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal_) - { - Vec3f normal = (P2 - P1).cross(P3 - P1); - normal.normalize(); - const Vec3f& center = tf1.getTranslation(); - const FCL_REAL& radius = s.radius; - assert (radius >= 0); - Vec3f p1_to_center = center - P1; - FCL_REAL distance_from_plane = p1_to_center.dot(normal); - Vec3f closest_point - (Vec3f::Constant(std::numeric_limits::quiet_NaN())); - FCL_REAL min_distance_sqr, distance_sqr; - - if(distance_from_plane < 0) { - distance_from_plane *= -1; - normal *= -1; - } - - if (projectInTriangle (P1, P2, P3, normal, center)) { - closest_point = center - normal * distance_from_plane; - min_distance_sqr = distance_from_plane; } else { - // Compute distance to each each and take minimal distance - Vec3f nearest_on_edge; - min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point); - - distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); - if (distance_sqr < min_distance_sqr) { - min_distance_sqr = distance_sqr; - closest_point = nearest_on_edge; - } - distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge); - if (distance_sqr < min_distance_sqr) { - min_distance_sqr = distance_sqr; - closest_point = nearest_on_edge; + s = 0; + if (tmp1 <= 0) { + t = 1; + sqr_dist = a11 + 2 * b1 + c; + } else if (b1 >= 0) { + t = 0; + sqr_dist = c; + } else { + t = -b1 / a11; + sqr_dist = b1 * t + c; } } - - if (min_distance_sqr < radius * radius) { - // Collision - normal_ = (closest_point - center).normalized (); - p1 = p2 = closest_point; - distance = sqrt (min_distance_sqr) - radius; - assert (distance < 0); - return true; + } else if (t < 0) // region 6 + { + tmp0 = a01 + b1; + tmp1 = a00 + b0; + if (tmp1 > tmp0) { + numer = tmp1 - tmp0; + denom = a00 - 2 * a01 + a11; + if (numer >= denom) { + t = 1; + s = 0; + sqr_dist = a11 + 2 * b1 + c; + } else { + t = numer / denom; + s = 1 - t; + sqr_dist = s * (a00 * s + a01 * t + 2 * b0) + + t * (a01 * s + a11 * t + 2 * b1) + c; + } } else { - normal_ = (closest_point - center).normalized (); - p1 = center + normal_ * radius; - p2 = closest_point; - distance = sqrt (min_distance_sqr) - radius; - assert (distance >= 0); - return false; + t = 0; + if (tmp1 <= 0) { + s = 1; + sqr_dist = a00 + 2 * b0 + c; + } else if (b0 >= 0) { + s = 0; + sqr_dist = c; + } else { + s = -b0 / a00; + sqr_dist = b0 * s + c; + } } - } - - inline bool sphereTriangleDistance - (const Sphere& sp, const Transform3f& tf, - const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist) + } else // region 1 { - // from geometric tools, very different from the collision code. - - const Vec3f& center = tf.getTranslation(); - FCL_REAL radius = sp.radius; - Vec3f diff = P1 - center; - Vec3f edge0 = P2 - P1; - Vec3f edge1 = P3 - P1; - FCL_REAL a00 = edge0.squaredNorm(); - FCL_REAL a01 = edge0.dot(edge1); - FCL_REAL a11 = edge1.squaredNorm(); - FCL_REAL b0 = diff.dot(edge0); - FCL_REAL b1 = diff.dot(edge1); - FCL_REAL c = diff.squaredNorm(); - FCL_REAL det = fabs(a00*a11 - a01*a01); - FCL_REAL s = a01*b1 - a11*b0; - FCL_REAL t = a01*b0 - a00*b1; - - FCL_REAL sqr_dist; - - if(s + t <= det) - { - if(s < 0) - { - if(t < 0) // region 4 - { - if(b0 < 0) - { - t = 0; - if(-b0 >= a00) - { - s = 1; - sqr_dist = a00 + 2*b0 + c; - } - else - { - s = -b0/a00; - sqr_dist = b0*s + c; - } - } - else - { - s = 0; - if(b1 >= 0) - { - t = 0; - sqr_dist = c; - } - else if(-b1 >= a11) - { - t = 1; - sqr_dist = a11 + 2*b1 + c; - } - else - { - t = -b1/a11; - sqr_dist = b1*t + c; - } - } - } - else // region 3 - { - s = 0; - if(b1 >= 0) - { - t = 0; - sqr_dist = c; - } - else if(-b1 >= a11) - { - t = 1; - sqr_dist = a11 + 2*b1 + c; - } - else - { - t = -b1/a11; - sqr_dist = b1*t + c; - } - } - } - else if(t < 0) // region 5 - { - t = 0; - if(b0 >= 0) - { - s = 0; - sqr_dist = c; - } - else if(-b0 >= a00) - { - s = 1; - sqr_dist = a00 + 2*b0 + c; - } - else - { - s = -b0/a00; - sqr_dist = b0*s + c; - } - } - else // region 0 - { - // minimum at interior point - FCL_REAL inv_det = (1)/det; - s *= inv_det; - t *= inv_det; - sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; - } - } - else - { - FCL_REAL tmp0, tmp1, numer, denom; - - if(s < 0) // region 2 - { - tmp0 = a01 + b0; - tmp1 = a11 + b1; - if(tmp1 > tmp0) - { - numer = tmp1 - tmp0; - denom = a00 - 2*a01 + a11; - if(numer >= denom) - { - s = 1; - t = 0; - sqr_dist = a00 + 2*b0 + c; - } - else - { - s = numer/denom; - t = 1 - s; - sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; - } - } - else - { - s = 0; - if(tmp1 <= 0) - { - t = 1; - sqr_dist = a11 + 2*b1 + c; - } - else if(b1 >= 0) - { - t = 0; - sqr_dist = c; - } - else - { - t = -b1/a11; - sqr_dist = b1*t + c; - } - } - } - else if(t < 0) // region 6 - { - tmp0 = a01 + b1; - tmp1 = a00 + b0; - if(tmp1 > tmp0) - { - numer = tmp1 - tmp0; - denom = a00 - 2*a01 + a11; - if(numer >= denom) - { - t = 1; - s = 0; - sqr_dist = a11 + 2*b1 + c; - } - else - { - t = numer/denom; - s = 1 - t; - sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; - } - } - else - { - t = 0; - if(tmp1 <= 0) - { - s = 1; - sqr_dist = a00 + 2*b0 + c; - } - else if(b0 >= 0) - { - s = 0; - sqr_dist = c; - } - else - { - s = -b0/a00; - sqr_dist = b0*s + c; - } - } - } - else // region 1 - { - numer = a11 + b1 - a01 - b0; - if(numer <= 0) - { - s = 0; - t = 1; - sqr_dist = a11 + 2*b1 + c; - } - else - { - denom = a00 - 2*a01 + a11; - if(numer >= denom) - { - s = 1; - t = 0; - sqr_dist = a00 + 2*b0 + c; - } - else - { - s = numer/denom; - t = 1 - s; - sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; - } - } - } + numer = a11 + b1 - a01 - b0; + if (numer <= 0) { + s = 0; + t = 1; + sqr_dist = a11 + 2 * b1 + c; + } else { + denom = a00 - 2 * a01 + a11; + if (numer >= denom) { + s = 1; + t = 0; + sqr_dist = a00 + 2 * b0 + c; + } else { + s = numer / denom; + t = 1 - s; + sqr_dist = s * (a00 * s + a01 * t + 2 * b0) + + t * (a01 * s + a11 * t + 2 * b1) + c; } - - // Account for numerical round-off error. - if(sqr_dist < 0) - sqr_dist = 0; - - if(sqr_dist > radius * radius) - { - if(dist) *dist = std::sqrt(sqr_dist) - radius; - return true; + } + } + } + + // Account for numerical round-off error. + if (sqr_dist < 0) sqr_dist = 0; + + if (sqr_dist > radius * radius) { + if (dist) *dist = std::sqrt(sqr_dist) - radius; + return true; + } else { + if (dist) *dist = -1; + return false; + } +} + +inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf, + const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, + Vec3f* p2) { + if (p1 || p2) { + const Vec3f& o = tf.getTranslation(); + Project::ProjectResult result; + result = Project::projectTriangle(P1, P2, P3, o); + if (result.sqr_distance > sp.radius * sp.radius) { + if (dist) *dist = std::sqrt(result.sqr_distance) - sp.radius; + Vec3f project_p = P1 * result.parameterization[0] + + P2 * result.parameterization[1] + + P3 * result.parameterization[2]; + Vec3f dir = o - project_p; + dir.normalize(); + if (p1) { + *p1 = o - dir * sp.radius; + } + if (p2) *p2 = project_p; + return true; + } else + return false; + } else { + return sphereTriangleDistance(sp, tf, P1, P2, P3, dist); + } +} + +inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1, + const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Transform3f& tf2, + FCL_REAL* dist, Vec3f* p1, Vec3f* p2) { + bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), + tf2.transform(P2), + tf2.transform(P3), dist, p1, p2); + return res; +} + +struct HPP_FCL_LOCAL ContactPoint { + Vec3f normal; + Vec3f point; + FCL_REAL depth; + ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) + : normal(n), point(p), depth(d) {} +}; + +static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua, + const Vec3f& pb, const Vec3f& ub, + FCL_REAL* alpha, FCL_REAL* beta) { + Vec3f p = pb - pa; + FCL_REAL uaub = ua.dot(ub); + FCL_REAL q1 = ua.dot(p); + FCL_REAL q2 = -ub.dot(p); + FCL_REAL d = 1 - uaub * uaub; + if (d <= (FCL_REAL)(0.0001f)) { + *alpha = 0; + *beta = 0; + } else { + d = 1 / d; + *alpha = (q1 + uaub * q2) * d; + *beta = (uaub * q1 + q2) * d; + } +} + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). +static unsigned int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], + FCL_REAL ret[16]) { + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + unsigned int nq = 4, nr = 0; + FCL_REAL buffer[16]; + FCL_REAL* q = p; + FCL_REAL* r = ret; + for (int dir = 0; dir <= 1; ++dir) { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign = -1; sign <= 1; sign += 2) { + // chop q along the line xy[dir] = sign*h[dir] + FCL_REAL* pq = q; + FCL_REAL* pr = r; + nr = 0; + for (unsigned int i = nq; i > 0; --i) { + // go through all points in q and all lines between adjacent points + if (sign * pq[dir] < h[dir]) { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } } - else - { - if(dist) *dist = -1; - return false; + FCL_REAL* nextq = (i > 1) ? pq + 2 : q; + if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir])) { + // this line crosses the chopping line + pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) / + (nextq[dir] - pq[dir]) * + (sign * h[dir] - pq[dir]); + pr[dir] = sign * h[dir]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } } + pq += 2; + } + q = r; + r = (q == ret) ? buffer : ret; + nq = nr; } - - - inline bool sphereTriangleDistance - (const Sphere& sp, const Transform3f& tf, const Vec3f& P1, - const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) - { - if(p1 || p2) - { - const Vec3f & o = tf.getTranslation(); - Project::ProjectResult result; - result = Project::projectTriangle(P1, P2, P3, o); - if(result.sqr_distance > sp.radius * sp.radius) - { - if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius; - Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2]; - Vec3f dir = o - project_p; - dir.normalize(); - if(p1) { *p1 = o - dir * sp.radius; } - if(p2) *p2 = project_p; - return true; - } - else - return false; - } + } + +done: + if (q != ret) memcpy(ret, q, nr * 2 * sizeof(FCL_REAL)); + return nr; +} + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. +static inline void cullPoints2(unsigned int n, FCL_REAL p[], unsigned int m, + unsigned int i0, unsigned int iret[]) { + // compute the centroid of the polygon in cx,cy + FCL_REAL a, cx, cy, q; + switch (n) { + case 1: + cx = p[0]; + cy = p[1]; + break; + case 2: + cx = 0.5 * (p[0] + p[2]); + cy = 0.5 * (p[1] + p[3]); + break; + default: + a = 0; + cx = 0; + cy = 0; + assert(n > 0 && "n should be positive"); + for (int i = 0; i < (int)n - 1; ++i) { + q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1]; + a += q; + cx += q * (p[i * 2] + p[i * 2 + 2]); + cy += q * (p[i * 2 + 1] + p[i * 2 + 3]); + } + q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1]; + if (std::abs(a + q) > std::numeric_limits::epsilon()) + a = 1 / (3 * (a + q)); else - { - return sphereTriangleDistance(sp, tf, P1, P2, P3, dist); + a = 1e18f; + + cx = a * (cx + q * (p[n * 2 - 2] + p[0])); + cy = a * (cy + q * (p[n * 2 - 1] + p[1])); + } + + // compute the angle of each point w.r.t. the centroid + FCL_REAL A[8]; + for (unsigned int i = 0; i < n; ++i) + A[i] = atan2(p[i * 2 + 1] - cy, p[i * 2] - cx); + + // search for points that have angles closest to A[i0] + i*(2*pi/m). + int avail[] = {1, 1, 1, 1, 1, 1, 1, 1}; + avail[i0] = 0; + iret[0] = i0; + iret++; + const double pi = boost::math::constants::pi(); + for (unsigned int j = 1; j < m; ++j) { + a = j * (2 * pi / m) + A[i0]; + if (a > pi) a -= 2 * pi; + FCL_REAL maxdiff = 1e9, diff; + + *iret = i0; // iret is not allowed to keep this value, but it sometimes + // does, when diff=#QNAN0 + for (unsigned int i = 0; i < n; ++i) { + if (avail[i]) { + diff = std::abs(A[i] - a); + if (diff > pi) diff = 2 * pi - diff; + if (diff < maxdiff) { + maxdiff = diff; + *iret = i; } + } } - - - inline bool sphereTriangleDistance - (const Sphere& sp, const Transform3f& tf1, const Vec3f& P1, - const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, - FCL_REAL* dist, Vec3f* p1, Vec3f* p2) - { - bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2); - return res; + avail[*iret] = 0; + iret++; + } +} + +inline unsigned int boxBox2(const Vec3f& halfSide1, const Matrix3f& R1, + const Vec3f& T1, const Vec3f& halfSide2, + const Matrix3f& R2, const Vec3f& T2, Vec3f& normal, + FCL_REAL* depth, int* return_code, + unsigned int maxc, + std::vector& contacts) { + const FCL_REAL fudge_factor = FCL_REAL(1.05); + Vec3f normalC; + FCL_REAL s, s2, l; + int invert_normal, code; + + Vec3f p(T2 - + T1); // get vector from centers of box 1 to box 2, relative to box 1 + Vec3f pp(R1.transpose() * p); // get pp = p relative to body 1 + + // get side lengths / 2 + const Vec3f& A(halfSide1); + const Vec3f& B(halfSide2); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + Matrix3f R(R1.transpose() * R2); + Matrix3f Q(R.cwiseAbs()); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + + int best_col_id = -1; + const Matrix3f* normalR = 0; + FCL_REAL tmp = 0; + + s = -(std::numeric_limits::max)(); + invert_normal = 0; + code = 0; + + // separating axis = u1, u2, u3 + tmp = pp[0]; + s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]); + if (s2 > 0) { + *return_code = 0; + return 0; + } + if (s2 > s) { + s = s2; + best_col_id = 0; + normalR = &R1; + invert_normal = (tmp < 0); + code = 1; + } + + tmp = pp[1]; + s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]); + if (s2 > 0) { + *return_code = 0; + return 0; + } + if (s2 > s) { + s = s2; + best_col_id = 1; + normalR = &R1; + invert_normal = (tmp < 0); + code = 2; + } + + tmp = pp[2]; + s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]); + if (s2 > 0) { + *return_code = 0; + return 0; + } + if (s2 > s) { + s = s2; + best_col_id = 2; + normalR = &R1; + invert_normal = (tmp < 0); + code = 3; + } + + // separating axis = v1, v2, v3 + tmp = R2.col(0).dot(p); + s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]); + if (s2 > 0) { + *return_code = 0; + return 0; + } + if (s2 > s) { + s = s2; + best_col_id = 0; + normalR = &R2; + invert_normal = (tmp < 0); + code = 4; + } + + tmp = R2.col(1).dot(p); + s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]); + if (s2 > 0) { + *return_code = 0; + return 0; + } + if (s2 > s) { + s = s2; + best_col_id = 1; + normalR = &R2; + invert_normal = (tmp < 0); + code = 5; + } + + tmp = R2.col(2).dot(p); + s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]); + if (s2 > 0) { + *return_code = 0; + return 0; + } + if (s2 > s) { + s = s2; + best_col_id = 2; + normalR = &R2; + invert_normal = (tmp < 0); + code = 6; + } + + FCL_REAL fudge2(1.0e-6); + Q.array() += fudge2; + + Vec3f n; + static const FCL_REAL eps = std::numeric_limits::epsilon(); + + // separating axis = u1 x (v1,v2,v3) + tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0); + s2 = std::abs(tmp) - + (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(0, -R(2, 0), R(1, 0)); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 7; } - - - - struct HPP_FCL_LOCAL ContactPoint - { - Vec3f normal; - Vec3f point; - FCL_REAL depth; - ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {} - }; - - - static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua, - const Vec3f& pb, const Vec3f& ub, - FCL_REAL* alpha, FCL_REAL* beta) - { - Vec3f p = pb - pa; - FCL_REAL uaub = ua.dot(ub); - FCL_REAL q1 = ua.dot(p); - FCL_REAL q2 = -ub.dot(p); - FCL_REAL d = 1 - uaub * uaub; - if(d <= (FCL_REAL)(0.0001f)) - { - *alpha = 0; - *beta = 0; - } - else - { - d = 1 / d; - *alpha = (q1 + uaub * q2) * d; - *beta = (uaub * q1 + q2) * d; - } + } + + tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1); + s2 = std::abs(tmp) - + (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(0, -R(2, 1), R(1, 1)); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 8; } - - // find all the intersection points between the 2D rectangle with vertices - // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), - // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). - // - // the intersection points are returned as x,y pairs in the 'ret' array. - // the number of intersection points is returned by the function (this will - // be in the range 0 to 8). - static unsigned int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16]) - { - // q (and r) contain nq (and nr) coordinate points for the current (and - // chopped) polygons - unsigned int nq = 4, nr = 0; - FCL_REAL buffer[16]; - FCL_REAL* q = p; - FCL_REAL* r = ret; - for(int dir = 0; dir <= 1; ++dir) - { - // direction notation: xy[0] = x axis, xy[1] = y axis - for(int sign = -1; sign <= 1; sign += 2) - { - // chop q along the line xy[dir] = sign*h[dir] - FCL_REAL* pq = q; - FCL_REAL* pr = r; - nr = 0; - for(unsigned int i = nq; i > 0; --i) - { - // go through all points in q and all lines between adjacent points - if(sign * pq[dir] < h[dir]) - { - // this point is inside the chopping line - pr[0] = pq[0]; - pr[1] = pq[1]; - pr += 2; - nr++; - if(nr & 8) - { - q = r; - goto done; - } - } - FCL_REAL* nextq = (i > 1) ? pq+2 : q; - if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) - { - // this line crosses the chopping line - pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / - (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); - pr[dir] = sign*h[dir]; - pr += 2; - nr++; - if(nr & 8) - { - q = r; - goto done; - } - } - pq += 2; - } - q = r; - r = (q == ret) ? buffer : ret; - nq = nr; - } - } - - done: - if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL)); - return nr; + } + + tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2); + s2 = std::abs(tmp) - + (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(0, -R(2, 2), R(1, 2)); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 9; } - - // given n points in the plane (array p, of size 2*n), generate m points that - // best represent the whole set. the definition of 'best' here is not - // predetermined - the idea is to select points that give good box-box - // collision detection behavior. the chosen point indexes are returned in the - // array iret (of size m). 'i0' is always the first entry in the array. - // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be - // in the range [0..n-1]. - static inline void cullPoints2(unsigned int n, FCL_REAL p[], unsigned int m, unsigned int i0, unsigned int iret[]) - { - // compute the centroid of the polygon in cx,cy - FCL_REAL a, cx, cy, q; - switch(n) - { - case 1: - cx = p[0]; - cy = p[1]; - break; - case 2: - cx = 0.5 * (p[0] + p[2]); - cy = 0.5 * (p[1] + p[3]); - break; - default: - a = 0; - cx = 0; - cy = 0; - assert(n > 0 && "n should be positive"); - for(int i = 0; i < (int)n-1; ++i) - { - q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; - a += q; - cx += q*(p[i*2]+p[i*2+2]); - cy += q*(p[i*2+1]+p[i*2+3]); - } - q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - if(std::abs(a+q) > std::numeric_limits::epsilon()) - a = 1/(3*(a+q)); - else - a= 1e18f; - - cx = a*(cx + q*(p[n*2-2]+p[0])); - cy = a*(cy + q*(p[n*2-1]+p[1])); - } - - - // compute the angle of each point w.r.t. the centroid - FCL_REAL A[8]; - for(unsigned int i = 0; i < n; ++i) - A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx); - - // search for points that have angles closest to A[i0] + i*(2*pi/m). - int avail[] = { 1, 1, 1, 1, 1, 1, 1, 1 }; - avail[i0] = 0; - iret[0] = i0; - iret++; - const double pi = boost::math::constants::pi(); - for(unsigned int j = 1; j < m; ++j) - { - a = j*(2*pi/m) + A[i0]; - if (a > pi) a -= 2*pi; - FCL_REAL maxdiff= 1e9, diff; - - *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 - for(unsigned int i = 0; i < n; ++i) - { - if(avail[i]) - { - diff = std::abs(A[i]-a); - if(diff > pi) diff = 2*pi - diff; - if(diff < maxdiff) - { - maxdiff = diff; - *iret = i; - } - } - } - avail[*iret] = 0; - iret++; - } + } + + // separating axis = u2 x (v1,v2,v3) + tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0); + s2 = std::abs(tmp) - + (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(R(2, 0), 0, -R(0, 0)); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 10; } + } + + tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1); + s2 = std::abs(tmp) - + (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(R(2, 1), 0, -R(0, 1)); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 11; + } + } + + tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2); + s2 = std::abs(tmp) - + (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(R(2, 2), 0, -R(0, 2)); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 12; + } + } + + // separating axis = u3 x (v1,v2,v3) + tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0); + s2 = std::abs(tmp) - + (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(-R(1, 0), R(0, 0), 0); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 13; + } + } + + tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1); + s2 = std::abs(tmp) - + (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(-R(1, 1), R(0, 1), 0); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 14; + } + } + + tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2); + s2 = std::abs(tmp) - + (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0)); + if (s2 > 0) { + *return_code = 0; + return 0; + } + n = Vec3f(-R(1, 2), R(0, 2), 0); + l = n.norm(); + if (l > eps) { + s2 /= l; + if (s2 * fudge_factor > s) { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 15; + } + } + if (!code) { + *return_code = code; + return 0; + } + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (best_col_id != -1) + normal = normalR->col(best_col_id); + else + normal.noalias() = R1 * normalC; - inline unsigned int boxBox2(const Vec3f& halfSide1, const Matrix3f& R1, const Vec3f& T1, - const Vec3f& halfSide2, const Matrix3f& R2, const Vec3f& T2, - Vec3f& normal, FCL_REAL* depth, int* return_code, - unsigned int maxc, std::vector& contacts) - { - const FCL_REAL fudge_factor = FCL_REAL(1.05); - Vec3f normalC; - FCL_REAL s, s2, l; - int invert_normal, code; - - Vec3f p (T2 - T1); // get vector from centers of box 1 to box 2, relative to box 1 - Vec3f pp (R1.transpose() * p); // get pp = p relative to body 1 - - // get side lengths / 2 - const Vec3f& A (halfSide1); - const Vec3f& B (halfSide2); - - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 - Matrix3f R (R1.transpose() * R2); - Matrix3f Q (R.cwiseAbs()); - - - // for all 15 possible separating axes: - // * see if the axis separates the boxes. if so, return 0. - // * find the depth of the penetration along the separating axis (s2) - // * if this is the largest depth so far, record it. - // the normal vector will be set to the separating axis with the smallest - // depth. note: normalR is set to point to a column of R1 or R2 if that is - // the smallest depth normal so far. otherwise normalR is 0 and normalC is - // set to a vector relative to body 1. invert_normal is 1 if the sign of - // the normal should be flipped. - - int best_col_id = -1; - const Matrix3f* normalR = 0; - FCL_REAL tmp = 0; - - s = - (std::numeric_limits::max)(); - invert_normal = 0; - code = 0; - - // separating axis = u1, u2, u3 - tmp = pp[0]; - s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 0; - normalR = &R1; - invert_normal = (tmp < 0); - code = 1; - } - - tmp = pp[1]; - s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 1; - normalR = &R1; - invert_normal = (tmp < 0); - code = 2; - } - - tmp = pp[2]; - s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 2; - normalR = &R1; - invert_normal = (tmp < 0); - code = 3; - } - - // separating axis = v1, v2, v3 - tmp = R2.col(0).dot(p); - s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 0; - normalR = &R2; - invert_normal = (tmp < 0); - code = 4; - } - - tmp = R2.col(1).dot(p); - s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 1; - normalR = &R2; - invert_normal = (tmp < 0); - code = 5; - } - - tmp = R2.col(2).dot(p); - s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 2; - normalR = &R2; - invert_normal = (tmp < 0); - code = 6; - } - - - FCL_REAL fudge2(1.0e-6); - Q.array() += fudge2; - - Vec3f n; - static const FCL_REAL eps = std::numeric_limits::epsilon(); - - // separating axis = u1 x (v1,v2,v3) - tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0); - s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(0, -R(2, 0), R(1, 0)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 7; - } - } - - tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1); - s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(0, -R(2, 1), R(1, 1)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 8; - } - } - - tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2); - s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(0, -R(2, 2), R(1, 2)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 9; - } - } - - // separating axis = u2 x (v1,v2,v3) - tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0); - s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(R(2, 0), 0, -R(0, 0)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 10; - } - } - - tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1); - s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(R(2, 1), 0, -R(0, 1)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 11; - } - } - - tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2); - s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(R(2, 2), 0, -R(0, 2)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 12; - } - } - - // separating axis = u3 x (v1,v2,v3) - tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0); - s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(-R(1, 0), R(0, 0), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 13; - } - } - - tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1); - s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(-R(1, 1), R(0, 1), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 14; - } - } - - tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2); - s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vec3f(-R(1, 2), R(0, 2), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 15; - } - } - - - - if (!code) { *return_code = code; return 0; } - - // if we get to this point, the boxes interpenetrate. compute the normal - // in global coordinates. - if(best_col_id != -1) - normal = normalR->col(best_col_id); - else - normal.noalias() = R1 * normalC; - - if(invert_normal) - normal = -normal; - - *depth = -s; // s is negative when the boxes are in collision - - // compute contact point(s) - - if(code > 6) - { - // an edge from box 1 touches an edge from box 2. - // find a point pa on the intersecting edge of box 1 - Vec3f pa(T1); - FCL_REAL sign; - - for(int j = 0; j < 3; ++j) - { - sign = (R1.col(j).dot(normal) > 0) ? 1 : -1; - pa += R1.col(j) * (A[j] * sign); - } - - // find a point pb on the intersecting edge of box 2 - Vec3f pb(T2); - - for(int j = 0; j < 3; ++j) - { - sign = (R2.col(j).dot(normal) > 0) ? -1 : 1; - pb += R2.col(j) * (B[j] * sign); - } - - FCL_REAL alpha, beta; - Vec3f ua(R1.col((code-7)/3)); - Vec3f ub(R2.col((code-7)%3)); - - lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); - pa += ua * alpha; - pb += ub * beta; - - - // Vec3f pointInWorld((pa + pb) * 0.5); - // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); - contacts.push_back(ContactPoint(-normal,pb,-*depth)); - *return_code = code; - - return 1; - } - - // okay, we have a face-something intersection (because the separating - // axis is perpendicular to a face). define face 'a' to be the reference - // face (i.e. the normal vector is perpendicular to this) and face 'b' to be - // the incident face (the closest face of the other box). - - const Matrix3f *Ra, *Rb; - const Vec3f *pa, *pb, *Sa, *Sb; - - if(code <= 3) - { - Ra = &R1; - Rb = &R2; - pa = &T1; - pb = &T2; - Sa = &A; - Sb = &B; - } - else - { - Ra = &R2; - Rb = &R1; - pa = &T2; - pb = &T1; - Sa = &B; - Sb = &A; - } + if (invert_normal) normal = -normal; - // nr = normal vector of reference face dotted with axes of incident box. - // anr = absolute values of nr. - Vec3f normal2, nr, anr; - if(code <= 3) - normal2 = normal; - else - normal2 = -normal; - - nr.noalias() = Rb->transpose() * normal2; - anr = nr.cwiseAbs(); - - // find the largest compontent of anr: this corresponds to the normal - // for the indident face. the other axis numbers of the indicent face - // are stored in a1,a2. - int lanr, a1, a2; - if(anr[1] > anr[0]) - { - if(anr[1] > anr[2]) - { - a1 = 0; - lanr = 1; - a2 = 2; - } - else - { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - else - { - if(anr[0] > anr[2]) - { - lanr = 0; - a1 = 1; - a2 = 2; - } - else - { - a1 = 0; - a2 = 1; - lanr = 2; - } - } + *depth = -s; // s is negative when the boxes are in collision - // compute center point of incident face, in reference-face coordinates - Vec3f center; - if(nr[lanr] < 0) - center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]); - else - center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]); - - // find the normal and non-normal axis numbers of the reference box - int codeN, code1, code2; - if(code <= 3) - codeN = code-1; - else codeN = code-4; - - if(codeN == 0) - { - code1 = 1; - code2 = 2; - } - else if(codeN == 1) - { - code1 = 0; - code2 = 2; - } - else - { - code1 = 0; - code2 = 1; - } + // compute contact point(s) - // find the four corners of the incident face, in reference-face coordinates - FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs) - FCL_REAL c1, c2, m11, m12, m21, m22; - c1 = Ra->col(code1).dot(center); - c2 = Ra->col(code2).dot(center); - // optimize this? - we have already computed this data above, but it is not - // stored in an easy-to-index format. for now it's quicker just to recompute - // the four dot products. - Vec3f tempRac = Ra->col(code1); - m11 = Rb->col(a1).dot(tempRac); - m12 = Rb->col(a2).dot(tempRac); - tempRac = Ra->col(code2); - m21 = Rb->col(a1).dot(tempRac); - m22 = Rb->col(a2).dot(tempRac); - - FCL_REAL k1 = m11 * (*Sb)[a1]; - FCL_REAL k2 = m21 * (*Sb)[a1]; - FCL_REAL k3 = m12 * (*Sb)[a2]; - FCL_REAL k4 = m22 * (*Sb)[a2]; - quad[0] = c1 - k1 - k3; - quad[1] = c2 - k2 - k4; - quad[2] = c1 - k1 + k3; - quad[3] = c2 - k2 + k4; - quad[4] = c1 + k1 + k3; - quad[5] = c2 + k2 + k4; - quad[6] = c1 + k1 - k3; - quad[7] = c2 + k2 - k4; - - // find the size of the reference face - FCL_REAL rect[2]; - rect[0] = (*Sa)[code1]; - rect[1] = (*Sa)[code2]; - - // intersect the incident and reference faces - FCL_REAL ret[16]; - const unsigned int n_intersect = intersectRectQuad2(rect, quad, ret); - if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen - - // convert the intersection points into reference-face coordinates, - // and compute the contact position and depth for each point. only keep - // those points that have a positive (penetrating) depth. delete points in - // the 'ret' array as necessary so that 'point' and 'ret' correspond. - Vec3f points[8]; // penetrating contact points - FCL_REAL dep[8]; // depths for those points - FCL_REAL det1 = 1.f/(m11*m22 - m12*m21); - m11 *= det1; - m12 *= det1; - m21 *= det1; - m22 *= det1; - unsigned int cnum = 0; // number of penetrating contact points found - for(unsigned int j = 0; j < n_intersect; ++j) - { - FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); - FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); - points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2; - dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); - if(dep[cnum] >= 0) - { - ret[cnum*2] = ret[j*2]; - ret[cnum*2+1] = ret[j*2+1]; - cnum++; - } - } - if(cnum < 1) { *return_code = code; return 0; } // this should never happen - - // we can't generate more contacts than we actually have - if(maxc > cnum) maxc = cnum; - if(maxc < 1) maxc = 1; - - if(cnum <= maxc) - { - if(code<4) - { - // we have less contacts than we need, so we use them all - for(unsigned int j = 0; j < cnum; ++j) - { - Vec3f pointInWorld = points[j] + (*pa); - contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); - } - } - else - { - // we have less contacts than we need, so we use them all - for(unsigned int j = 0; j < cnum; ++j) - { - Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j]; - contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); - } - } - } - else - { - // we have more contacts than are wanted, some of them must be culled. - // find the deepest point, it is always the first contact. - unsigned int i1 = 0; - FCL_REAL maxdepth = dep[0]; - for(unsigned int i = 1; i < cnum; ++i) - { - if(dep[i] > maxdepth) - { - maxdepth = dep[i]; - i1 = i; - } - } - - unsigned int iret[8]; - cullPoints2(cnum, ret, maxc, i1, iret); - - for(unsigned int j = 0; j < maxc; ++j) - { - Vec3f posInWorld = points[iret[j]] + (*pa); - if(code < 4) - contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]])); - else - contacts.push_back(ContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]])); - } - cnum = maxc; - } + if (code > 6) { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + Vec3f pa(T1); + FCL_REAL sign; - *return_code = code; - return cnum; + for (int j = 0; j < 3; ++j) { + sign = (R1.col(j).dot(normal) > 0) ? 1 : -1; + pa += R1.col(j) * (A[j] * sign); } - inline bool compareContactPoints - (const ContactPoint& c1,const ContactPoint& c2) - { - return c1.depth < c2.depth; - } // Accending order, assuming penetration depth is a negative number! - - inline bool boxBoxIntersect(const Box& s1, const Transform3f& tf1, - const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, - FCL_REAL* penetration_depth_, Vec3f* normal_) - { - std::vector contacts; - int return_code; - Vec3f normal; - FCL_REAL depth; - /* int cnum = */ boxBox2(s1.halfSide, tf1.getRotation(), tf1.getTranslation(), - s2.halfSide, tf2.getRotation(), tf2.getTranslation(), - normal, &depth, &return_code, - 4, contacts); - - if(normal_) *normal_ = normal; - if(penetration_depth_) *penetration_depth_ = depth; - - if(contact_points) - { - if(contacts.size() > 0) - { - std::sort(contacts.begin(), contacts.end(), compareContactPoints); - *contact_points = contacts[0].point; - if(penetration_depth_) *penetration_depth_ = -contacts[0].depth; - } - } + // find a point pb on the intersecting edge of box 2 + Vec3f pb(T2); - return return_code != 0; + for (int j = 0; j < 3; ++j) { + sign = (R2.col(j).dot(normal) > 0) ? -1 : 1; + pb += R2.col(j) * (B[j] * sign); } - template - inline T halfspaceIntersectTolerance() - { - return 0; - } - - template<> - inline float halfspaceIntersectTolerance() - { - return 0.0001f; - } - - template<> - inline double halfspaceIntersectTolerance() - { - return 0.0000001; + FCL_REAL alpha, beta; + Vec3f ua(R1.col((code - 7) / 3)); + Vec3f ub(R2.col((code - 7) % 3)); + + lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); + pa += ua * alpha; + pb += ub * beta; + + // Vec3f pointInWorld((pa + pb) * 0.5); + // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); + contacts.push_back(ContactPoint(-normal, pb, -*depth)); + *return_code = code; + + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const Matrix3f *Ra, *Rb; + const Vec3f *pa, *pb, *Sa, *Sb; + + if (code <= 3) { + Ra = &R1; + Rb = &R2; + pa = &T1; + pb = &T2; + Sa = &A; + Sb = &B; + } else { + Ra = &R2; + Rb = &R1; + pa = &T2; + pb = &T1; + Sa = &B; + Sb = &A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + Vec3f normal2, nr, anr; + if (code <= 3) + normal2 = normal; + else + normal2 = -normal; + + nr.noalias() = Rb->transpose() * normal2; + anr = nr.cwiseAbs(); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr, a1, a2; + if (anr[1] > anr[0]) { + if (anr[1] > anr[2]) { + a1 = 0; + lanr = 1; + a2 = 2; + } else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } else { + if (anr[0] > anr[2]) { + lanr = 0; + a1 = 1; + a2 = 2; + } else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + Vec3f center; + if (nr[lanr] < 0) + center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]); + else + center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]); + + // find the normal and non-normal axis numbers of the reference box + int codeN, code1, code2; + if (code <= 3) + codeN = code - 1; + else + codeN = code - 4; + + if (codeN == 0) { + code1 = 1; + code2 = 2; + } else if (codeN == 1) { + code1 = 0; + code2 = 2; + } else { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs) + FCL_REAL c1, c2, m11, m12, m21, m22; + c1 = Ra->col(code1).dot(center); + c2 = Ra->col(code2).dot(center); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + Vec3f tempRac = Ra->col(code1); + m11 = Rb->col(a1).dot(tempRac); + m12 = Rb->col(a2).dot(tempRac); + tempRac = Ra->col(code2); + m21 = Rb->col(a1).dot(tempRac); + m22 = Rb->col(a2).dot(tempRac); + + FCL_REAL k1 = m11 * (*Sb)[a1]; + FCL_REAL k2 = m21 * (*Sb)[a1]; + FCL_REAL k3 = m12 * (*Sb)[a2]; + FCL_REAL k4 = m22 * (*Sb)[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + + // find the size of the reference face + FCL_REAL rect[2]; + rect[0] = (*Sa)[code1]; + rect[1] = (*Sa)[code2]; + + // intersect the incident and reference faces + FCL_REAL ret[16]; + const unsigned int n_intersect = intersectRectQuad2(rect, quad, ret); + if (n_intersect < 1) { + *return_code = code; + return 0; + } // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + Vec3f points[8]; // penetrating contact points + FCL_REAL dep[8]; // depths for those points + FCL_REAL det1 = 1.f / (m11 * m22 - m12 * m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + unsigned int cnum = 0; // number of penetrating contact points found + for (unsigned int j = 0; j < n_intersect; ++j) { + FCL_REAL k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2); + FCL_REAL k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2); + points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2; + dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); + if (dep[cnum] >= 0) { + ret[cnum * 2] = ret[j * 2]; + ret[cnum * 2 + 1] = ret[j * 2 + 1]; + cnum++; + } + } + if (cnum < 1) { + *return_code = code; + return 0; + } // this should never happen + + // we can't generate more contacts than we actually have + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; + + if (cnum <= maxc) { + if (code < 4) { + // we have less contacts than we need, so we use them all + for (unsigned int j = 0; j < cnum; ++j) { + Vec3f pointInWorld = points[j] + (*pa); + contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); } - - inline bool sphereHalfspaceIntersect - (const Sphere& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - { - Halfspace new_s2 = transform(s2, tf2); - const Vec3f& center = tf1.getTranslation(); - distance = new_s2.signedDistance(center) - s1.radius; - if(distance <= 0) - { - normal = -new_s2.n; // pointing from s1 to s2 - p1 = p2 = center - new_s2.n * s1.radius - (distance * 0.5) * new_s2.n; - return true; + } else { + // we have less contacts than we need, so we use them all + for (unsigned int j = 0; j < cnum; ++j) { + Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j]; + contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); } - else { - p1 = center - s1.radius * new_s2.n; - p2 = p1 - distance * new_s2.n; - return false; - } - } - - /// @brief box half space, a, b, c = +/- edge size - /// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d - /// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d - /// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c - /// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough - inline bool boxHalfspaceIntersect - (const Box& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2) - { - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3f& R = tf1.getRotation(); - const Vec3f& T = tf1.getTranslation(); - - Vec3f Q (R.transpose() * new_s2.n); - - FCL_REAL depth = Q.cwiseProduct(s1.halfSide).lpNorm<1>() - new_s2.signedDistance(T); - return (depth >= 0); } - - - inline bool boxHalfspaceIntersect - (const Box& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) - { - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3f& R = tf1.getRotation(); - const Vec3f& T = tf1.getTranslation(); - - // Q: plane normal expressed in box frame - const Vec3f Q (R.transpose() * new_s2.n); - // A: scalar products of each side with normal - const Vec3f A (Q.cwiseProduct(s1.halfSide)); - - distance = new_s2.signedDistance(T) - A.lpNorm<1>(); - if(distance > 0) { - p1.noalias() = T + R * (A.array() > 0).select (s1.halfSide, - s1.halfSide); - p2.noalias() = p1 - distance * new_s2.n; - return false; + } else { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + unsigned int i1 = 0; + FCL_REAL maxdepth = dep[0]; + for (unsigned int i = 1; i < cnum; ++i) { + if (dep[i] > maxdepth) { + maxdepth = dep[i]; + i1 = i; } - - /// find deepest point - Vec3f p(T); - int sign = 0; - - if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance()) - { - sign = (A[0] > 0) ? -1 : 1; - p += R.col(0) * (s1.halfSide[0] * sign); - } - else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance()) - { - sign = (A[1] > 0) ? -1 : 1; - p += R.col(1) * (s1.halfSide[1] * sign); - } - else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance()) - { - sign = (A[2] > 0) ? -1 : 1; - p += R.col(2) * (s1.halfSide[2] * sign); - } - else - { - p.noalias() += R * (A.array() > 0).select (-s1.halfSide, s1.halfSide); - } - - /// compute the contact point from the deepest point - normal = -new_s2.n; - p1 = p2 = p - new_s2.n * (distance * 0.5); - - return true; } - inline bool capsuleHalfspaceIntersect - (const Capsule& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - { - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3f& R = tf1.getRotation(); - const Vec3f& T = tf1.getTranslation(); - - Vec3f dir_z = R.col(2); - - FCL_REAL cosa = dir_z.dot(new_s2.n); - if(std::abs(cosa) < halfspaceIntersectTolerance()) - { - // Capsule parallel to plane - FCL_REAL signed_dist = new_s2.signedDistance(T); - distance = signed_dist - s1.radius; - if(distance > 0) { - p1 = T - s1.radius * new_s2.n; - p2 = p1 - distance * new_s2.n; - return false; - } + unsigned int iret[8]; + cullPoints2(cnum, ret, maxc, i1, iret); - normal = -new_s2.n; - p1 = p2 = T + new_s2.n * (-0.5 * distance - s1.radius); - return true; - } + for (unsigned int j = 0; j < maxc; ++j) { + Vec3f posInWorld = points[iret[j]] + (*pa); + if (code < 4) + contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]])); else - { - int sign = (cosa > 0) ? -1 : 1; - // closest capsule vertex to halfspace if no collision, - // or deeper inside halspace if collision - Vec3f p = T + dir_z * (s1.halfLength * sign); - - FCL_REAL signed_dist = new_s2.signedDistance(p); - distance = signed_dist - s1.radius; - if(distance > 0) { - p1 = T - s1.radius * new_s2.n; - p2 = p1 - distance * new_s2.n; - return false; - } - normal = -new_s2.n; - // deepest point - Vec3f c = p - new_s2.n * s1.radius; - p1 = p2 = c - (0.5 * distance) * new_s2.n; - return true; - } + contacts.push_back(ContactPoint( + -normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]])); } - - - inline bool cylinderHalfspaceIntersect - (const Cylinder& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - { - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3f& R = tf1.getRotation(); - const Vec3f& T = tf1.getTranslation(); - - Vec3f dir_z = R.col(2); - FCL_REAL cosa = dir_z.dot(new_s2.n); - - if(cosa < halfspaceIntersectTolerance()) - { - FCL_REAL signed_dist = new_s2.signedDistance(T); - distance = signed_dist - s1.radius; - if(distance > 0) { - // TODO: compute closest points - p1 = p2 = Vec3f(0, 0, 0); - return false; - } - - normal = -new_s2.n; - p1 = p2 = T - (0.5 * distance + s1.radius) * new_s2.n; - return true; - } - else - { - Vec3f C = dir_z * cosa - new_s2.n; - if(std::abs(cosa + 1) < halfspaceIntersectTolerance() || - std::abs(cosa - 1) < halfspaceIntersectTolerance()) - C = Vec3f(0, 0, 0); - else - { - FCL_REAL s = C.norm(); - s = s1.radius / s; - C *= s; - } - - int sign = (cosa > 0) ? -1 : 1; - // deepest point - Vec3f p = T + dir_z * (s1.halfLength * sign) + C; - distance = new_s2.signedDistance(p); - if(distance > 0) { - // TODO: compute closest points - p1 = p2 = Vec3f(0, 0, 0); - return false; - } - else - { - normal = -new_s2.n; - p1 = p2 = p - (0.5 * distance) * new_s2.n; - return true; - } - } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + +inline bool compareContactPoints(const ContactPoint& c1, + const ContactPoint& c2) { + return c1.depth < c2.depth; +} // Accending order, assuming penetration depth is a negative number! + +inline bool boxBoxIntersect(const Box& s1, const Transform3f& tf1, + const Box& s2, const Transform3f& tf2, + Vec3f* contact_points, FCL_REAL* penetration_depth_, + Vec3f* normal_) { + std::vector contacts; + int return_code; + Vec3f normal; + FCL_REAL depth; + /* int cnum = */ boxBox2(s1.halfSide, tf1.getRotation(), tf1.getTranslation(), + s2.halfSide, tf2.getRotation(), tf2.getTranslation(), + normal, &depth, &return_code, 4, contacts); + + if (normal_) *normal_ = normal; + if (penetration_depth_) *penetration_depth_ = depth; + + if (contact_points) { + if (contacts.size() > 0) { + std::sort(contacts.begin(), contacts.end(), compareContactPoints); + *contact_points = contacts[0].point; + if (penetration_depth_) *penetration_depth_ = -contacts[0].depth; } - - - inline bool coneHalfspaceIntersect - (const Cone& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - { - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3f& R = tf1.getRotation(); - const Vec3f& T = tf1.getTranslation(); - - Vec3f dir_z = R.col(2); - FCL_REAL cosa = dir_z.dot(new_s2.n); - - if(cosa < halfspaceIntersectTolerance()) - { - FCL_REAL signed_dist = new_s2.signedDistance(T); - distance = signed_dist - s1.radius; - if(distance > 0) { - p1 = p2 = Vec3f (0, 0, 0); - return false; - } - else - { - normal = -new_s2.n; - p1 = p2 = T - dir_z * (s1.halfLength) - - new_s2.n * (0.5 * distance + s1.radius); - return true; - } - } - else - { - Vec3f C = dir_z * cosa - new_s2.n; - if(std::abs(cosa + 1) < halfspaceIntersectTolerance() || - std::abs(cosa - 1) < halfspaceIntersectTolerance()) - C = Vec3f(0, 0, 0); - else - { - FCL_REAL s = C.norm(); - s = s1.radius / s; - C *= s; - } - - Vec3f a1 = T + dir_z * (s1.halfLength); - Vec3f a2 = T - dir_z * (s1.halfLength) + C; - - FCL_REAL d1 = new_s2.signedDistance(a1); - FCL_REAL d2 = new_s2.signedDistance(a2); - - if(d1 > 0 && d2 > 0) return false; - else - { - distance = std::min(d1, d2); - normal = -new_s2.n; - p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 * distance) * new_s2.n; - return true; - } - } + } + + return return_code != 0; +} + +template +inline T halfspaceIntersectTolerance() { + return 0; +} + +template <> +inline float halfspaceIntersectTolerance() { + return 0.0001f; +} + +template <> +inline double halfspaceIntersectTolerance() { + return 0.0000001; +} + +inline bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1, + const Halfspace& s2, + const Transform3f& tf2, FCL_REAL& distance, + Vec3f& p1, Vec3f& p2, Vec3f& normal) { + Halfspace new_s2 = transform(s2, tf2); + const Vec3f& center = tf1.getTranslation(); + distance = new_s2.signedDistance(center) - s1.radius; + if (distance <= 0) { + normal = -new_s2.n; // pointing from s1 to s2 + p1 = p2 = center - new_s2.n * s1.radius - (distance * 0.5) * new_s2.n; + return true; + } else { + p1 = center - s1.radius * new_s2.n; + p2 = p1 - distance * new_s2.n; + return false; + } +} + +/// @brief box half space, a, b, c = +/- edge size +/// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d +/// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d +/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c +/// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, +/// check that is enough +inline bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, + const Halfspace& s2, const Transform3f& tf2) { + Halfspace new_s2 = transform(s2, tf2); + + const Matrix3f& R = tf1.getRotation(); + const Vec3f& T = tf1.getTranslation(); + + Vec3f Q(R.transpose() * new_s2.n); + + FCL_REAL depth = + Q.cwiseProduct(s1.halfSide).lpNorm<1>() - new_s2.signedDistance(T); + return (depth >= 0); +} + +inline bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, + const Halfspace& s2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Halfspace new_s2 = transform(s2, tf2); + + const Matrix3f& R = tf1.getRotation(); + const Vec3f& T = tf1.getTranslation(); + + // Q: plane normal expressed in box frame + const Vec3f Q(R.transpose() * new_s2.n); + // A: scalar products of each side with normal + const Vec3f A(Q.cwiseProduct(s1.halfSide)); + + distance = new_s2.signedDistance(T) - A.lpNorm<1>(); + if (distance > 0) { + p1.noalias() = T + R * (A.array() > 0).select(s1.halfSide, -s1.halfSide); + p2.noalias() = p1 - distance * new_s2.n; + return false; + } + + /// find deepest point + Vec3f p(T); + int sign = 0; + + if (std::abs(Q[0] - 1) < halfspaceIntersectTolerance() || + std::abs(Q[0] + 1) < halfspaceIntersectTolerance()) { + sign = (A[0] > 0) ? -1 : 1; + p += R.col(0) * (s1.halfSide[0] * sign); + } else if (std::abs(Q[1] - 1) < halfspaceIntersectTolerance() || + std::abs(Q[1] + 1) < halfspaceIntersectTolerance()) { + sign = (A[1] > 0) ? -1 : 1; + p += R.col(1) * (s1.halfSide[1] * sign); + } else if (std::abs(Q[2] - 1) < halfspaceIntersectTolerance() || + std::abs(Q[2] + 1) < halfspaceIntersectTolerance()) { + sign = (A[2] > 0) ? -1 : 1; + p += R.col(2) * (s1.halfSide[2] * sign); + } else { + p.noalias() += R * (A.array() > 0).select(-s1.halfSide, s1.halfSide); + } + + /// compute the contact point from the deepest point + normal = -new_s2.n; + p1 = p2 = p - new_s2.n * (distance * 0.5); + + return true; +} + +inline bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1, + const Halfspace& s2, + const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Halfspace new_s2 = transform(s2, tf2); + + const Matrix3f& R = tf1.getRotation(); + const Vec3f& T = tf1.getTranslation(); + + Vec3f dir_z = R.col(2); + + FCL_REAL cosa = dir_z.dot(new_s2.n); + if (std::abs(cosa) < halfspaceIntersectTolerance()) { + // Capsule parallel to plane + FCL_REAL signed_dist = new_s2.signedDistance(T); + distance = signed_dist - s1.radius; + if (distance > 0) { + p1 = T - s1.radius * new_s2.n; + p2 = p1 - distance * new_s2.n; + return false; } - inline bool convexHalfspaceIntersect - (const ConvexBase& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) - { - Halfspace new_s2 = transform(s2, tf2); - - Vec3f v; - FCL_REAL depth = (std::numeric_limits::max)(); - - for(unsigned int i = 0; i < s1.num_points; ++i) - { - Vec3f p = tf1.transform(s1.points[i]); - - FCL_REAL d = new_s2.signedDistance(p); - if(d < depth) - { - depth = d; - v = p; - } - } - - if(depth <= 0) - { - if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth); - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - return true; - } - else - return false; + normal = -new_s2.n; + p1 = p2 = T + new_s2.n * (-0.5 * distance - s1.radius); + return true; + } else { + int sign = (cosa > 0) ? -1 : 1; + // closest capsule vertex to halfspace if no collision, + // or deeper inside halspace if collision + Vec3f p = T + dir_z * (s1.halfLength * sign); + + FCL_REAL signed_dist = new_s2.signedDistance(p); + distance = signed_dist - s1.radius; + if (distance > 0) { + p1 = T - s1.radius * new_s2.n; + p2 = p1 - distance * new_s2.n; + return false; } - - // Intersection between half-space and triangle - // Half-space is in pose tf1, - // Triangle is in pose tf2 - // Computes distance and closest points (p1, p2) if no collision, - // contact point (p1 = p2), normal and penetration depth (-distance) - // if collision - inline bool halfspaceTriangleIntersect - (const Halfspace& s1, const Transform3f& tf1, const Vec3f& P1, - const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) - { - Halfspace new_s1 = transform(s1, tf1); - - Vec3f v = tf2.transform(P1); - FCL_REAL depth = new_s1.signedDistance(v); - - Vec3f p = tf2.transform(P2); - FCL_REAL d = new_s1.signedDistance(p); - if(d < depth) - { - depth = d; - v = p; - } - - p = tf2.transform(P3); - d = new_s1.signedDistance(p); - if(d < depth) - { - depth = d; - v = p; - } - // v is the vertex with minimal projection abscissa (depth) on normal to - //plane, - distance = depth; - if(depth <= 0) - { - normal = new_s1.n; - p1 = p2 = v - (0.5 * depth) * new_s1.n; - return true; - } - else - { - p1 = v - depth * new_s1.n; - p2 = v; - return false; - } + normal = -new_s2.n; + // deepest point + Vec3f c = p - new_s2.n * s1.radius; + p1 = p2 = c - (0.5 * distance) * new_s2.n; + return true; + } +} + +inline bool cylinderHalfspaceIntersect(const Cylinder& s1, + const Transform3f& tf1, + const Halfspace& s2, + const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Halfspace new_s2 = transform(s2, tf2); + + const Matrix3f& R = tf1.getRotation(); + const Vec3f& T = tf1.getTranslation(); + + Vec3f dir_z = R.col(2); + FCL_REAL cosa = dir_z.dot(new_s2.n); + + if (cosa < halfspaceIntersectTolerance()) { + FCL_REAL signed_dist = new_s2.signedDistance(T); + distance = signed_dist - s1.radius; + if (distance > 0) { + // TODO: compute closest points + p1 = p2 = Vec3f(0, 0, 0); + return false; } + normal = -new_s2.n; + p1 = p2 = T - (0.5 * distance + s1.radius) * new_s2.n; + return true; + } else { + Vec3f C = dir_z * cosa - new_s2.n; + if (std::abs(cosa + 1) < halfspaceIntersectTolerance() || + std::abs(cosa - 1) < halfspaceIntersectTolerance()) + C = Vec3f(0, 0, 0); + else { + FCL_REAL s = C.norm(); + s = s1.radius / s; + C *= s; + } - /// @brief return whether plane collides with halfspace - /// if the separation plane of the halfspace is parallel with the plane - /// return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl - /// return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl - /// plane is outside halfspace, collision-free - /// if not parallel - /// return the intersection ray, return code 3. ray origin is p and direction is d - inline bool planeHalfspaceIntersect - (const Plane& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, Plane& pl, Vec3f& p, - Vec3f& d, FCL_REAL& penetration_depth, int& ret) - { - Plane new_s1 = transform(s1, tf1); - Halfspace new_s2 = transform(s2, tf2); - - ret = 0; - - penetration_depth = (std::numeric_limits::max)(); - Vec3f dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_norm = dir.squaredNorm(); - if(dir_norm < std::numeric_limits::epsilon()) // parallel - { - if((new_s1.n).dot(new_s2.n) > 0) - { - penetration_depth = new_s2.d - new_s1.d; - if(penetration_depth < 0) - return false; - else - { - ret = 1; - pl = new_s1; - return true; - } - } - else - { - penetration_depth = -(new_s1.d + new_s2.d); - if(penetration_depth < 0) - return false; - else - { - ret = 2; - pl = new_s1; - return true; - } - } - } - - Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; - Vec3f origin = n.cross(dir); - origin *= (1.0 / dir_norm); - - p = origin; - d = dir; - ret = 3; - + int sign = (cosa > 0) ? -1 : 1; + // deepest point + Vec3f p = T + dir_z * (s1.halfLength * sign) + C; + distance = new_s2.signedDistance(p); + if (distance > 0) { + // TODO: compute closest points + p1 = p2 = Vec3f(0, 0, 0); + return false; + } else { + normal = -new_s2.n; + p1 = p2 = p - (0.5 * distance) * new_s2.n; return true; } + } +} - ///@ brief return whether two halfspace intersect - /// if the separation planes of the two halfspaces are parallel - /// return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s; - /// return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s; - /// return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other; - /// collision free, if two halfspaces' are separate; - /// if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d - /// collision free return code 0 - inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1, +inline bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f& p, Vec3f& d, - Halfspace& s, - FCL_REAL& penetration_depth, int& ret) - { - Halfspace new_s1 = transform(s1, tf1); - Halfspace new_s2 = transform(s2, tf2); - - ret = 0; - - penetration_depth = (std::numeric_limits::max)(); - Vec3f dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_norm = dir.squaredNorm(); - if(dir_norm < std::numeric_limits::epsilon()) // parallel - { - if((new_s1.n).dot(new_s2.n) > 0) - { - if(new_s1.d < new_s2.d) // s1 is inside s2 - { - ret = 1; - s = new_s1; - } - else // s2 is inside s1 - { - ret = 2; - s = new_s2; - } - return true; - } - else - { - penetration_depth = -(new_s1.d + new_s2.d); - if(penetration_depth < 0) // not collision - return false; - else // in each other - { - ret = 3; - return true; - } - } - } + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Halfspace new_s2 = transform(s2, tf2); - Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; - Vec3f origin = n.cross(dir); - origin *= (1.0 / dir_norm); + const Matrix3f& R = tf1.getRotation(); + const Vec3f& T = tf1.getTranslation(); - p = origin; - d = dir; - ret = 4; + Vec3f dir_z = R.col(2); + FCL_REAL cosa = dir_z.dot(new_s2.n); + if (cosa < halfspaceIntersectTolerance()) { + FCL_REAL signed_dist = new_s2.signedDistance(T); + distance = signed_dist - s1.radius; + if (distance > 0) { + p1 = p2 = Vec3f(0, 0, 0); + return false; + } else { + normal = -new_s2.n; + p1 = p2 = + T - dir_z * (s1.halfLength) - new_s2.n * (0.5 * distance + s1.radius); return true; } - - /// @param p1 closest (or most penetrating) point on the Halfspace, - /// @param p2 closest (or most penetrating) point on the shape, - /// @param normal the halfspace normal. - /// @return true if the distance is negative (the shape overlaps). - inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1, - const ShapeBase& s, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - { - Vec3f n_w = tf1.getRotation() * h.n; - Vec3f n_2 (tf2.getRotation().transpose() * n_w); - int hint = 0; - p2 = getSupport(&s, -n_2, true, hint); - p2 = tf2.transform(p2); - - dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d; - p1 = p2 - dist * n_w; - normal = n_w; - - return dist <= 0; + } else { + Vec3f C = dir_z * cosa - new_s2.n; + if (std::abs(cosa + 1) < halfspaceIntersectTolerance() || + std::abs(cosa - 1) < halfspaceIntersectTolerance()) + C = Vec3f(0, 0, 0); + else { + FCL_REAL s = C.norm(); + s = s1.radius / s; + C *= s; } - template - inline T planeIntersectTolerance() - { - return 0; - } - - template<> - inline double planeIntersectTolerance() - { - return 0.0000001; - } - - template<> - float inline planeIntersectTolerance() - { - return 0.0001f; - } + Vec3f a1 = T + dir_z * (s1.halfLength); + Vec3f a2 = T - dir_z * (s1.halfLength) + C; - inline bool spherePlaneIntersect - (const Sphere& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - // Vec3f& contact_points, FCL_REAL& penetration_depth, Vec3f* normal) - { - Plane new_s2 = transform(s2, tf2); + FCL_REAL d1 = new_s2.signedDistance(a1); + FCL_REAL d2 = new_s2.signedDistance(a2); - const Vec3f& center = tf1.getTranslation(); - FCL_REAL signed_dist = new_s2.signedDistance(center); - distance = std::abs(signed_dist) - s1.radius; - if(distance <= 0) - { - if (signed_dist > 0) - normal = -new_s2.n; - else - normal = new_s2.n; - p1 = p2 = center - new_s2.n * signed_dist; - return true; - } - else - { - if (signed_dist > 0) - { - p1 = center - s1.radius * new_s2.n; - p2 = center - signed_dist * new_s2.n; - } - else - { - p1 = center + s1.radius * new_s2.n; - p2 = center + signed_dist * new_s2.n; - } - return false; - } + if (d1 > 0 && d2 > 0) + return false; + else { + distance = std::min(d1, d2); + normal = -new_s2.n; + p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 * distance) * new_s2.n; + return true; } + } +} - /// @brief box half space, a, b, c = +/- edge size - /// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d - /// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d - /// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c - /// so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows: - /// (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. - /// if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side. - inline bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - // Vec3f* contact_points, - // FCL_REAL* penetration_depth, Vec3f* normal) - { - static const FCL_REAL eps (sqrt (std::numeric_limits::epsilon())); - const Plane new_s2 = transform(s2, tf2); - - const Matrix3f& R = tf1.getRotation(); - const Vec3f& T = tf1.getTranslation(); - - const Vec3f Q (R.transpose() * new_s2.n); - const Vec3f A (Q.cwiseProduct(s1.halfSide)); - - const FCL_REAL signed_dist = new_s2.signedDistance(T); - distance = std::abs(signed_dist) - A.lpNorm<1>(); - if(distance > 0) { - // Is the box above or below the plane - const bool positive = signed_dist > 0; - // Set p1 at the center of the box - p1 = T; - for (Vec3f::Index i=0; i<3; ++i) { - // scalar product between box axis and plane normal - FCL_REAL alpha ((positive ? 1 : -1 ) * R.col (i).dot (new_s2.n)); - if (alpha > eps) { - p1 -= R.col (i) * s1.halfSide [i]; - } else if (alpha < -eps) { - p1 += R.col (i) * s1.halfSide [i]; - } - } - p2 = p1 - ( positive ? distance : -distance) * new_s2.n; - assert (new_s2.distance (p2) < 3 *eps); - return false; - } +inline bool convexHalfspaceIntersect( + const ConvexBase& s1, const Transform3f& tf1, const Halfspace& s2, + const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, + Vec3f* normal) { + Halfspace new_s2 = transform(s2, tf2); - // find the deepest point - Vec3f p = T; + Vec3f v; + FCL_REAL depth = (std::numeric_limits::max)(); - // when center is on the positive side of the plane, use a, b, c - // make (R^T n) (a v1 + b v2 + c v3) the minimum - // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum - int sign = (signed_dist > 0) ? 1 : -1; + for (unsigned int i = 0; i < s1.num_points; ++i) { + Vec3f p = tf1.transform(s1.points[i]); - if(std::abs(Q[0] - 1) < planeIntersectTolerance() || - std::abs(Q[0] + 1) < planeIntersectTolerance()) - { - int sign2 = (A[0] > 0) ? -sign : sign; - p.noalias() += R.col(0) * (s1.halfSide[0] * sign2); - } - else if(std::abs(Q[1] - 1) < planeIntersectTolerance() || - std::abs(Q[1] + 1) < planeIntersectTolerance()) - { - int sign2 = (A[1] > 0) ? -sign : sign; - p.noalias() += R.col(1) * (s1.halfSide[1] * sign2); - } - else if(std::abs(Q[2] - 1) < planeIntersectTolerance() || - std::abs(Q[2] + 1) < planeIntersectTolerance()) - { - int sign2 = (A[2] > 0) ? -sign : sign; - p.noalias() += R.col(2) * (s1.halfSide[2] * sign2); - } - else - { - Vec3f tmp(sign * R * s1.halfSide); - p.noalias() += (A.array() > 0).select (- tmp, tmp); - } - - // compute the contact point by project the deepest point onto the plane - if (signed_dist > 0) normal = -new_s2.n; else normal = new_s2.n; - p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p); - - return true; + FCL_REAL d = new_s2.signedDistance(p); + if (d < depth) { + depth = d; + v = p; } - - /// Taken from book Real Time Collision Detection, from Christer Ericson - /// @param pb the closest point to the sphere center on the box surface - /// @param ps when colliding, matches pb, which is inside the sphere. - /// when not colliding, the closest point on the sphere - /// @param normal direction of motion of the box - /// @return true if the distance is negative (the shape overlaps). - inline bool boxSphereDistance(const Box & b, const Transform3f& tfb, - const Sphere& s, const Transform3f& tfs, - FCL_REAL& dist, Vec3f& pb, Vec3f& ps, - Vec3f& normal) - { - const Vec3f& os = tfs.getTranslation(); - const Vec3f& ob = tfb.getTranslation(); - const Matrix3f& Rb = tfb.getRotation(); - - pb = ob; - - bool outside = false; - const Vec3f os_in_b_frame (Rb.transpose() * (os - ob)); - int axis = -1; - FCL_REAL min_d = (std::numeric_limits::max)(); - for (int i = 0; i < 3; ++i) { - FCL_REAL facedist; - if (os_in_b_frame(i) < - b.halfSide(i)) { // outside - pb.noalias() -= b.halfSide(i) * Rb.col(i); - outside = true; - } else if (os_in_b_frame(i) > b.halfSide(i)) { // outside - pb.noalias() += b.halfSide(i) * Rb.col(i); - outside = true; - } else { - pb.noalias() += os_in_b_frame(i) * Rb.col(i); - if (!outside && (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) { - axis = i; - min_d = facedist; - } - } - } - normal.noalias() = pb - os; - FCL_REAL pdist = normal.norm(); - if (outside) { // pb is on the box - dist = pdist - s.radius; - normal /= - pdist; - } else { // pb is inside the box - if (os_in_b_frame(axis) >= 0 ) normal = Rb.col(axis); - else normal = -Rb.col(axis); - dist = - min_d - s.radius; - } - if (!outside || dist <= 0) { - ps = pb; + } + + if (depth <= 0) { + if (contact_points) *contact_points = v - new_s2.n * (0.5 * depth); + if (penetration_depth) *penetration_depth = depth; + if (normal) *normal = -new_s2.n; + return true; + } else + return false; +} + +// Intersection between half-space and triangle +// Half-space is in pose tf1, +// Triangle is in pose tf2 +// Computes distance and closest points (p1, p2) if no collision, +// contact point (p1 = p2), normal and penetration depth (-distance) +// if collision +inline bool halfspaceTriangleIntersect(const Halfspace& s1, + const Transform3f& tf1, const Vec3f& P1, + const Vec3f& P2, const Vec3f& P3, + const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Halfspace new_s1 = transform(s1, tf1); + + Vec3f v = tf2.transform(P1); + FCL_REAL depth = new_s1.signedDistance(v); + + Vec3f p = tf2.transform(P2); + FCL_REAL d = new_s1.signedDistance(p); + if (d < depth) { + depth = d; + v = p; + } + + p = tf2.transform(P3); + d = new_s1.signedDistance(p); + if (d < depth) { + depth = d; + v = p; + } + // v is the vertex with minimal projection abscissa (depth) on normal to + // plane, + distance = depth; + if (depth <= 0) { + normal = new_s1.n; + p1 = p2 = v - (0.5 * depth) * new_s1.n; + return true; + } else { + p1 = v - depth * new_s1.n; + p2 = v; + return false; + } +} + +/// @brief return whether plane collides with halfspace +/// if the separation plane of the halfspace is parallel with the plane +/// return code 1, if the plane's normal is the same with halfspace's normal +/// and plane is inside halfspace, also return plane in pl return code 2, if +/// the plane's normal is oppositie to the halfspace's normal and plane is +/// inside halfspace, also return plane in pl plane is outside halfspace, +/// collision-free +/// if not parallel +/// return the intersection ray, return code 3. ray origin is p and +/// direction is d +inline bool planeHalfspaceIntersect(const Plane& s1, const Transform3f& tf1, + const Halfspace& s2, const Transform3f& tf2, + Plane& pl, Vec3f& p, Vec3f& d, + FCL_REAL& penetration_depth, int& ret) { + Plane new_s1 = transform(s1, tf1); + Halfspace new_s2 = transform(s2, tf2); + + ret = 0; + + penetration_depth = (std::numeric_limits::max)(); + Vec3f dir = (new_s1.n).cross(new_s2.n); + FCL_REAL dir_norm = dir.squaredNorm(); + if (dir_norm < std::numeric_limits::epsilon()) // parallel + { + if ((new_s1.n).dot(new_s2.n) > 0) { + penetration_depth = new_s2.d - new_s1.d; + if (penetration_depth < 0) + return false; + else { + ret = 1; + pl = new_s1; return true; - } else { - ps = os - s.radius * normal; + } + } else { + penetration_depth = -(new_s1.d + new_s2.d); + if (penetration_depth < 0) return false; + else { + ret = 2; + pl = new_s1; + return true; } } - - inline bool capsulePlaneIntersect - (const Capsule& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - { - Plane new_s2 = transform(s2, tf2); - - // position orientation of capsule - const Matrix3f& R1 = tf1.getRotation(); - const Vec3f& T1 = tf1.getTranslation(); - - Vec3f dir_z = R1.col(2); - - // ends of capsule inner segment - Vec3f a1 = T1 + dir_z * s1.halfLength; - Vec3f a2 = T1 - dir_z * s1.halfLength; - - FCL_REAL d1 = new_s2.signedDistance(a1); - FCL_REAL d2 = new_s2.signedDistance(a2); - - FCL_REAL abs_d1 = std::abs(d1); - FCL_REAL abs_d2 = std::abs(d2); - - // two end points on different side of the plane - // the contact point is the intersect of axis with the plane - // the normal is the direction to avoid intersection - // the depth is the minimum distance to resolve the collision - if(d1 * d2 < -planeIntersectTolerance()) + } + + Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; + Vec3f origin = n.cross(dir); + origin *= (1.0 / dir_norm); + + p = origin; + d = dir; + ret = 3; + + return true; +} + +///@ brief return whether two halfspace intersect +/// if the separation planes of the two halfspaces are parallel +/// return code 1, if two halfspaces' normal are same and s1 is in s2, also +/// return s1 in s; return code 2, if two halfspaces' normal are same and s2 +/// is in s1, also return s2 in s; return code 3, if two halfspaces' normal +/// are opposite and s1 and s2 are into each other; collision free, if two +/// halfspaces' are separate; +/// if the separation planes of the two halfspaces are not parallel, return +/// intersection ray, return code 4. ray origin is p and direction is d +/// collision free return code 0 +inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1, + const Halfspace& s2, const Transform3f& tf2, + Vec3f& p, Vec3f& d, Halfspace& s, + FCL_REAL& penetration_depth, int& ret) { + Halfspace new_s1 = transform(s1, tf1); + Halfspace new_s2 = transform(s2, tf2); + + ret = 0; + + penetration_depth = (std::numeric_limits::max)(); + Vec3f dir = (new_s1.n).cross(new_s2.n); + FCL_REAL dir_norm = dir.squaredNorm(); + if (dir_norm < std::numeric_limits::epsilon()) // parallel + { + if ((new_s1.n).dot(new_s2.n) > 0) { + if (new_s1.d < new_s2.d) // s1 is inside s2 { - if(abs_d1 < abs_d2) - { - distance = -abs_d1 - s1.radius; - p1 = p2 = a1 * (abs_d2 / (abs_d1 + abs_d2)) + - a2 * (abs_d1 / (abs_d1 + abs_d2)); - if (d1 < 0) normal = -new_s2.n; else normal = new_s2.n; - } - else - { - distance = -abs_d2 - s1.radius; - p1 = p2 = a1 * (abs_d2 / (abs_d1 + abs_d2)) + - a2 * (abs_d1 / (abs_d1 + abs_d2)); - if (d2 < 0) normal = -new_s2.n; else normal = new_s2.n; - } - assert (!p1.hasNaN () && !p2.hasNaN ()); - return true; + ret = 1; + s = new_s1; + } else // s2 is inside s1 + { + ret = 2; + s = new_s2; } - - if(abs_d1 > s1.radius && abs_d2 > s1.radius) { - // Here both capsule ends are on the same side of the plane - if (d1 > 0) normal = new_s2.n; else normal = -new_s2.n; - if (abs_d1 < abs_d2) { - distance = abs_d1 - s1.radius; - p1 = a1 - s1.radius * normal; - p2 = p1 - distance * normal; - } else { - distance = abs_d2 - s1.radius; - p1 = a2 - s1.radius * normal; - p2 = p1 - distance * normal; - } - assert (!p1.hasNaN () && !p2.hasNaN ()); + return true; + } else { + penetration_depth = -(new_s1.d + new_s2.d); + if (penetration_depth < 0) // not collision return false; - } - else + else // in each other { - // Both capsule ends are on the same side of the plane, but one - // is closer than the capsule radius, hence collision - distance = std::min(abs_d1, abs_d2) - s1.radius; - - if(abs_d1 <= s1.radius && abs_d2 <= s1.radius) - { - Vec3f c1 = a1 - new_s2.n * d1; - Vec3f c2 = a2 - new_s2.n * d2; - p1 = p2 = (c1 + c2) * 0.5; - } - else if(abs_d1 <= s1.radius) - { - Vec3f c = a1 - new_s2.n * d1; - p1 = p2 = c; - } - else if(abs_d2 <= s1.radius) - { - Vec3f c = a2 - new_s2.n * d2; - p1 = p2 = c; - } else { - assert (false); - } - - if (d1 < 0) normal = new_s2.n; else normal = -new_s2.n; - assert (!p1.hasNaN () && !p2.hasNaN ()); + ret = 3; return true; } - assert (false); } + } + + Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; + Vec3f origin = n.cross(dir); + origin *= (1.0 / dir_norm); + + p = origin; + d = dir; + ret = 4; + + return true; +} + +/// @param p1 closest (or most penetrating) point on the Halfspace, +/// @param p2 closest (or most penetrating) point on the shape, +/// @param normal the halfspace normal. +/// @return true if the distance is negative (the shape overlaps). +inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1, + const ShapeBase& s, const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Vec3f n_w = tf1.getRotation() * h.n; + Vec3f n_2(tf2.getRotation().transpose() * n_w); + int hint = 0; + p2 = getSupport(&s, -n_2, true, hint); + p2 = tf2.transform(p2); + + dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d; + p1 = p2 - dist * n_w; + normal = n_w; + + return dist <= 0; +} + +template +inline T planeIntersectTolerance() { + return 0; +} + +template <> +inline double planeIntersectTolerance() { + return 0.0000001; +} + +template <> +float inline planeIntersectTolerance() { + return 0.0001f; +} + +inline bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) +// Vec3f& contact_points, FCL_REAL& penetration_depth, Vec3f* normal) +{ + Plane new_s2 = transform(s2, tf2); - /// @brief cylinder-plane intersect - /// n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d - /// need one point to be positive and one to be negative - /// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 - /// (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0 - inline bool cylinderPlaneIntersect - (const Cylinder& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - { - Plane new_s2 = transform(s2, tf2); - - const Matrix3f& R = tf1.getRotation(); - const Vec3f& T = tf1.getTranslation(); - - Vec3f dir_z = R.col(2); - FCL_REAL cosa = dir_z.dot(new_s2.n); - - if(std::abs(cosa) < planeIntersectTolerance()) - { - FCL_REAL d = new_s2.signedDistance(T); - distance = std::abs(d) - s1.radius; - if(distance > 0) return false; - else - { - if (d < 0) normal = new_s2.n; - else normal = -new_s2.n; - p1 = p2 = T - new_s2.n * d; - return true; - } + const Vec3f& center = tf1.getTranslation(); + FCL_REAL signed_dist = new_s2.signedDistance(center); + distance = std::abs(signed_dist) - s1.radius; + if (distance <= 0) { + if (signed_dist > 0) + normal = -new_s2.n; + else + normal = new_s2.n; + p1 = p2 = center - new_s2.n * signed_dist; + return true; + } else { + if (signed_dist > 0) { + p1 = center - s1.radius * new_s2.n; + p2 = center - signed_dist * new_s2.n; + } else { + p1 = center + s1.radius * new_s2.n; + p2 = center + signed_dist * new_s2.n; + } + return false; + } +} + +/// @brief box half space, a, b, c = +/- edge size +/// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d +/// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d +/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c +/// and <=0 for some a, b, c so need to check whether |d - n * T| <= |(R^T n)(a +/// v1 + b v2 + c v3)|, the reason is as follows: (R^T n) (a v1 + b v2 + c v3) +/// can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. if |d - n * T| <= +/// |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value +/// on the right side. +inline bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) +// Vec3f* contact_points, +// FCL_REAL* penetration_depth, Vec3f* normal) +{ + static const FCL_REAL eps(sqrt(std::numeric_limits::epsilon())); + const Plane new_s2 = transform(s2, tf2); + + const Matrix3f& R = tf1.getRotation(); + const Vec3f& T = tf1.getTranslation(); + + const Vec3f Q(R.transpose() * new_s2.n); + const Vec3f A(Q.cwiseProduct(s1.halfSide)); + + const FCL_REAL signed_dist = new_s2.signedDistance(T); + distance = std::abs(signed_dist) - A.lpNorm<1>(); + if (distance > 0) { + // Is the box above or below the plane + const bool positive = signed_dist > 0; + // Set p1 at the center of the box + p1 = T; + for (Vec3f::Index i = 0; i < 3; ++i) { + // scalar product between box axis and plane normal + FCL_REAL alpha((positive ? 1 : -1) * R.col(i).dot(new_s2.n)); + if (alpha > eps) { + p1 -= R.col(i) * s1.halfSide[i]; + } else if (alpha < -eps) { + p1 += R.col(i) * s1.halfSide[i]; } - else - { - Vec3f C = dir_z * cosa - new_s2.n; - if(std::abs(cosa + 1) < planeIntersectTolerance() || - std::abs(cosa - 1) < planeIntersectTolerance()) - C = Vec3f(0, 0, 0); - else - { - FCL_REAL s = C.norm(); - s = s1.radius / s; - C *= s; - } - - Vec3f a1 = T + dir_z * (s1.halfLength); - Vec3f a2 = T - dir_z * (s1.halfLength); - - Vec3f c1, c2; - if(cosa > 0) - { - c1 = a1 - C; - c2 = a2 + C; - } - else - { - c1 = a1 + C; - c2 = a2 - C; - } - - FCL_REAL d1 = new_s2.signedDistance(c1); - FCL_REAL d2 = new_s2.signedDistance(c2); - - if(d1 * d2 <= 0) - { - FCL_REAL abs_d1 = std::abs(d1); - FCL_REAL abs_d2 = std::abs(d2); - - if(abs_d1 > abs_d2) - { - distance = -abs_d2; - p1 = p2 = c2 - new_s2.n * d2; - if (d2 < 0) normal = -new_s2.n; else normal = new_s2.n; - } - else - { - distance = -abs_d1; - p1 = p2 = c1 - new_s2.n * d1; - if (d1 < 0) normal = -new_s2.n; - else normal = new_s2.n; - } - return true; - } - else - return false; + } + p2 = p1 - (positive ? distance : -distance) * new_s2.n; + assert(new_s2.distance(p2) < 3 * eps); + return false; + } + + // find the deepest point + Vec3f p = T; + + // when center is on the positive side of the plane, use a, b, c + // make (R^T n) (a v1 + b v2 + c v3) the minimum + // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum + int sign = (signed_dist > 0) ? 1 : -1; + + if (std::abs(Q[0] - 1) < planeIntersectTolerance() || + std::abs(Q[0] + 1) < planeIntersectTolerance()) { + int sign2 = (A[0] > 0) ? -sign : sign; + p.noalias() += R.col(0) * (s1.halfSide[0] * sign2); + } else if (std::abs(Q[1] - 1) < planeIntersectTolerance() || + std::abs(Q[1] + 1) < planeIntersectTolerance()) { + int sign2 = (A[1] > 0) ? -sign : sign; + p.noalias() += R.col(1) * (s1.halfSide[1] * sign2); + } else if (std::abs(Q[2] - 1) < planeIntersectTolerance() || + std::abs(Q[2] + 1) < planeIntersectTolerance()) { + int sign2 = (A[2] > 0) ? -sign : sign; + p.noalias() += R.col(2) * (s1.halfSide[2] * sign2); + } else { + Vec3f tmp(sign * R * s1.halfSide); + p.noalias() += (A.array() > 0).select(-tmp, tmp); + } + + // compute the contact point by project the deepest point onto the plane + if (signed_dist > 0) + normal = -new_s2.n; + else + normal = new_s2.n; + p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p); + + return true; +} + +/// Taken from book Real Time Collision Detection, from Christer Ericson +/// @param pb the closest point to the sphere center on the box surface +/// @param ps when colliding, matches pb, which is inside the sphere. +/// when not colliding, the closest point on the sphere +/// @param normal direction of motion of the box +/// @return true if the distance is negative (the shape overlaps). +inline bool boxSphereDistance(const Box& b, const Transform3f& tfb, + const Sphere& s, const Transform3f& tfs, + FCL_REAL& dist, Vec3f& pb, Vec3f& ps, + Vec3f& normal) { + const Vec3f& os = tfs.getTranslation(); + const Vec3f& ob = tfb.getTranslation(); + const Matrix3f& Rb = tfb.getRotation(); + + pb = ob; + + bool outside = false; + const Vec3f os_in_b_frame(Rb.transpose() * (os - ob)); + int axis = -1; + FCL_REAL min_d = (std::numeric_limits::max)(); + for (int i = 0; i < 3; ++i) { + FCL_REAL facedist; + if (os_in_b_frame(i) < -b.halfSide(i)) { // outside + pb.noalias() -= b.halfSide(i) * Rb.col(i); + outside = true; + } else if (os_in_b_frame(i) > b.halfSide(i)) { // outside + pb.noalias() += b.halfSide(i) * Rb.col(i); + outside = true; + } else { + pb.noalias() += os_in_b_frame(i) * Rb.col(i); + if (!outside && + (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) { + axis = i; + min_d = facedist; } } - - inline bool conePlaneIntersect - (const Cone& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) - { - Plane new_s2 = transform(s2, tf2); - - const Matrix3f& R = tf1.getRotation(); - const Vec3f& T = tf1.getTranslation(); - - Vec3f dir_z = R.col(2); - FCL_REAL cosa = dir_z.dot(new_s2.n); - - if(std::abs(cosa) < planeIntersectTolerance()) - { - FCL_REAL d = new_s2.signedDistance(T); - distance = std::abs(d) - s1.radius; - if(distance > 0) { - p1 = p2 = Vec3f (0,0,0); - return false; - } - else - { - if (d < 0) normal = new_s2.n; else normal = -new_s2.n; - p1 = p2 = T - dir_z * (s1.halfLength) + - dir_z * (- distance / s1.radius * s1.halfLength) - new_s2.n * d; - return true; - } - } + } + normal.noalias() = pb - os; + FCL_REAL pdist = normal.norm(); + if (outside) { // pb is on the box + dist = pdist - s.radius; + normal /= -pdist; + } else { // pb is inside the box + if (os_in_b_frame(axis) >= 0) + normal = Rb.col(axis); + else + normal = -Rb.col(axis); + dist = -min_d - s.radius; + } + if (!outside || dist <= 0) { + ps = pb; + return true; + } else { + ps = os - s.radius * normal; + return false; + } +} + +inline bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Plane new_s2 = transform(s2, tf2); + + // position orientation of capsule + const Matrix3f& R1 = tf1.getRotation(); + const Vec3f& T1 = tf1.getTranslation(); + + Vec3f dir_z = R1.col(2); + + // ends of capsule inner segment + Vec3f a1 = T1 + dir_z * s1.halfLength; + Vec3f a2 = T1 - dir_z * s1.halfLength; + + FCL_REAL d1 = new_s2.signedDistance(a1); + FCL_REAL d2 = new_s2.signedDistance(a2); + + FCL_REAL abs_d1 = std::abs(d1); + FCL_REAL abs_d2 = std::abs(d2); + + // two end points on different side of the plane + // the contact point is the intersect of axis with the plane + // the normal is the direction to avoid intersection + // the depth is the minimum distance to resolve the collision + if (d1 * d2 < -planeIntersectTolerance()) { + if (abs_d1 < abs_d2) { + distance = -abs_d1 - s1.radius; + p1 = p2 = + a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2)); + if (d1 < 0) + normal = -new_s2.n; else - { - Vec3f C = dir_z * cosa - new_s2.n; - if(std::abs(cosa + 1) < planeIntersectTolerance() || - std::abs(cosa - 1) < planeIntersectTolerance()) - C = Vec3f(0, 0, 0); - else - { - FCL_REAL s = C.norm(); - s = s1.radius / s; - C *= s; - } - - Vec3f c[3]; - c[0] = T + dir_z * (s1.halfLength); - c[1] = T - dir_z * (s1.halfLength) + C; - c[2] = T - dir_z * (s1.halfLength) - C; - - FCL_REAL d[3]; - d[0] = new_s2.signedDistance(c[0]); - d[1] = new_s2.signedDistance(c[1]); - d[2] = new_s2.signedDistance(c[2]); - - if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || - (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) - return false; - else - { - bool positive[3]; - for(std::size_t i = 0; i < 3; ++i) - positive[i] = (d[i] >= 0); - - int n_positive = 0; - FCL_REAL d_positive = 0, d_negative = 0; - for(std::size_t i = 0; i < 3; ++i) - { - if(positive[i]) - { - n_positive++; - if(d_positive <= d[i]) d_positive = d[i]; - } - else - { - if(d_negative <= -d[i]) d_negative = -d[i]; - } - } - - distance = -std::min(d_positive, d_negative); - if (d_positive > d_negative) normal = -new_s2.n; - else normal = new_s2.n; - Vec3f p[2]; - Vec3f q; - - FCL_REAL p_d[2]; - FCL_REAL q_d(0); - - if(n_positive == 2) - { - for(std::size_t i = 0, j = 0; i < 3; ++i) - { - if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } - else { q = c[i]; q_d = d[i]; } - } - - Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); - Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); - p1 = p2 = (t1 + t2) * 0.5; - } - else - { - for(std::size_t i = 0, j = 0; i < 3; ++i) - { - if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } - else { q = c[i]; q_d = d[i]; } - } - - Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); - Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); - p1 = p2 = (t1 + t2) * 0.5; - } - } - return true; - } + normal = new_s2.n; + } else { + distance = -abs_d2 - s1.radius; + p1 = p2 = + a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2)); + if (d2 < 0) + normal = -new_s2.n; + else + normal = new_s2.n; + } + assert(!p1.hasNaN() && !p2.hasNaN()); + return true; + } + + if (abs_d1 > s1.radius && abs_d2 > s1.radius) { + // Here both capsule ends are on the same side of the plane + if (d1 > 0) + normal = new_s2.n; + else + normal = -new_s2.n; + if (abs_d1 < abs_d2) { + distance = abs_d1 - s1.radius; + p1 = a1 - s1.radius * normal; + p2 = p1 - distance * normal; + } else { + distance = abs_d2 - s1.radius; + p1 = a2 - s1.radius * normal; + p2 = p1 - distance * normal; + } + assert(!p1.hasNaN() && !p2.hasNaN()); + return false; + } else { + // Both capsule ends are on the same side of the plane, but one + // is closer than the capsule radius, hence collision + distance = std::min(abs_d1, abs_d2) - s1.radius; + + if (abs_d1 <= s1.radius && abs_d2 <= s1.radius) { + Vec3f c1 = a1 - new_s2.n * d1; + Vec3f c2 = a2 - new_s2.n * d2; + p1 = p2 = (c1 + c2) * 0.5; + } else if (abs_d1 <= s1.radius) { + Vec3f c = a1 - new_s2.n * d1; + p1 = p2 = c; + } else if (abs_d2 <= s1.radius) { + Vec3f c = a2 - new_s2.n * d2; + p1 = p2 = c; + } else { + assert(false); } - inline bool convexPlaneIntersect - (const ConvexBase& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) - { - Plane new_s2 = transform(s2, tf2); + if (d1 < 0) + normal = new_s2.n; + else + normal = -new_s2.n; + assert(!p1.hasNaN() && !p2.hasNaN()); + return true; + } + assert(false); +} + +/// @brief cylinder-plane intersect +/// n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d +/// need one point to be positive and one to be negative +/// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * +/// v2)) ~ 0 (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * +/// v2)) + n * T - d ~ 0 +inline bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Plane new_s2 = transform(s2, tf2); + + const Matrix3f& R = tf1.getRotation(); + const Vec3f& T = tf1.getTranslation(); + + Vec3f dir_z = R.col(2); + FCL_REAL cosa = dir_z.dot(new_s2.n); + + if (std::abs(cosa) < planeIntersectTolerance()) { + FCL_REAL d = new_s2.signedDistance(T); + distance = std::abs(d) - s1.radius; + if (distance > 0) + return false; + else { + if (d < 0) + normal = new_s2.n; + else + normal = -new_s2.n; + p1 = p2 = T - new_s2.n * d; + return true; + } + } else { + Vec3f C = dir_z * cosa - new_s2.n; + if (std::abs(cosa + 1) < planeIntersectTolerance() || + std::abs(cosa - 1) < planeIntersectTolerance()) + C = Vec3f(0, 0, 0); + else { + FCL_REAL s = C.norm(); + s = s1.radius / s; + C *= s; + } - Vec3f v_min, v_max; - FCL_REAL d_min = (std::numeric_limits::max)(), d_max = -(std::numeric_limits::max)(); + Vec3f a1 = T + dir_z * (s1.halfLength); + Vec3f a2 = T - dir_z * (s1.halfLength); - for(unsigned int i = 0; i < s1.num_points; ++i) - { - Vec3f p = tf1.transform(s1.points[i]); + Vec3f c1, c2; + if (cosa > 0) { + c1 = a1 - C; + c2 = a2 + C; + } else { + c1 = a1 + C; + c2 = a2 - C; + } - FCL_REAL d = new_s2.signedDistance(p); + FCL_REAL d1 = new_s2.signedDistance(c1); + FCL_REAL d2 = new_s2.signedDistance(c2); - if(d < d_min) { d_min = d; v_min = p; } - if(d > d_max) { d_max = d; v_max = p; } - } + if (d1 * d2 <= 0) { + FCL_REAL abs_d1 = std::abs(d1); + FCL_REAL abs_d2 = std::abs(d2); - if(d_min * d_max > 0) return false; + if (abs_d1 > abs_d2) { + distance = -abs_d2; + p1 = p2 = c2 - new_s2.n * d2; + if (d2 < 0) + normal = -new_s2.n; + else + normal = new_s2.n; + } else { + distance = -abs_d1; + p1 = p2 = c1 - new_s2.n * d1; + if (d1 < 0) + normal = -new_s2.n; + else + normal = new_s2.n; + } + return true; + } else + return false; + } +} + +inline bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Plane new_s2 = transform(s2, tf2); + + const Matrix3f& R = tf1.getRotation(); + const Vec3f& T = tf1.getTranslation(); + + Vec3f dir_z = R.col(2); + FCL_REAL cosa = dir_z.dot(new_s2.n); + + if (std::abs(cosa) < planeIntersectTolerance()) { + FCL_REAL d = new_s2.signedDistance(T); + distance = std::abs(d) - s1.radius; + if (distance > 0) { + p1 = p2 = Vec3f(0, 0, 0); + return false; + } else { + if (d < 0) + normal = new_s2.n; else - { - if(d_min + d_max > 0) - { - if(penetration_depth) *penetration_depth = -d_min; - if(normal) *normal = -new_s2.n; - if(contact_points) *contact_points = v_min - new_s2.n * d_min; - } - else - { - if(penetration_depth) *penetration_depth = d_max; - if(normal) *normal = new_s2.n; - if(contact_points) *contact_points = v_max - new_s2.n * d_max; - } - return true; - } + normal = -new_s2.n; + p1 = p2 = T - dir_z * (s1.halfLength) + + dir_z * (-distance / s1.radius * s1.halfLength) - new_s2.n * d; + return true; + } + } else { + Vec3f C = dir_z * cosa - new_s2.n; + if (std::abs(cosa + 1) < planeIntersectTolerance() || + std::abs(cosa - 1) < planeIntersectTolerance()) + C = Vec3f(0, 0, 0); + else { + FCL_REAL s = C.norm(); + s = s1.radius / s; + C *= s; } + Vec3f c[3]; + c[0] = T + dir_z * (s1.halfLength); + c[1] = T - dir_z * (s1.halfLength) + C; + c[2] = T - dir_z * (s1.halfLength) - C; + FCL_REAL d[3]; + d[0] = new_s2.signedDistance(c[0]); + d[1] = new_s2.signedDistance(c[1]); + d[2] = new_s2.signedDistance(c[2]); - inline bool planeTriangleIntersect - (const Plane& s1, const Transform3f& tf1, const Vec3f& P1, - const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) - { - Plane new_s1 = transform(s1, tf1); - - Vec3f c[3]; - c[0] = tf2.transform(P1); - c[1] = tf2.transform(P2); - c[2] = tf2.transform(P3); - - FCL_REAL d[3]; - d[0] = new_s1.signedDistance(c[0]); - d[1] = new_s1.signedDistance(c[1]); - d[2] = new_s1.signedDistance(c[2]); - int imin; - if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0) - { - if (d[0] < d[1]) - if (d[0] < d[2]) { - imin = 0; - } - else { // d [2] <= d[0] < d [1] - imin = 2; - } - else { // d[1] <= d[0] - if (d[2] < d[1]) { - imin = 2; - } else { // d[1] <= d[2] - imin = 1; - } - } - distance = d[imin]; - p2 = c[imin]; - p1 = c[imin] - d[imin] * new_s1.n; - return false; - } - if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0) - { - if (d[0] > d[1]) - if (d[0] > d[2]) { - imin = 0; - } - else { // d [2] >= d[0] > d [1] - imin = 2; - } - else { // d[1] >= d[0] - if (d[2] > d[1]) { - imin = 2; - } else { // d[1] >= d[2] - imin = 1; - } - } - distance = -d[imin]; - p2 = c[imin]; - p1 = c[imin] - d[imin] * new_s1.n; - return false; - } + if ((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || + (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) + return false; + else { bool positive[3]; - for(std::size_t i = 0; i < 3; ++i) - positive[i] = (d[i] > 0); + for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] >= 0); int n_positive = 0; FCL_REAL d_positive = 0, d_negative = 0; - for(std::size_t i = 0; i < 3; ++i) - { - if(positive[i]) - { - n_positive++; - if(d_positive <= d[i]) d_positive = d[i]; - } - else - { - if(d_negative <= -d[i]) d_negative = -d[i]; - } + for (std::size_t i = 0; i < 3; ++i) { + if (positive[i]) { + n_positive++; + if (d_positive <= d[i]) d_positive = d[i]; + } else { + if (d_negative <= -d[i]) d_negative = -d[i]; } + } distance = -std::min(d_positive, d_negative); if (d_positive > d_negative) - { - normal = new_s1.n; - } else - { - normal = -new_s1.n; - } + normal = -new_s2.n; + else + normal = new_s2.n; Vec3f p[2]; Vec3f q; FCL_REAL p_d[2]; FCL_REAL q_d(0); - if(n_positive == 2) - { - for(std::size_t i = 0, j = 0; i < 3; ++i) - { - if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } - else { q = c[i]; q_d = d[i]; } - } - - Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); - Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); - p1 = p2 = (t1 + t2) * 0.5; + if (n_positive == 2) { + for (std::size_t i = 0, j = 0; i < 3; ++i) { + if (positive[i]) { + p[j] = c[i]; + p_d[j] = d[i]; + j++; + } else { + q = c[i]; + q_d = d[i]; + } } - else - { - for(std::size_t i = 0, j = 0; i < 3; ++i) - { - if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } - else { q = c[i]; q_d = d[i]; } - } - - Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); - Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); - p1 = p2 = (t1 + t2) * 0.5; + + Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); + Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); + p1 = p2 = (t1 + t2) * 0.5; + } else { + for (std::size_t i = 0, j = 0; i < 3; ++i) { + if (!positive[i]) { + p[j] = c[i]; + p_d[j] = d[i]; + j++; + } else { + q = c[i]; + q_d = d[i]; + } } - return true; - } - inline bool halfspacePlaneIntersect - (const Halfspace& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - Plane& pl, Vec3f& p, Vec3f& d, FCL_REAL& penetration_depth, int& ret) - { - return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, ret); + Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); + Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); + p1 = p2 = (t1 + t2) * 0.5; + } } + return true; + } +} - inline bool planeIntersect - (const Plane& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) - { - Plane new_s1 = transform(s1, tf1); - Plane new_s2 = transform(s2, tf2); +inline bool convexPlaneIntersect(const ConvexBase& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + Vec3f* contact_points, + FCL_REAL* penetration_depth, Vec3f* normal) { + Plane new_s2 = transform(s2, tf2); - FCL_REAL a = (new_s1.n).dot(new_s2.n); - if(a == 1 && new_s1.d != new_s2.d) - return false; - if(a == -1 && new_s1.d != -new_s2.d) - return false; + Vec3f v_min, v_max; + FCL_REAL d_min = (std::numeric_limits::max)(), + d_max = -(std::numeric_limits::max)(); - return true; - } + for (unsigned int i = 0; i < s1.num_points; ++i) { + Vec3f p = tf1.transform(s1.points[i]); - /// See the prototype below - inline FCL_REAL computePenetration - (const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, - const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, - Vec3f& normal) - { - Vec3f u ((P2-P1).cross (P3-P1)); - normal = u.normalized (); - FCL_REAL depth1 ((P1-Q1).dot (normal)); - FCL_REAL depth2 ((P1-Q2).dot (normal)); - FCL_REAL depth3 ((P1-Q3).dot (normal)); - return std::max (depth1, std::max (depth2, depth3)); + FCL_REAL d = new_s2.signedDistance(p); + + if (d < d_min) { + d_min = d; + v_min = p; + } + if (d > d_max) { + d_max = d; + v_max = p; + } + } + + if (d_min * d_max > 0) + return false; + else { + if (d_min + d_max > 0) { + if (penetration_depth) *penetration_depth = -d_min; + if (normal) *normal = -new_s2.n; + if (contact_points) *contact_points = v_min - new_s2.n * d_min; + } else { + if (penetration_depth) *penetration_depth = d_max; + if (normal) *normal = new_s2.n; + if (contact_points) *contact_points = v_max - new_s2.n * d_max; + } + return true; + } +} + +inline bool planeTriangleIntersect(const Plane& s1, const Transform3f& tf1, + const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, + Vec3f& normal) { + Plane new_s1 = transform(s1, tf1); + + Vec3f c[3]; + c[0] = tf2.transform(P1); + c[1] = tf2.transform(P2); + c[2] = tf2.transform(P3); + + FCL_REAL d[3]; + d[0] = new_s1.signedDistance(c[0]); + d[1] = new_s1.signedDistance(c[1]); + d[2] = new_s1.signedDistance(c[2]); + int imin; + if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0) { + if (d[0] < d[1]) + if (d[0] < d[2]) { + imin = 0; + } else { // d [2] <= d[0] < d [1] + imin = 2; + } + else { // d[1] <= d[0] + if (d[2] < d[1]) { + imin = 2; + } else { // d[1] <= d[2] + imin = 1; + } + } + distance = d[imin]; + p2 = c[imin]; + p1 = c[imin] - d[imin] * new_s1.n; + return false; + } + if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0) { + if (d[0] > d[1]) + if (d[0] > d[2]) { + imin = 0; + } else { // d [2] >= d[0] > d [1] + imin = 2; + } + else { // d[1] >= d[0] + if (d[2] > d[1]) { + imin = 2; + } else { // d[1] >= d[2] + imin = 1; + } + } + distance = -d[imin]; + p2 = c[imin]; + p1 = c[imin] - d[imin] * new_s1.n; + return false; + } + bool positive[3]; + for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] > 0); + + int n_positive = 0; + FCL_REAL d_positive = 0, d_negative = 0; + for (std::size_t i = 0; i < 3; ++i) { + if (positive[i]) { + n_positive++; + if (d_positive <= d[i]) d_positive = d[i]; + } else { + if (d_negative <= -d[i]) d_negative = -d[i]; + } + } + + distance = -std::min(d_positive, d_negative); + if (d_positive > d_negative) { + normal = new_s1.n; + } else { + normal = -new_s1.n; + } + Vec3f p[2]; + Vec3f q; + + FCL_REAL p_d[2]; + FCL_REAL q_d(0); + + if (n_positive == 2) { + for (std::size_t i = 0, j = 0; i < 3; ++i) { + if (positive[i]) { + p[j] = c[i]; + p_d[j] = d[i]; + j++; + } else { + q = c[i]; + q_d = d[i]; + } } - // Compute penetration distance and normal of two triangles in collision - // Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the - // minimal distance (Q1, Q2, Q3) should be translated along the normal so - // that the triangles are collision free. - // - // Note that we compute here an upper bound of the penetration distance, - // not the exact value. - inline FCL_REAL computePenetration - (const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, - const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, - const Transform3f& tf1, const Transform3f& tf2, Vec3f& normal) - { - Vec3f globalP1 (tf1.transform (P1)); - Vec3f globalP2 (tf1.transform (P2)); - Vec3f globalP3 (tf1.transform (P3)); - Vec3f globalQ1 (tf2.transform (Q1)); - Vec3f globalQ2 (tf2.transform (Q2)); - Vec3f globalQ3 (tf2.transform (Q3)); - return computePenetration (globalP1, globalP2, globalP3, - globalQ1, globalQ2, globalQ3, normal); + Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); + Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); + p1 = p2 = (t1 + t2) * 0.5; + } else { + for (std::size_t i = 0, j = 0; i < 3; ++i) { + if (!positive[i]) { + p[j] = c[i]; + p_d[j] = d[i]; + j++; + } else { + q = c[i]; + q_d = d[i]; + } } - } // details -} // namespace fcl -} // namespace hpp -#endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H + Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); + Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); + p1 = p2 = (t1 + t2) * 0.5; + } + return true; +} + +inline bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + Plane& pl, Vec3f& p, Vec3f& d, + FCL_REAL& penetration_depth, int& ret) { + return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, + ret); +} + +inline bool planeIntersect(const Plane& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + Vec3f* /*contact_points*/, + FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) { + Plane new_s1 = transform(s1, tf1); + Plane new_s2 = transform(s2, tf2); + + FCL_REAL a = (new_s1.n).dot(new_s2.n); + if (a == 1 && new_s1.d != new_s2.d) return false; + if (a == -1 && new_s1.d != -new_s2.d) return false; + + return true; +} + +/// See the prototype below +inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Vec3f& Q1, + const Vec3f& Q2, const Vec3f& Q3, + Vec3f& normal) { + Vec3f u((P2 - P1).cross(P3 - P1)); + normal = u.normalized(); + FCL_REAL depth1((P1 - Q1).dot(normal)); + FCL_REAL depth2((P1 - Q2).dot(normal)); + FCL_REAL depth3((P1 - Q3).dot(normal)); + return std::max(depth1, std::max(depth2, depth3)); +} + +// Compute penetration distance and normal of two triangles in collision +// Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the +// minimal distance (Q1, Q2, Q3) should be translated along the normal so +// that the triangles are collision free. +// +// Note that we compute here an upper bound of the penetration distance, +// not the exact value. +inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Vec3f& Q1, + const Vec3f& Q2, const Vec3f& Q3, + const Transform3f& tf1, + const Transform3f& tf2, Vec3f& normal) { + Vec3f globalP1(tf1.transform(P1)); + Vec3f globalP2(tf1.transform(P2)); + Vec3f globalP3(tf1.transform(P3)); + Vec3f globalQ1(tf2.transform(Q1)); + Vec3f globalQ2(tf2.transform(Q2)); + Vec3f globalQ3(tf2.transform(Q3)); + return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2, + globalQ3, normal); +} +} // namespace details +} // namespace fcl +} // namespace hpp + +#endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 83b832605..0ee105b6b 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -39,98 +39,86 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace details -{ +namespace details { -struct HPP_FCL_LOCAL shape_traits_base -{ - enum { NeedNormalizedDir = true - }; +struct HPP_FCL_LOCAL shape_traits_base { + enum { NeedNormalizedDir = true }; }; -template struct HPP_FCL_LOCAL shape_traits : shape_traits_base {}; +template +struct HPP_FCL_LOCAL shape_traits : shape_traits_base {}; -template <> struct HPP_FCL_LOCAL shape_traits : shape_traits_base -{ - enum { NeedNormalizedDir = false - }; +template <> +struct HPP_FCL_LOCAL shape_traits : shape_traits_base { + enum { NeedNormalizedDir = false }; }; -template <> struct HPP_FCL_LOCAL shape_traits : shape_traits_base -{ - enum { NeedNormalizedDir = false - }; +template <> +struct HPP_FCL_LOCAL shape_traits : shape_traits_base { + enum { NeedNormalizedDir = false }; }; -template <> struct HPP_FCL_LOCAL shape_traits : shape_traits_base -{ - enum { NeedNormalizedDir = false - }; +template <> +struct HPP_FCL_LOCAL shape_traits : shape_traits_base { + enum { NeedNormalizedDir = false }; }; -template <> struct HPP_FCL_LOCAL shape_traits : shape_traits_base -{ - enum { NeedNormalizedDir = false - }; +template <> +struct HPP_FCL_LOCAL shape_traits : shape_traits_base { + enum { NeedNormalizedDir = false }; }; -template <> struct HPP_FCL_LOCAL shape_traits : shape_traits_base -{ - enum { NeedNormalizedDir = false - }; +template <> +struct HPP_FCL_LOCAL shape_traits : shape_traits_base { + enum { NeedNormalizedDir = false }; }; -template <> struct HPP_FCL_LOCAL shape_traits : shape_traits_base -{ - enum { NeedNormalizedDir = false - }; +template <> +struct HPP_FCL_LOCAL shape_traits : shape_traits_base { + enum { NeedNormalizedDir = false }; }; -template <> struct HPP_FCL_LOCAL shape_traits : shape_traits_base -{ - enum { NeedNormalizedDir = false - }; +template <> +struct HPP_FCL_LOCAL shape_traits : shape_traits_base { + enum { NeedNormalizedDir = false }; }; -void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*) -{ +void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, + Vec3f& support, int&, MinkowskiDiff::ShapeData*) { FCL_REAL dota = dir.dot(triangle->a); FCL_REAL dotb = dir.dot(triangle->b); FCL_REAL dotc = dir.dot(triangle->c); - if(dota > dotb) - { - if(dotc > dota) + if (dota > dotb) { + if (dotc > dota) support = triangle->c; else support = triangle->a; - } - else - { - if(dotc > dotb) + } else { + if (dotc > dotb) support = triangle->c; else support = triangle->b; } } -inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*) -{ +inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, + int&, MinkowskiDiff::ShapeData*) { const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.; - support.noalias() = (dir.array() > 0).select(inflate * box->halfSide, -inflate * box->halfSide); + support.noalias() = + (dir.array() > 0) + .select(inflate * box->halfSide, -inflate * box->halfSide); } -inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, int&, MinkowskiDiff::ShapeData*) -{ +inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, + int&, MinkowskiDiff::ShapeData*) { support.setZero(); } -inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*) -{ +inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, + Vec3f& support, int&, MinkowskiDiff::ShapeData*) { FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; @@ -142,15 +130,17 @@ inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, Vec3f& support = v / d; } -inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*) -{ +inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, + Vec3f& support, int&, MinkowskiDiff::ShapeData*) { support.head<2>().setZero(); - if (dir[2] > 0) support[2] = capsule->halfLength; - else support[2] = - capsule->halfLength; + if (dir[2] > 0) + support[2] = capsule->halfLength; + else + support[2] = -capsule->halfLength; } -void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*) -{ +void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, + MinkowskiDiff::ShapeData*) { // The cone radius is, for -h < z < h, (h - z) * r / (2*h) static const FCL_REAL inflate = 1.00001; FCL_REAL h = cone->halfLength; @@ -161,7 +151,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, M if (dir[2] > 0) support[2] = h; else - support[2] = - inflate * h; + support[2] = -inflate * h; return; } FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1]; @@ -178,7 +168,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, M len = std::sqrt(len); FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h); - if(dir[2] > len * sin_a) + if (dir[2] > len * sin_a) support << 0, 0, h; else { FCL_REAL rad = r / zdist; @@ -187,8 +177,8 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, M } } -void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*) -{ +void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, + int&, MinkowskiDiff::ShapeData*) { // The inflation makes the object look strictly convex to GJK and EPA. This // helps solving particular cases (e.g. a cylinder with itself at the same // position...) @@ -196,31 +186,36 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, FCL_REAL half_h = cylinder->halfLength; FCL_REAL r = cylinder->radius; - if (dir.head<2>() == Eigen::Matrix::Zero()) half_h *= inflate; + if (dir.head<2>() == Eigen::Matrix::Zero()) half_h *= inflate; - if (dir[2] > 0) support[2] = half_h; - else if (dir[2] < 0) support[2] = -half_h; - else { support[2] = 0; r *= inflate; } - if (dir.head<2>() == Eigen::Matrix::Zero()) + if (dir[2] > 0) + support[2] = half_h; + else if (dir[2] < 0) + support[2] = -half_h; + else { + support[2] = 0; + r *= inflate; + } + if (dir.head<2>() == Eigen::Matrix::Zero()) support.head<2>().setZero(); else support.head<2>() = dir.head<2>().normalized() * r; - assert (fabs (support [0] * dir [1] - support [1] * dir [0]) - < sqrt(std::numeric_limits::epsilon())); + assert(fabs(support[0] * dir[1] - support[1] * dir[0]) < + sqrt(std::numeric_limits::epsilon())); } -struct SmallConvex : ShapeBase{}; -struct LargeConvex : ShapeBase{}; +struct SmallConvex : ShapeBase {}; +struct LargeConvex : ShapeBase {}; -void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data) -{ +void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, + Vec3f& support, int& hint, + MinkowskiDiff::ShapeData* data) { assert(data != NULL); const Vec3f* pts = convex->points; const ConvexBase::Neighbors* nn = convex->neighbors; - if (hint < 0 || hint >= (int)convex->num_points) - hint = 0; + if (hint < 0 || hint >= (int)convex->num_points) hint = 0; FCL_REAL maxdot = pts[hint].dot(dir); std::vector& visited = data->visited; visited.assign(convex->num_points, false); @@ -228,8 +223,7 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, Vec3f& suppo // when the first face is orthogonal to dir, all the dot products will be // equal. Yet, the neighbors must be visited. bool found = true, loose_check = true; - while (found) - { + while (found) { const ConvexBase::Neighbors& n = nn[hint]; found = false; for (int in = 0; in < n.count(); ++in) { @@ -254,8 +248,9 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, Vec3f& suppo support = pts[hint]; } -void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData*) -{ +void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, + Vec3f& support, int& hint, + MinkowskiDiff::ShapeData*) { const Vec3f* pts = convex->points; hint = 0; @@ -270,68 +265,72 @@ void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, Vec3f& su support = pts[hint]; } -void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData*) -{ +void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, + int& hint, MinkowskiDiff::ShapeData*) { // TODO add benchmark to set a proper value for switching between linear and // logarithmic. if (convex->num_points > 32) { MinkowskiDiff::ShapeData data; getShapeSupportLog(convex, dir, support, hint, &data); - } - else + } else getShapeSupportLinear(convex, dir, support, hint, NULL); } -inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data) -{ - getShapeSupportLinear(reinterpret_cast(convex), dir, support, hint, data); +inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, + Vec3f& support, int& hint, + MinkowskiDiff::ShapeData* data) { + getShapeSupportLinear(reinterpret_cast(convex), dir, + support, hint, data); } -inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data) -{ - getShapeSupportLog(reinterpret_cast(convex), dir, support, hint, data); +inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, + Vec3f& support, int& hint, + MinkowskiDiff::ShapeData* data) { + getShapeSupportLog(reinterpret_cast(convex), dir, support, + hint, data); } -#define CALL_GET_SHAPE_SUPPORT(ShapeType) \ - getShapeSupport (static_cast(shape), \ - (shape_traits::NeedNormalizedDir && !dirIsNormalized) \ - ? dir.normalized() : dir, \ +#define CALL_GET_SHAPE_SUPPORT(ShapeType) \ + getShapeSupport( \ + static_cast(shape), \ + (shape_traits::NeedNormalizedDir && !dirIsNormalized) \ + ? dir.normalized() \ + : dir, \ support, hint, NULL) -Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint) -{ +Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, + int& hint) { Vec3f support; - switch(shape->getNodeType()) - { - case GEOM_TRIANGLE: - CALL_GET_SHAPE_SUPPORT(TriangleP); - break; - case GEOM_BOX: - CALL_GET_SHAPE_SUPPORT(Box); - break; - case GEOM_SPHERE: - CALL_GET_SHAPE_SUPPORT(Sphere); - break; - case GEOM_ELLIPSOID: - CALL_GET_SHAPE_SUPPORT(Ellipsoid); - break; - case GEOM_CAPSULE: - CALL_GET_SHAPE_SUPPORT(Capsule); - break; - case GEOM_CONE: - CALL_GET_SHAPE_SUPPORT(Cone); - break; - case GEOM_CYLINDER: - CALL_GET_SHAPE_SUPPORT(Cylinder); - break; - case GEOM_CONVEX: - CALL_GET_SHAPE_SUPPORT(ConvexBase); - break; - case GEOM_PLANE: - case GEOM_HALFSPACE: - default: - support.setZero(); - ; // nothing + switch (shape->getNodeType()) { + case GEOM_TRIANGLE: + CALL_GET_SHAPE_SUPPORT(TriangleP); + break; + case GEOM_BOX: + CALL_GET_SHAPE_SUPPORT(Box); + break; + case GEOM_SPHERE: + CALL_GET_SHAPE_SUPPORT(Sphere); + break; + case GEOM_ELLIPSOID: + CALL_GET_SHAPE_SUPPORT(Ellipsoid); + break; + case GEOM_CAPSULE: + CALL_GET_SHAPE_SUPPORT(Capsule); + break; + case GEOM_CONE: + CALL_GET_SHAPE_SUPPORT(Cone); + break; + case GEOM_CYLINDER: + CALL_GET_SHAPE_SUPPORT(Cylinder); + break; + case GEOM_CONVEX: + CALL_GET_SHAPE_SUPPORT(ConvexBase); + break; + case GEOM_PLANE: + case GEOM_HALFSPACE: + default: + support.setZero(); + ; // nothing } return support; @@ -340,176 +339,197 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, #undef CALL_GET_SHAPE_SUPPORT template -void getSupportTpl (const Shape0* s0, const Shape1* s1, - const Matrix3f& oR1, const Vec3f& ot1, - const Vec3f& dir, Vec3f& support0, Vec3f& support1, - support_func_guess_t& hint, MinkowskiDiff::ShapeData data[2]) -{ - getShapeSupport (s0, dir, support0, hint[0], &(data[0])); +void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1, + const Vec3f& ot1, const Vec3f& dir, Vec3f& support0, + Vec3f& support1, support_func_guess_t& hint, + MinkowskiDiff::ShapeData data[2]) { + getShapeSupport(s0, dir, support0, hint[0], &(data[0])); if (TransformIsIdentity) - getShapeSupport (s1, - dir, support1, hint[1], &(data[1])); + getShapeSupport(s1, -dir, support1, hint[1], &(data[1])); else { - getShapeSupport (s1, - oR1.transpose() * dir, support1, hint[1], &(data[1])); + getShapeSupport(s1, -oR1.transpose() * dir, support1, hint[1], &(data[1])); support1 = oR1 * support1 + ot1; } } template -void getSupportFuncTpl (const MinkowskiDiff& md, - const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1, - support_func_guess_t& hint, MinkowskiDiff::ShapeData data[2]) -{ - enum { NeedNormalizedDir = - bool ( (bool)shape_traits::NeedNormalizedDir - || (bool)shape_traits::NeedNormalizedDir) +void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, + bool dirIsNormalized, Vec3f& support0, Vec3f& support1, + support_func_guess_t& hint, + MinkowskiDiff::ShapeData data[2]) { + enum { + NeedNormalizedDir = bool((bool)shape_traits::NeedNormalizedDir || + (bool)shape_traits::NeedNormalizedDir) }; #ifndef NDEBUG // Need normalized direction and direction is normalized - assert(!NeedNormalizedDir || !dirIsNormalized || fabs(dir.squaredNorm() - 1) < 1e-6); + assert(!NeedNormalizedDir || !dirIsNormalized || + fabs(dir.squaredNorm() - 1) < 1e-6); // Need normalized direction but direction is not normalized. - assert(!NeedNormalizedDir || dirIsNormalized || fabs(dir.normalized().squaredNorm() - 1) < 1e-6); + assert(!NeedNormalizedDir || dirIsNormalized || + fabs(dir.normalized().squaredNorm() - 1) < 1e-6); // Don't need normalized direction. Check that dir is not zero. - assert( NeedNormalizedDir || dir.cwiseAbs().maxCoeff() >= 1e-6); + assert(NeedNormalizedDir || dir.cwiseAbs().maxCoeff() >= 1e-6); #endif - getSupportTpl ( - static_cast (md.shapes[0]), - static_cast (md.shapes[1]), - md.oR1, md.ot1, + getSupportTpl( + static_cast(md.shapes[0]), + static_cast(md.shapes[1]), md.oR1, md.ot1, (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir, support0, support1, hint, data); } template -MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity, - Eigen::Array& inflation, int linear_log_convex_threshold) -{ +MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( + const ShapeBase* s1, bool identity, Eigen::Array& inflation, + int linear_log_convex_threshold) { inflation[1] = 0; - switch(s1->getNodeType()) - { - case GEOM_TRIANGLE: - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - case GEOM_BOX: - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - case GEOM_SPHERE: - inflation[1] = static_cast(s1)->radius; - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - case GEOM_ELLIPSOID: - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - case GEOM_CAPSULE: - inflation[1] = static_cast(s1)->radius; - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - case GEOM_CONE: - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - case GEOM_CYLINDER: - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - case GEOM_CONVEX: - if ((int)static_cast(s1)->num_points > - linear_log_convex_threshold) { - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - } else { - if (identity) return getSupportFuncTpl; - else return getSupportFuncTpl; - } - default: - throw std::logic_error ("Unsupported geometric shape"); + switch (s1->getNodeType()) { + case GEOM_TRIANGLE: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_BOX: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_SPHERE: + inflation[1] = static_cast(s1)->radius; + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_ELLIPSOID: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_CAPSULE: + inflation[1] = static_cast(s1)->radius; + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_CONE: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_CYLINDER: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_CONVEX: + if ((int)static_cast(s1)->num_points > + linear_log_convex_threshold) { + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + } else { + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + } + default: + throw std::logic_error("Unsupported geometric shape"); } } -MinkowskiDiff::GetSupportFunction makeGetSupportFunction0 (const ShapeBase* s0, const ShapeBase* s1, bool identity, - Eigen::Array& inflation, int linear_log_convex_threshold) -{ +MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( + const ShapeBase* s0, const ShapeBase* s1, bool identity, + Eigen::Array& inflation, int linear_log_convex_threshold) { inflation[0] = 0; - switch(s0->getNodeType()) - { - case GEOM_TRIANGLE: - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - break; - case GEOM_BOX: - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - break; - case GEOM_SPHERE: - inflation[0] = static_cast(s0)->radius; - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - break; - case GEOM_ELLIPSOID: - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - break; - case GEOM_CAPSULE: - inflation[0] = static_cast(s0)->radius; - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - break; - case GEOM_CONE: - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - break; - case GEOM_CYLINDER: - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - break; - case GEOM_CONVEX: - if ((int)static_cast(s0)->num_points > - linear_log_convex_threshold) - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - else - return makeGetSupportFunction1 (s1, identity, inflation, linear_log_convex_threshold); - break; - default: - throw std::logic_error ("Unsupported geometric shape"); + switch (s0->getNodeType()) { + case GEOM_TRIANGLE: + return makeGetSupportFunction1(s1, identity, inflation, + linear_log_convex_threshold); + break; + case GEOM_BOX: + return makeGetSupportFunction1(s1, identity, inflation, + linear_log_convex_threshold); + break; + case GEOM_SPHERE: + inflation[0] = static_cast(s0)->radius; + return makeGetSupportFunction1(s1, identity, inflation, + linear_log_convex_threshold); + break; + case GEOM_ELLIPSOID: + return makeGetSupportFunction1(s1, identity, inflation, + linear_log_convex_threshold); + break; + case GEOM_CAPSULE: + inflation[0] = static_cast(s0)->radius; + return makeGetSupportFunction1(s1, identity, inflation, + linear_log_convex_threshold); + break; + case GEOM_CONE: + return makeGetSupportFunction1(s1, identity, inflation, + linear_log_convex_threshold); + break; + case GEOM_CYLINDER: + return makeGetSupportFunction1(s1, identity, inflation, + linear_log_convex_threshold); + break; + case GEOM_CONVEX: + if ((int)static_cast(s0)->num_points > + linear_log_convex_threshold) + return makeGetSupportFunction1( + s1, identity, inflation, linear_log_convex_threshold); + else + return makeGetSupportFunction1( + s1, identity, inflation, linear_log_convex_threshold); + break; + default: + throw std::logic_error("Unsupported geometric shape"); } } -void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1, - const Transform3f& tf0, const Transform3f& tf1) -{ +void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, + const Transform3f& tf0, const Transform3f& tf1) { shapes[0] = shape0; shapes[1] = shape1; oR1 = tf0.getRotation().transpose() * tf1.getRotation(); - ot1 = tf0.getRotation().transpose() * (tf1.getTranslation() - tf0.getTranslation()); + ot1 = tf0.getRotation().transpose() * + (tf1.getTranslation() - tf0.getTranslation()); bool identity = (oR1.isIdentity() && ot1.isZero()); - getSupportFunc = makeGetSupportFunction0 (shape0, shape1, identity, inflation, linear_log_convex_threshold); + getSupportFunc = makeGetSupportFunction0(shape0, shape1, identity, inflation, + linear_log_convex_threshold); } -void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1) -{ +void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { shapes[0] = shape0; shapes[1] = shape1; oR1.setIdentity(); ot1.setZero(); - getSupportFunc = makeGetSupportFunction0 (shape0, shape1, true, inflation, linear_log_convex_threshold); + getSupportFunc = makeGetSupportFunction0(shape0, shape1, true, inflation, + linear_log_convex_threshold); } -void GJK::initialize() -{ +void GJK::initialize() { nfree = 0; status = Failed; distance_upper_bound = (std::numeric_limits::max)(); simplex = NULL; } -Vec3f GJK::getGuessFromSimplex() const -{ - return ray; -} +Vec3f GJK::getGuessFromSimplex() const { return ray; } namespace details { -bool getClosestPoints (const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) -{ +bool getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { GJK::SimplexV* const* vs = simplex.vertex; for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) { - assert (vs[i]->w.isApprox (vs[i]->w0 - vs[i]->w1)); + assert(vs[i]->w.isApprox(vs[i]->w0 - vs[i]->w1)); } Project::ProjectResult projection; @@ -518,41 +538,41 @@ bool getClosestPoints (const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) w0 = vs[0]->w0; w1 = vs[0]->w1; return true; - case 2: - { - const Vec3f& a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, - b = vs[1]->w, b0 = vs[1]->w0, b1 = vs[1]->w1; - FCL_REAL la, lb; - Vec3f N (b - a); - la = N.dot(-a); - if (la <= 0) { + case 2: { + const Vec3f &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w, + b0 = vs[1]->w0, b1 = vs[1]->w1; + FCL_REAL la, lb; + Vec3f N(b - a); + la = N.dot(-a); + if (la <= 0) { + assert(false); + w0 = a0; + w1 = a1; + } else { + lb = N.squaredNorm(); + if (la > lb) { assert(false); - w0 = a0; - w1 = a1; + w0 = b0; + w1 = b1; } else { - lb = N.squaredNorm(); - if (la > lb) { - assert(false); - w0 = b0; - w1 = b1; - } else { - lb = la / lb; - la = 1 - lb; - w0 = la * a0 + lb * b0; - w1 = la * a1 + lb * b1; - } + lb = la / lb; + la = 1 - lb; + w0 = la * a0 + lb * b0; + w1 = la * a1 + lb * b1; } } + } return true; case 3: // TODO avoid the reprojection - projection = Project::projectTriangleOrigin (vs[0]->w, vs[1]->w, vs[2]->w); + projection = Project::projectTriangleOrigin(vs[0]->w, vs[1]->w, vs[2]->w); break; - case 4: // We are in collision. - projection = Project::projectTetrahedraOrigin (vs[0]->w, vs[1]->w, vs[2]->w, vs[3]->w); + case 4: // We are in collision. + projection = Project::projectTetrahedraOrigin(vs[0]->w, vs[1]->w, + vs[2]->w, vs[3]->w); break; default: - throw std::logic_error ("The simplex rank must be in [ 1, 4 ]"); + throw std::logic_error("The simplex rank must be in [ 1, 4 ]"); } w0.setZero(); w1.setZero(); @@ -565,17 +585,16 @@ bool getClosestPoints (const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) /// Inflate the points template -void inflate (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) -{ - const Eigen::Array& I (shape.inflation); - Eigen::Array inflate (I > 0); +void inflate(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) { + const Eigen::Array& I(shape.inflation); + Eigen::Array inflate(I > 0); if (!inflate.any()) return; - Vec3f w (w0 - w1); + Vec3f w(w0 - w1); FCL_REAL n2 = w.squaredNorm(); // TODO should be use a threshold (Eigen::NumTraits::epsilon()) ? if (n2 == 0.) { - if (inflate[0]) w0[0] += I[0] * (Separated ? -1 : 1); - if (inflate[1]) w1[0] += I[1] * (Separated ? 1 : -1); + if (inflate[0]) w0[0] += I[0] * (Separated ? -1 : 1); + if (inflate[1]) w1[0] += I[1] * (Separated ? 1 : -1); return; } @@ -589,19 +608,17 @@ void inflate (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) } } -} // namespace details +} // namespace details -bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) -{ +bool GJK::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) { bool res = details::getClosestPoints(*simplex, w0, w1); if (!res) return false; - details::inflate (shape, w0, w1); + details::inflate(shape, w0, w1); return true; } GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, - const support_func_guess_t& supportHint) -{ + const support_func_guess_t& supportHint) { size_t iterations = 0; FCL_REAL alpha = 0; const FCL_REAL inflation = shape_.inflation.sum(); @@ -611,7 +628,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, free_v[1] = &store_v[1]; free_v[2] = &store_v[2]; free_v[3] = &store_v[3]; - + nfree = 4; current = 0; status = Valid; @@ -622,13 +639,12 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, FCL_REAL rl = guess.norm(); if (rl < tolerance) { - ray = Vec3f(-1,0,0); + ray = Vec3f(-1, 0, 0); rl = 1; } else ray = guess; - do - { + do { vertex_id_t next = (vertex_id_t)(1 - current); Simplex& curr_simplex = simplices[current]; Simplex& next_simplex = simplices[next]; @@ -638,204 +654,182 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // - EPA will not run correctly because it starts with a tetrahedron which // does not include the origin. Note that, at this stage, we do not know // whether a tetrahedron including the origin exists. - if(rl < tolerance) // mean origin is near the face of original simplex, return touch + if (rl < tolerance) // mean origin is near the face of original simplex, + // return touch { assert(rl > 0); status = Inside; - distance = - inflation; // should we take rl into account ? + distance = -inflation; // should we take rl into account ? break; } - appendVertex(curr_simplex, -ray, false, support_hint); // see below, ray points away from origin + appendVertex(curr_simplex, -ray, false, + support_hint); // see below, ray points away from origin - // check removed (by ?): when the new support point is close to previous support points, stop (as the new simplex is degenerated) + // check removed (by ?): when the new support point is close to previous + // support points, stop (as the new simplex is degenerated) const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w; // check B: no collision if omega > 0 FCL_REAL omega = ray.dot(w) / rl; - if (omega > upper_bound) - { + if (omega > upper_bound) { distance = omega - inflation; break; } - // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated) + // check C: when the new support point is close to the sub-simplex where the + // ray point lies, stop (as the new simplex again is degenerated) alpha = std::max(alpha, omega); - FCL_REAL diff (rl - alpha); + FCL_REAL diff(rl - alpha); if (iterations == 0) diff = std::abs(diff); // TODO here, we can stop at iteration 0 if this condition is met. // We stopping at iteration 0, the closest point will not be valid. // if(diff - tolerance * rl <= 0) - if(iterations > 0 && diff - tolerance * rl <= 0) - { - if (iterations > 0) - removeVertex(simplices[current]); + if (iterations > 0 && diff - tolerance * rl <= 0) { + if (iterations > 0) removeVertex(simplices[current]); distance = rl - inflation; // TODO When inflation is strictly positive, the distance may be exactly - // zero (so the ray is not zero) and we are not in the case rl < tolerance. - if (distance < tolerance) - status = Inside; + // zero (so the ray is not zero) and we are not in the case rl < + // tolerance. + if (distance < tolerance) status = Inside; break; } // This has been rewritten thanks to the excellent video: // https://youtu.be/Qupqu1xe7Io bool inside; - switch(curr_simplex.rank) - { - case 1: // Only at the first iteration - assert(iterations == 0); - ray = w; - inside = false; - next_simplex.rank = 1; - next_simplex.vertex[0] = curr_simplex.vertex[0]; - break; - case 2: - inside = projectLineOrigin (curr_simplex, next_simplex); - break; - case 3: - inside = projectTriangleOrigin (curr_simplex, next_simplex); - break; - case 4: - inside = projectTetrahedraOrigin (curr_simplex, next_simplex); - break; - default: - throw std::logic_error("Invalid simplex rank"); + switch (curr_simplex.rank) { + case 1: // Only at the first iteration + assert(iterations == 0); + ray = w; + inside = false; + next_simplex.rank = 1; + next_simplex.vertex[0] = curr_simplex.vertex[0]; + break; + case 2: + inside = projectLineOrigin(curr_simplex, next_simplex); + break; + case 3: + inside = projectTriangleOrigin(curr_simplex, next_simplex); + break; + case 4: + inside = projectTetrahedraOrigin(curr_simplex, next_simplex); + break; + default: + throw std::logic_error("Invalid simplex rank"); } - assert (nfree+next_simplex.rank == 4); + assert(nfree + next_simplex.rank == 4); current = next; - if (!inside) - rl = ray.norm(); - if(inside || rl == 0) { + if (!inside) rl = ray.norm(); + if (inside || rl == 0) { status = Inside; - distance = - inflation - 1.; + distance = -inflation - 1.; break; } status = ((++iterations) < max_iterations) ? status : Failed; - - } while(status == Valid); + + } while (status == Valid); simplex = &simplices[current]; assert(simplex->rank > 0 && simplex->rank < 5); return status; } -inline void GJK::removeVertex(Simplex& simplex) -{ +inline void GJK::removeVertex(Simplex& simplex) { free_v[nfree++] = simplex.vertex[--simplex.rank]; } -inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, support_func_guess_t& hint) -{ - simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory - getSupport (v, isNormalized, *simplex.vertex[simplex.rank++], hint); +inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, + bool isNormalized, support_func_guess_t& hint) { + simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory + getSupport(v, isNormalized, *simplex.vertex[simplex.rank++], hint); } -bool GJK::encloseOrigin() -{ +bool GJK::encloseOrigin() { Vec3f axis(Vec3f::Zero()); support_func_guess_t hint = support_func_guess_t::Zero(); - switch(simplex->rank) - { - case 1: - for(int i = 0; i < 3; ++i) - { - axis[i] = 1; - appendVertex(*simplex, axis, true, hint); - if(encloseOrigin()) return true; - removeVertex(*simplex); - axis[i] = -1; - appendVertex(*simplex, -axis, true, hint); - if(encloseOrigin()) return true; - removeVertex(*simplex); - axis[i] = 0; - } - break; - case 2: - { + switch (simplex->rank) { + case 1: + for (int i = 0; i < 3; ++i) { + axis[i] = 1; + appendVertex(*simplex, axis, true, hint); + if (encloseOrigin()) return true; + removeVertex(*simplex); + axis[i] = -1; + appendVertex(*simplex, -axis, true, hint); + if (encloseOrigin()) return true; + removeVertex(*simplex); + axis[i] = 0; + } + break; + case 2: { Vec3f d = simplex->vertex[1]->w - simplex->vertex[0]->w; - for(int i = 0; i < 3; ++i) - { + for (int i = 0; i < 3; ++i) { axis[i] = 1; Vec3f p = d.cross(axis); - if(!p.isZero()) - { + if (!p.isZero()) { appendVertex(*simplex, p, false, hint); - if(encloseOrigin()) return true; + if (encloseOrigin()) return true; removeVertex(*simplex); appendVertex(*simplex, -p, false, hint); - if(encloseOrigin()) return true; + if (encloseOrigin()) return true; removeVertex(*simplex); } axis[i] = 0; } - } - break; - case 3: - axis.noalias() = - (simplex->vertex[1]->w - simplex->vertex[0]->w).cross - (simplex->vertex[2]->w - simplex->vertex[0]->w); - if(!axis.isZero()) - { - appendVertex(*simplex, axis, false, hint); - if(encloseOrigin()) return true; - removeVertex(*simplex); - appendVertex(*simplex, -axis, false, hint); - if(encloseOrigin()) return true; - removeVertex(*simplex); - } - break; - case 4: - if(std::abs(triple(simplex->vertex[0]->w - simplex->vertex[3]->w, - simplex->vertex[1]->w - simplex->vertex[3]->w, - simplex->vertex[2]->w - simplex->vertex[3]->w)) > 0) - return true; - break; + } break; + case 3: + axis.noalias() = + (simplex->vertex[1]->w - simplex->vertex[0]->w) + .cross(simplex->vertex[2]->w - simplex->vertex[0]->w); + if (!axis.isZero()) { + appendVertex(*simplex, axis, false, hint); + if (encloseOrigin()) return true; + removeVertex(*simplex); + appendVertex(*simplex, -axis, false, hint); + if (encloseOrigin()) return true; + removeVertex(*simplex); + } + break; + case 4: + if (std::abs(triple(simplex->vertex[0]->w - simplex->vertex[3]->w, + simplex->vertex[1]->w - simplex->vertex[3]->w, + simplex->vertex[2]->w - simplex->vertex[3]->w)) > 0) + return true; + break; } return false; } -inline void originToPoint ( - const GJK::Simplex& current, GJK::vertex_id_t a, - const Vec3f& A, - GJK::Simplex& next, - Vec3f& ray) -{ - // A is the closest to the origin - ray = A; - next.vertex[0] = current.vertex[a]; - next.rank = 1; +inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a, + const Vec3f& A, GJK::Simplex& next, Vec3f& ray) { + // A is the closest to the origin + ray = A; + next.vertex[0] = current.vertex[a]; + next.rank = 1; } -inline void originToSegment ( - const GJK::Simplex& current, GJK::vertex_id_t a, GJK::vertex_id_t b, - const Vec3f& A, const Vec3f& B, - const Vec3f& AB, - const FCL_REAL& ABdotAO, - GJK::Simplex& next, - Vec3f& ray) -{ - // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B - ray = AB.dot(B) * A + ABdotAO * B; +inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a, + GJK::vertex_id_t b, const Vec3f& A, const Vec3f& B, + const Vec3f& AB, const FCL_REAL& ABdotAO, + GJK::Simplex& next, Vec3f& ray) { + // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B + ray = AB.dot(B) * A + ABdotAO * B; - next.vertex[0] = current.vertex[b]; - next.vertex[1] = current.vertex[a]; - next.rank = 2; + next.vertex[0] = current.vertex[b]; + next.vertex[1] = current.vertex[a]; + next.rank = 2; - // To ensure backward compatibility - ray /= AB.squaredNorm(); + // To ensure backward compatibility + ray /= AB.squaredNorm(); } -inline bool originToTriangle ( - const GJK::Simplex& current, - GJK::vertex_id_t a, GJK::vertex_id_t b, GJK::vertex_id_t c, - const Vec3f& ABC, - const FCL_REAL& ABCdotAO, - GJK::Simplex& next, - Vec3f& ray) -{ +inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a, + GJK::vertex_id_t b, GJK::vertex_id_t c, + const Vec3f& ABC, const FCL_REAL& ABCdotAO, + GJK::Simplex& next, Vec3f& ray) { next.rank = 3; next.vertex[2] = current.vertex[a]; @@ -845,7 +839,7 @@ inline bool originToTriangle ( ray.setZero(); return true; } - if (ABCdotAO > 0) { // Above triangle + if (ABCdotAO > 0) { // Above triangle next.vertex[0] = current.vertex[c]; next.vertex[1] = current.vertex[b]; } else { @@ -854,12 +848,11 @@ inline bool originToTriangle ( } // To ensure backward compatibility - ray = - ABCdotAO / ABC.squaredNorm() * ABC; + ray = -ABCdotAO / ABC.squaredNorm() * ABC; return false; } -bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) -{ +bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) { const vertex_id_t a = 1, b = 0; // A is the last point we added. const Vec3f& A = current.vertex[a]->w; @@ -867,7 +860,7 @@ bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) const Vec3f AB = B - A; const FCL_REAL d = AB.dot(-A); - assert (d <= AB.squaredNorm()); + assert(d <= AB.squaredNorm()); if (d == 0) { // Two extremely unlikely cases: @@ -875,87 +868,82 @@ bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) // function did not do any progress and GJK should have stopped. // - A == origin // In any case, A is the closest to the origin - originToPoint (current, a, A, next, ray); + originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; return A.isZero(); } else if (d < 0) { // A is the closest to the origin - originToPoint (current, a, A, next, ray); + originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; } else - originToSegment (current, a, b, A, B, AB, d, next, ray); + originToSegment(current, a, b, A, B, AB, d, next, ray); return false; } -bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) -{ +bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { const vertex_id_t a = 2, b = 1, c = 0; // A is the last point we added. - const Vec3f& A = current.vertex[a]->w, - B = current.vertex[b]->w, - C = current.vertex[c]->w; + const Vec3f &A = current.vertex[a]->w, B = current.vertex[b]->w, + C = current.vertex[c]->w; - const Vec3f AB = B - A, - AC = C - A, - ABC = AB.cross(AC); + const Vec3f AB = B - A, AC = C - A, ABC = AB.cross(AC); - FCL_REAL edgeAC2o = ABC.cross(AC).dot (-A); + FCL_REAL edgeAC2o = ABC.cross(AC).dot(-A); if (edgeAC2o >= 0) { - FCL_REAL towardsC = AC.dot (-A); - if (towardsC >= 0) { // Region 1 - originToSegment (current, a, c, A, C, AC, towardsC, next, ray); + FCL_REAL towardsC = AC.dot(-A); + if (towardsC >= 0) { // Region 1 + originToSegment(current, a, c, A, C, AC, towardsC, next, ray); free_v[nfree++] = current.vertex[b]; - } else { // Region 4 or 5 + } else { // Region 4 or 5 FCL_REAL towardsB = AB.dot(-A); - if (towardsB < 0) { // Region 5 + if (towardsB < 0) { // Region 5 // A is the closest to the origin - originToPoint (current, a, A, next, ray); + originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; - } else // Region 4 - originToSegment (current, a, b, A, B, AB, towardsB, next, ray); + } else // Region 4 + originToSegment(current, a, b, A, B, AB, towardsB, next, ray); free_v[nfree++] = current.vertex[c]; } } else { - FCL_REAL edgeAB2o = AB.cross(ABC).dot (-A); - if (edgeAB2o >= 0) { // Region 4 or 5 + FCL_REAL edgeAB2o = AB.cross(ABC).dot(-A); + if (edgeAB2o >= 0) { // Region 4 or 5 FCL_REAL towardsB = AB.dot(-A); - if (towardsB < 0) { // Region 5 + if (towardsB < 0) { // Region 5 // A is the closest to the origin - originToPoint (current, a, A, next, ray); + originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; - } else // Region 4 - originToSegment (current, a, b, A, B, AB, towardsB, next, ray); + } else // Region 4 + originToSegment(current, a, b, A, B, AB, towardsB, next, ray); free_v[nfree++] = current.vertex[c]; } else { - return originToTriangle (current, a, b, c, ABC, ABC.dot(-A), next, ray); + return originToTriangle(current, a, b, c, ABC, ABC.dot(-A), next, ray); } } return false; } -bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) -{ +bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { // The code of this function was generated using doc/gjk.py const vertex_id_t a = 3, b = 2, c = 1, d = 0; - const Vec3f& A (current.vertex[a]->w); - const Vec3f& B (current.vertex[b]->w); - const Vec3f& C (current.vertex[c]->w); - const Vec3f& D (current.vertex[d]->w); + const Vec3f& A(current.vertex[a]->w); + const Vec3f& B(current.vertex[b]->w); + const Vec3f& C(current.vertex[c]->w); + const Vec3f& D(current.vertex[d]->w); const FCL_REAL aa = A.squaredNorm(); - const FCL_REAL da = D.dot(A); - const FCL_REAL db = D.dot(B); - const FCL_REAL dc = D.dot(C); - const FCL_REAL dd = D.dot(D); + const FCL_REAL da = D.dot(A); + const FCL_REAL db = D.dot(B); + const FCL_REAL dc = D.dot(C); + const FCL_REAL dd = D.dot(D); const FCL_REAL da_aa = da - aa; - const FCL_REAL ca = C.dot(A); - const FCL_REAL cb = C.dot(B); - const FCL_REAL cc = C.dot(C); - const FCL_REAL& cd = dc; + const FCL_REAL ca = C.dot(A); + const FCL_REAL cb = C.dot(B); + const FCL_REAL cc = C.dot(C); + const FCL_REAL& cd = dc; const FCL_REAL ca_aa = ca - aa; - const FCL_REAL ba = B.dot(A); - const FCL_REAL bb = B.dot(B); - const FCL_REAL& bc = cb; - const FCL_REAL& bd = db; + const FCL_REAL ba = B.dot(A); + const FCL_REAL bb = B.dot(B); + const FCL_REAL& bc = cb; + const FCL_REAL& bd = db; const FCL_REAL ba_aa = ba - aa; const FCL_REAL ba_ca = ba - ca; const FCL_REAL ca_da = ca - da; @@ -963,336 +951,397 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) const Vec3f a_cross_b = A.cross(B); const Vec3f a_cross_c = A.cross(C); -#define REGION_INSIDE() \ - ray.setZero(); \ - next.vertex[0] = current.vertex[d]; \ - next.vertex[1] = current.vertex[c]; \ - next.vertex[2] = current.vertex[b]; \ - next.vertex[3] = current.vertex[a]; \ - next.rank=4; \ - return true; +#define REGION_INSIDE() \ + ray.setZero(); \ + next.vertex[0] = current.vertex[d]; \ + next.vertex[1] = current.vertex[c]; \ + next.vertex[2] = current.vertex[b]; \ + next.vertex[3] = current.vertex[a]; \ + next.rank = 4; \ + return true; - if (ba_aa <= 0) { // if AB.AO >= 0 / a10 - if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / a10.a3 - if (ba * da_ba + bd * ba_aa - bb * da_aa <= 0) { // if (ADB ^ AB).AO >= 0 / a10.a3.a9 - if (da_aa <= 0) { // if AD.AO >= 0 / a10.a3.a9.a12 - assert(da * da_ba + dd * ba_aa - db * da_aa <= 0); // (ADB ^ AD).AO >= 0 / a10.a3.a9.a12.a8 - if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.a4 + if (ba_aa <= 0) { // if AB.AO >= 0 / a10 + if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / a10.a3 + if (ba * da_ba + bd * ba_aa - bb * da_aa <= + 0) { // if (ADB ^ AB).AO >= 0 / a10.a3.a9 + if (da_aa <= 0) { // if AD.AO >= 0 / a10.a3.a9.a12 + assert(da * da_ba + dd * ba_aa - db * da_aa <= + 0); // (ADB ^ AD).AO >= 0 / a10.a3.a9.a12.a8 + if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= + 0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.a4 // Region ABC - originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray); + originToTriangle(current, a, b, c, (B - A).cross(C - A), + -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; - } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.!a4 + } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.!a4 // Region AB - originToSegment (current, a, b, A, B, B-A, -ba_aa, next, ray); + originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray); free_v[nfree++] = current.vertex[c]; free_v[nfree++] = current.vertex[d]; - } // end of (ABC ^ AB).AO >= 0 - } else { // not AD.AO >= 0 / a10.a3.a9.!a12 - if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.a4 - if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5 - if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.a6 + } // end of (ABC ^ AB).AO >= 0 + } else { // not AD.AO >= 0 / a10.a3.a9.!a12 + if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= + 0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.a4 + if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= + 0) { // if (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5 + if (ca * ca_da + cc * da_aa - cd * ca_aa <= + 0) { // if (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.a6 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } else { // not (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.!a6 + } else { // not (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.!a6 // Region AC - originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray); + originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; - } // end of (ACD ^ AC).AO >= 0 - } else { // not (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.!a5 + } // end of (ACD ^ AC).AO >= 0 + } else { // not (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.!a5 // Region ABC - originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray); + originToTriangle(current, a, b, c, (B - A).cross(C - A), + -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; - } // end of (ABC ^ AC).AO >= 0 - } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.!a4 + } // end of (ABC ^ AC).AO >= 0 + } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.!a4 // Region AB - originToSegment (current, a, b, A, B, B-A, -ba_aa, next, ray); + originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray); free_v[nfree++] = current.vertex[c]; free_v[nfree++] = current.vertex[d]; - } // end of (ABC ^ AB).AO >= 0 - } // end of AD.AO >= 0 - } else { // not (ADB ^ AB).AO >= 0 / a10.a3.!a9 - if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0 / a10.a3.!a9.a8 + } // end of (ABC ^ AB).AO >= 0 + } // end of AD.AO >= 0 + } else { // not (ADB ^ AB).AO >= 0 / a10.a3.!a9 + if (da * da_ba + dd * ba_aa - db * da_aa <= + 0) { // if (ADB ^ AD).AO >= 0 / a10.a3.!a9.a8 // Region ADB - originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray); + originToTriangle(current, a, d, b, (D - A).cross(B - A), + D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; - } else { // not (ADB ^ AD).AO >= 0 / a10.a3.!a9.!a8 - if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.a6 - if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.a7 + } else { // not (ADB ^ AD).AO >= 0 / a10.a3.!a9.!a8 + if (ca * ca_da + cc * da_aa - cd * ca_aa <= + 0) { // if (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.a6 + if (da * ca_da + dc * da_aa - dd * ca_aa <= + 0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.a7 // Region AD - originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray); + originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; - } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.!a7 + } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.!a7 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } // end of (ACD ^ AD).AO >= 0 - } else { // not (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.!a6 - if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.a7 + } // end of (ACD ^ AD).AO >= 0 + } else { // not (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.!a6 + if (da * ca_da + dc * da_aa - dd * ca_aa <= + 0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.a7 // Region AD - originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray); + originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; - } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.!a7 + } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.!a7 // Region AC - originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray); + originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; - } // end of (ACD ^ AD).AO >= 0 - } // end of (ACD ^ AC).AO >= 0 - } // end of (ADB ^ AD).AO >= 0 - } // end of (ADB ^ AB).AO >= 0 - } else { // not ADB.AO >= 0 / a10.!a3 - if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0 / a10.!a3.a1 - if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0 / a10.!a3.a1.a4 - if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.a5 - if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.a6 + } // end of (ACD ^ AD).AO >= 0 + } // end of (ACD ^ AC).AO >= 0 + } // end of (ADB ^ AD).AO >= 0 + } // end of (ADB ^ AB).AO >= 0 + } else { // not ADB.AO >= 0 / a10.!a3 + if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / a10.!a3.a1 + if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= + 0) { // if (ABC ^ AB).AO >= 0 / a10.!a3.a1.a4 + if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= + 0) { // if (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.a5 + if (ca * ca_da + cc * da_aa - cd * ca_aa <= + 0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.a6 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.!a6 + } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.!a6 // Region AC - originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray); + originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; - } // end of (ACD ^ AC).AO >= 0 - } else { // not (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.!a5 + } // end of (ACD ^ AC).AO >= 0 + } else { // not (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.!a5 // Region ABC - originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray); + originToTriangle(current, a, b, c, (B - A).cross(C - A), + -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; - } // end of (ABC ^ AC).AO >= 0 - } else { // not (ABC ^ AB).AO >= 0 / a10.!a3.a1.!a4 + } // end of (ABC ^ AC).AO >= 0 + } else { // not (ABC ^ AB).AO >= 0 / a10.!a3.a1.!a4 // Region AB - originToSegment (current, a, b, A, B, B-A, -ba_aa, next, ray); + originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray); free_v[nfree++] = current.vertex[c]; free_v[nfree++] = current.vertex[d]; - } // end of (ABC ^ AB).AO >= 0 - } else { // not ABC.AO >= 0 / a10.!a3.!a1 - if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / a10.!a3.!a1.a2 - if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.a6 - if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.a7 + } // end of (ABC ^ AB).AO >= 0 + } else { // not ABC.AO >= 0 / a10.!a3.!a1 + if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / a10.!a3.!a1.a2 + if (ca * ca_da + cc * da_aa - cd * ca_aa <= + 0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.a6 + if (da * ca_da + dc * da_aa - dd * ca_aa <= + 0) { // if (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.a7 // Region AD - originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray); + originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; - } else { // not (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.!a7 + } else { // not (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.!a7 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } // end of (ACD ^ AD).AO >= 0 - } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.!a6 - if (ca_aa <= 0) { // if AC.AO >= 0 / a10.!a3.!a1.a2.!a6.a11 + } // end of (ACD ^ AD).AO >= 0 + } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.!a6 + if (ca_aa <= 0) { // if AC.AO >= 0 / a10.!a3.!a1.a2.!a6.a11 // Region AC - originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray); + originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; - } else { // not AC.AO >= 0 / a10.!a3.!a1.a2.!a6.!a11 + } else { // not AC.AO >= 0 / a10.!a3.!a1.a2.!a6.!a11 // Region AD - originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray); + originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; - } // end of AC.AO >= 0 - } // end of (ACD ^ AC).AO >= 0 - } else { // not ACD.AO >= 0 / a10.!a3.!a1.!a2 + } // end of AC.AO >= 0 + } // end of (ACD ^ AC).AO >= 0 + } else { // not ACD.AO >= 0 / a10.!a3.!a1.!a2 // Region Inside REGION_INSIDE() - } // end of ACD.AO >= 0 - } // end of ABC.AO >= 0 - } // end of ADB.AO >= 0 - } else { // not AB.AO >= 0 / !a10 - if (ca_aa <= 0) { // if AC.AO >= 0 / !a10.a11 - if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.a11.a2 - if (da_aa <= 0) { // if AD.AO >= 0 / !a10.a11.a2.a12 - if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.a6 - if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7 - if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.a8 + } // end of ACD.AO >= 0 + } // end of ABC.AO >= 0 + } // end of ADB.AO >= 0 + } else { // not AB.AO >= 0 / !a10 + if (ca_aa <= 0) { // if AC.AO >= 0 / !a10.a11 + if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.a11.a2 + if (da_aa <= 0) { // if AD.AO >= 0 / !a10.a11.a2.a12 + if (ca * ca_da + cc * da_aa - cd * ca_aa <= + 0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.a6 + if (da * ca_da + dc * da_aa - dd * ca_aa <= + 0) { // if (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7 + if (da * da_ba + dd * ba_aa - db * da_aa <= + 0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.a8 // Region ADB - originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray); + originToTriangle(current, a, d, b, (D - A).cross(B - A), + D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; - } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.!a8 + } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.!a8 // Region AD - originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray); + originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; - } // end of (ADB ^ AD).AO >= 0 - } else { // not (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.!a7 + } // end of (ADB ^ AD).AO >= 0 + } else { // not (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.!a7 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } // end of (ACD ^ AD).AO >= 0 - } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6 - assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= 0)); // Not (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.!a6.!a7 - if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.a5 + } // end of (ACD ^ AD).AO >= 0 + } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6 + assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= + 0)); // Not (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.!a6.!a7 + if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= + 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.a5 // Region AC - originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray); + originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; - } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.!a5 + } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.!a5 // Region ABC - originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray); + originToTriangle(current, a, b, c, (B - A).cross(C - A), + -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; - } // end of (ABC ^ AC).AO >= 0 - } // end of (ACD ^ AC).AO >= 0 - } else { // not AD.AO >= 0 / !a10.a11.a2.!a12 - if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5 - if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.a6 - assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= 0)); // Not (ACD ^ AD).AO >= 0 / !a10.a11.a2.!a12.a5.a6.!a7 + } // end of (ABC ^ AC).AO >= 0 + } // end of (ACD ^ AC).AO >= 0 + } else { // not AD.AO >= 0 / !a10.a11.a2.!a12 + if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= + 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5 + if (ca * ca_da + cc * da_aa - cd * ca_aa <= + 0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.a6 + assert( + !(da * ca_da + dc * da_aa - dd * ca_aa <= + 0)); // Not (ACD ^ AD).AO >= 0 / !a10.a11.a2.!a12.a5.a6.!a7 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.!a6 + } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.!a6 // Region AC - originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray); + originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; - } // end of (ACD ^ AC).AO >= 0 - } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.!a5 - if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.a1 - assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0); // (ABC ^ AB).AO >= 0 / !a10.a11.a2.!a12.!a5.a1.a4 + } // end of (ACD ^ AC).AO >= 0 + } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.!a5 + if (C.dot(a_cross_b) <= + 0) { // if ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.a1 + assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <= + 0); // (ABC ^ AB).AO >= 0 / !a10.a11.a2.!a12.!a5.a1.a4 // Region ABC - originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray); + originToTriangle(current, a, b, c, (B - A).cross(C - A), + -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; - } else { // not ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.!a1 - assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= 0)); // Not (ACD ^ AD).AO >= 0 / !a10.a11.a2.!a12.!a5.!a1.!a7 + } else { // not ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.!a1 + assert(!( + da * ca_da + dc * da_aa - dd * ca_aa <= + 0)); // Not (ACD ^ AD).AO >= 0 / !a10.a11.a2.!a12.!a5.!a1.!a7 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } // end of ABC.AO >= 0 - } // end of (ABC ^ AC).AO >= 0 - } // end of AD.AO >= 0 - } else { // not ACD.AO >= 0 / !a10.a11.!a2 - if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.a11.!a2.a1 - if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.a5 + } // end of ABC.AO >= 0 + } // end of (ABC ^ AC).AO >= 0 + } // end of AD.AO >= 0 + } else { // not ACD.AO >= 0 / !a10.a11.!a2 + if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.a11.!a2.a1 + if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= + 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.a5 // Region AC - originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray); + originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[d]; - } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.!a5 - assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0); // (ABC ^ AB).AO >= 0 / !a10.a11.!a2.a1.!a5.a4 + } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.!a5 + assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <= + 0); // (ABC ^ AB).AO >= 0 / !a10.a11.!a2.a1.!a5.a4 // Region ABC - originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray); + originToTriangle(current, a, b, c, (B - A).cross(C - A), + -C.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[d]; - } // end of (ABC ^ AC).AO >= 0 - } else { // not ABC.AO >= 0 / !a10.a11.!a2.!a1 - if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.a11.!a2.!a1.a3 - if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.a8 + } // end of (ABC ^ AC).AO >= 0 + } else { // not ABC.AO >= 0 / !a10.a11.!a2.!a1 + if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.a11.!a2.!a1.a3 + if (da * da_ba + dd * ba_aa - db * da_aa <= + 0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.a8 // Region ADB - originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray); + originToTriangle(current, a, d, b, (D - A).cross(B - A), + D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; - } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.!a8 + } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.!a8 // Region AD - originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray); + originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; - } // end of (ADB ^ AD).AO >= 0 - } else { // not ADB.AO >= 0 / !a10.a11.!a2.!a1.!a3 + } // end of (ADB ^ AD).AO >= 0 + } else { // not ADB.AO >= 0 / !a10.a11.!a2.!a1.!a3 // Region Inside REGION_INSIDE() - } // end of ADB.AO >= 0 - } // end of ABC.AO >= 0 - } // end of ACD.AO >= 0 - } else { // not AC.AO >= 0 / !a10.!a11 - if (da_aa <= 0) { // if AD.AO >= 0 / !a10.!a11.a12 - if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.!a11.a12.a3 - if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7 - if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.a8 - assert(!(ba * da_ba + bd * ba_aa - bb * da_aa <= 0)); // Not (ADB ^ AB).AO >= 0 / !a10.!a11.a12.a3.a7.a8.!a9 + } // end of ADB.AO >= 0 + } // end of ABC.AO >= 0 + } // end of ACD.AO >= 0 + } else { // not AC.AO >= 0 / !a10.!a11 + if (da_aa <= 0) { // if AD.AO >= 0 / !a10.!a11.a12 + if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.!a11.a12.a3 + if (da * ca_da + dc * da_aa - dd * ca_aa <= + 0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7 + if (da * da_ba + dd * ba_aa - db * da_aa <= + 0) { // if (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.a8 + assert( + !(ba * da_ba + bd * ba_aa - bb * da_aa <= + 0)); // Not (ADB ^ AB).AO >= 0 / !a10.!a11.a12.a3.a7.a8.!a9 // Region ADB - originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray); + originToTriangle(current, a, d, b, (D - A).cross(B - A), + D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; - } else { // not (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.!a8 + } else { // not (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.!a8 // Region AD - originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray); + originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; - } // end of (ADB ^ AD).AO >= 0 - } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.!a7 - if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.a2 - assert(ca * ca_da + cc * da_aa - cd * ca_aa <= 0); // (ACD ^ AC).AO >= 0 / !a10.!a11.a12.a3.!a7.a2.a6 + } // end of (ADB ^ AD).AO >= 0 + } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.!a7 + if (D.dot(a_cross_c) <= + 0) { // if ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.a2 + assert(ca * ca_da + cc * da_aa - cd * ca_aa <= + 0); // (ACD ^ AC).AO >= 0 / !a10.!a11.a12.a3.!a7.a2.a6 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } else { // not ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2 - if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.a1 - assert(!(ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0)); // Not (ABC ^ AB).AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.a1.!a4 + } else { // not ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2 + if (C.dot(a_cross_b) <= + 0) { // if ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.a1 + assert(!(ba * ba_ca + bb * ca_aa - bc * ba_aa <= + 0)); // Not (ABC ^ AB).AO >= 0 / + // !a10.!a11.a12.a3.!a7.!a2.a1.!a4 // Region ADB - originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray); + originToTriangle(current, a, d, b, (D - A).cross(B - A), + D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; - } else { // not ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.!a1 + } else { // not ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.!a1 // Region ADB - originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray); + originToTriangle(current, a, d, b, (D - A).cross(B - A), + D.dot(a_cross_b), next, ray); free_v[nfree++] = current.vertex[c]; - } // end of ABC.AO >= 0 - } // end of ACD.AO >= 0 - } // end of (ACD ^ AD).AO >= 0 - } else { // not ADB.AO >= 0 / !a10.!a11.a12.!a3 - if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.!a11.a12.!a3.a2 - if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.a7 + } // end of ABC.AO >= 0 + } // end of ACD.AO >= 0 + } // end of (ACD ^ AD).AO >= 0 + } else { // not ADB.AO >= 0 / !a10.!a11.a12.!a3 + if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.!a11.a12.!a3.a2 + if (da * ca_da + dc * da_aa - dd * ca_aa <= + 0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.a7 // Region AD - originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray); + originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; - } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.!a7 - assert(ca * ca_da + cc * da_aa - cd * ca_aa <= 0); // (ACD ^ AC).AO >= 0 / !a10.!a11.a12.!a3.a2.!a7.a6 + } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.!a7 + assert(ca * ca_da + cc * da_aa - cd * ca_aa <= + 0); // (ACD ^ AC).AO >= 0 / !a10.!a11.a12.!a3.a2.!a7.a6 // Region ACD - originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray); + originToTriangle(current, a, c, d, (C - A).cross(D - A), + -D.dot(a_cross_c), next, ray); free_v[nfree++] = current.vertex[b]; - } // end of (ACD ^ AD).AO >= 0 - } else { // not ACD.AO >= 0 / !a10.!a11.a12.!a3.!a2 + } // end of (ACD ^ AD).AO >= 0 + } else { // not ACD.AO >= 0 / !a10.!a11.a12.!a3.!a2 // Region Inside REGION_INSIDE() - } // end of ACD.AO >= 0 - } // end of ADB.AO >= 0 - } else { // not AD.AO >= 0 / !a10.!a11.!a12 + } // end of ACD.AO >= 0 + } // end of ADB.AO >= 0 + } else { // not AD.AO >= 0 / !a10.!a11.!a12 // Region A - originToPoint (current, a, A, next, ray); + originToPoint(current, a, A, next, ray); free_v[nfree++] = current.vertex[b]; free_v[nfree++] = current.vertex[c]; free_v[nfree++] = current.vertex[d]; - } // end of AD.AO >= 0 - } // end of AC.AO >= 0 - } // end of AB.AO >= 0 + } // end of AD.AO >= 0 + } // end of AC.AO >= 0 + } // end of AB.AO >= 0 #undef REGION_INSIDE return false; } -void EPA::initialize() -{ +void EPA::initialize() { sv_store = new SimplexV[max_vertex_num]; fc_store = new SimplexF[max_face_num]; status = Failed; normal = Vec3f(0, 0, 0); depth = 0; nextsv = 0; - for(size_t i = 0; i < max_face_num; ++i) - stock.append(&fc_store[max_face_num-i-1]); + for (size_t i = 0; i < max_face_num; ++i) + stock.append(&fc_store[max_face_num - i - 1]); } -bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist) -{ +bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, + FCL_REAL& dist) { Vec3f ab = b->w - a->w; Vec3f n_ab = ab.cross(face->n); FCL_REAL a_dot_nab = a->w.dot(n_ab); - if(a_dot_nab < 0) // the origin is on the outside part of ab + if (a_dot_nab < 0) // the origin is on the outside part of ab { // following is similar to projectOrigin for two points - // however, as we dont need to compute the parameterization, dont need to compute 0 or 1 - FCL_REAL a_dot_ab = a->w.dot(ab); + // however, as we dont need to compute the parameterization, dont need to + // compute 0 or 1 + FCL_REAL a_dot_ab = a->w.dot(ab); FCL_REAL b_dot_ab = b->w.dot(ab); - if(a_dot_ab > 0) + if (a_dot_ab > 0) dist = a->w.norm(); - else if(b_dot_ab < 0) + else if (b_dot_ab < 0) dist = b->w.norm(); - else - { + else { dist = std::sqrt(std::max( - a->w.squaredNorm() - a_dot_ab * a_dot_ab / ab.squaredNorm(), - 0.)); + a->w.squaredNorm() - a_dot_ab * a_dot_ab / ab.squaredNorm(), 0.)); } return true; @@ -1301,10 +1350,9 @@ bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist) return false; } -EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced) -{ - if(stock.root) - { +EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, + bool forced) { + if (stock.root) { SimplexF* face = stock.root; stock.remove(face); hull.append(face); @@ -1314,45 +1362,39 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced) face->vertex[2] = c; face->n = (b->w - a->w).cross(c->w - a->w); FCL_REAL l = face->n.norm(); - - if(l > Eigen::NumTraits::epsilon()) - { + + if (l > Eigen::NumTraits::epsilon()) { face->n /= l; - if(!(getEdgeDist(face, a, b, face->d) || - getEdgeDist(face, b, c, face->d) || - getEdgeDist(face, c, a, face->d))) - { + if (!(getEdgeDist(face, a, b, face->d) || + getEdgeDist(face, b, c, face->d) || + getEdgeDist(face, c, a, face->d))) { face->d = a->w.dot(face->n); } - if(forced || face->d >= -tolerance) + if (forced || face->d >= -tolerance) return face; else status = NonConvex; - } - else + } else status = Degenerated; hull.remove(face); stock.append(face); return NULL; } - + status = stock.root ? OutOfVertices : OutOfFaces; return NULL; } /** @brief Find the best polytope face to split */ -EPA::SimplexF* EPA::findBest() -{ +EPA::SimplexF* EPA::findBest() { SimplexF* minf = hull.root; FCL_REAL mind = minf->d * minf->d; - for(SimplexF* f = minf->l[1]; f; f = f->l[1]) - { + for (SimplexF* f = minf->l[1]; f; f = f->l[1]) { FCL_REAL sqd = f->d * f->d; - if(sqd < mind) - { + if (sqd < mind) { minf = f; mind = sqd; } @@ -1360,14 +1402,11 @@ EPA::SimplexF* EPA::findBest() return minf; } -EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) -{ +EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { GJK::Simplex& simplex = *gjk.getSimplex(); - support_func_guess_t hint (gjk.support_hint); - if((simplex.rank > 1) && gjk.encloseOrigin()) - { - while(hull.root) - { + support_func_guess_t hint(gjk.support_hint); + if ((simplex.rank > 1) && gjk.encloseOrigin()) { + while (hull.root) { SimplexF* f = hull.root; hull.remove(f); stock.append(f); @@ -1376,31 +1415,27 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) status = Valid; nextsv = 0; - if((simplex.vertex[0]->w - simplex.vertex[3]->w).dot - ((simplex.vertex[1]->w - simplex.vertex[3]->w).cross - (simplex.vertex[2]->w - simplex.vertex[3]->w)) < 0) - { + if ((simplex.vertex[0]->w - simplex.vertex[3]->w) + .dot((simplex.vertex[1]->w - simplex.vertex[3]->w) + .cross(simplex.vertex[2]->w - simplex.vertex[3]->w)) < 0) { SimplexV* tmp = simplex.vertex[0]; simplex.vertex[0] = simplex.vertex[1]; simplex.vertex[1] = tmp; } - SimplexF* tetrahedron[] = {newFace(simplex.vertex[0], simplex.vertex[1], - simplex.vertex[2], true), - newFace(simplex.vertex[1], simplex.vertex[0], - simplex.vertex[3], true), - newFace(simplex.vertex[2], simplex.vertex[1], - simplex.vertex[3], true), - newFace(simplex.vertex[0], simplex.vertex[2], - simplex.vertex[3], true) }; + SimplexF* tetrahedron[] = { + newFace(simplex.vertex[0], simplex.vertex[1], simplex.vertex[2], true), + newFace(simplex.vertex[1], simplex.vertex[0], simplex.vertex[3], true), + newFace(simplex.vertex[2], simplex.vertex[1], simplex.vertex[3], true), + newFace(simplex.vertex[0], simplex.vertex[2], simplex.vertex[3], true)}; - if(hull.count == 4) - { - SimplexF* best = findBest(); // find the best face (the face with the minimum distance to origin) to split + if (hull.count == 4) { + SimplexF* best = findBest(); // find the best face (the face with the + // minimum distance to origin) to split SimplexF outer = *best; size_t pass = 0; size_t iterations = 0; - + // set the face connectivity bind(tetrahedron[0], 0, tetrahedron[1], 0); bind(tetrahedron[0], 1, tetrahedron[2], 0); @@ -1410,8 +1445,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) bind(tetrahedron[2], 2, tetrahedron[3], 1); status = Valid; - for(; iterations < max_iterations; ++iterations) - { + for (; iterations < max_iterations; ++iterations) { if (nextsv >= max_vertex_num) { status = OutOfVertices; break; @@ -1421,17 +1455,18 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) SimplexV* w = &sv_store[nextsv++]; bool valid = true; best->pass = ++pass; - // At the moment, SimplexF.n is always normalized. This could be revised in the future... + // At the moment, SimplexF.n is always normalized. This could be revised + // in the future... gjk.getSupport(best->n, true, *w, hint); FCL_REAL wdist = best->n.dot(w->w) - best->d; - if(wdist <= tolerance) { + if (wdist <= tolerance) { status = AccuracyReached; break; } - for(size_t j = 0; (j < 3) && valid; ++j) + for (size_t j = 0; (j < 3) && valid; ++j) valid &= expand(pass, w, best->f[j], best->e[j], horizon); - if(!valid || horizon.nf < 3) { + if (!valid || horizon.nf < 3) { // The status has already been set by the expand function. assert(!(status & Valid)); break; @@ -1457,46 +1492,49 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) status = FallBack; normal = -guess; FCL_REAL nl = normal.norm(); - if(nl > 0) normal /= nl; - else normal = Vec3f(1, 0, 0); + if (nl > 0) + normal /= nl; + else + normal = Vec3f(1, 0, 0); depth = 0; result.rank = 1; result.vertex[0] = simplex.vertex[0]; return status; } - /** @brief the goal is to add a face connecting vertex w and face edge f[e] */ -bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon) -{ +bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, + SimplexHorizon& horizon) { static const size_t nexti[] = {1, 2, 0}; static const size_t previ[] = {2, 0, 1}; - if(f->pass == pass) - { + if (f->pass == pass) { status = InvalidHull; return false; } const size_t e1 = nexti[e]; - - // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f. - if(f->n.dot(w->w - f->vertex[e]->w) < -Eigen::NumTraits::epsilon()) - { + + // case 1: the new face is not degenerated, i.e., the new face is not coplanar + // with the old face f. + if (f->n.dot(w->w - f->vertex[e]->w) < + -Eigen::NumTraits::epsilon()) { SimplexF* nf = newFace(f->vertex[e1], f->vertex[e], w, false); - if(nf) - { + if (nf) { // add face-face connectivity bind(nf, 0, f, e); - - // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge. - // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function. - // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left) - if(horizon.cf) + + // if there is last face in the horizon, then need to add another + // connectivity, i.e. the edge connecting the current new add edge and the + // last new add edge. This does not finish all the connectivities because + // the final face need to connect with the first face, this will be + // handled in the evaluate function. Notice the face is anti-clockwise, so + // the edges are 0 (bottom), 1 (right), 2 (left) + if (horizon.cf) bind(nf, 2, horizon.cf, 1); else - horizon.ff = nf; - + horizon.ff = nf; + horizon.cf = nf; ++horizon.nf; return true; @@ -1504,11 +1542,12 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon return false; } - // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face + // case 2: the new face is coplanar with the old face f. We need to add two + // faces and delete the old face const size_t e2 = previ[e]; f->pass = pass; - if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon)) - { + if (expand(pass, w, f->f[e1], f->e[e1], horizon) && + expand(pass, w, f->f[e2], f->e[e2], horizon)) { hull.remove(f); stock.append(f); return true; @@ -1516,16 +1555,15 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon return false; } -bool EPA::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) -{ +bool EPA::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) { bool res = details::getClosestPoints(result, w0, w1); if (!res) return false; - details::inflate (shape, w0, w1); + details::inflate(shape, w0, w1); return true; } -} // details +} // namespace details -} // fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index d18d0519e..d0709f31d 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -43,86 +43,82 @@ #include #include "details.h" -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { // Shape intersect algorithms based on: // - built-in function: 0 // - GJK: 1 // // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | +// | | box | sphere | capsule | cone | cylinder | plane | half-space +// | triangle | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | box | 0 | 0 | 1 | 1 | 1 | 0 | 0 | 1 | +// | box | 0 | 0 | 1 | 1 | 1 | 0 | 0 | 1 +// | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| 0 | 0 | 1 | 1 | 0 | 0 | 0 | +// | sphere |/////| 0 | 0 | 1 | 1 | 0 | 0 | 0 +// | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | capsule |/////|////////| 1 | 1 | 1 | 0 | 0 | 1 | +// | capsule |/////|////////| 1 | 1 | 1 | 0 | 0 | 1 +// | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1 | +// | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1 +// | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1 | +// | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1 +// | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | plane |/////|////////|/////////|//////|//////////| 0 | 0 | 0 | +// | plane |/////|////////|/////////|//////|//////////| 0 | 0 | 0 +// | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | half-space |/////|////////|/////////|//////|//////////|///////| 0 | 0 | +// | half-space |/////|////////|/////////|//////|//////////|///////| 0 | 0 +// | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| 1 | +// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| +// 1 | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ - -#define SHAPE_INTERSECT_INVERTED(Shape1,Shape2) \ - template<> \ - bool GJKSolver::shapeIntersect \ - (const Shape1& s1, const Transform3f& tf1, \ - const Shape2& s2, const Transform3f& tf2, \ - FCL_REAL& distance_lower_bound, bool enable_penetration, \ - Vec3f* contact_points, Vec3f* normal) const \ - { \ - bool res = shapeIntersect (s2, tf2, s1, tf1, distance_lower_bound, \ - enable_penetration, contact_points, normal); \ - (*normal) *= -1.0; \ - return res; \ +#define SHAPE_INTERSECT_INVERTED(Shape1, Shape2) \ + template <> \ + bool GJKSolver::shapeIntersect( \ + const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ + const Transform3f& tf2, FCL_REAL& distance_lower_bound, \ + bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const { \ + bool res = shapeIntersect(s2, tf2, s1, tf1, distance_lower_bound, \ + enable_penetration, contact_points, normal); \ + (*normal) *= -1.0; \ + return res; \ } -template<> -bool GJKSolver::shapeIntersect - (const Sphere &s1, const Transform3f& tf1, - const Capsule &s2, const Transform3f& tf2, - FCL_REAL& distance_lower_bound, - bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Sphere& s1, const Transform3f& tf1, const Capsule& s2, + const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, + Vec3f* contact_points, Vec3f* normal) const { return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, distance_lower_bound, - contact_points, normal); + contact_points, normal); } SHAPE_INTERSECT_INVERTED(Capsule, Sphere) -template<> -bool GJKSolver::shapeIntersect - (const Sphere& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& distance_lower_bound, - bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Sphere& s1, const Transform3f& tf1, const Sphere& s2, + const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, + Vec3f* contact_points, Vec3f* normal) const { return details::sphereSphereIntersect(s1, tf1, s2, tf2, distance_lower_bound, - contact_points, normal); + contact_points, normal); } -template<> -bool GJKSolver::shapeIntersect - (const Box & s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Box& s1, const Transform3f& tf1, const Sphere& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f ps, pb, n; - bool res = details::boxSphereDistance (s1, tf1, s2, tf2, distance, ps, pb, n); - if (normal) *normal = n; - if (contact_points) *contact_points = pb; + bool res = details::boxSphereDistance(s1, tf1, s2, tf2, distance, ps, pb, n); + if (normal) *normal = n; + if (contact_points) *contact_points = pb; return res; } @@ -134,22 +130,22 @@ bool GJKSolver::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, - Vec3f* contact_points, Vec3f* normal) const + Vec3f* contact_points, Vec3f* normal) +const { - return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, &distance_lower_bound, normal); + return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, +&distance_lower_bound, normal); } */ -template<> -bool GJKSolver::shapeIntersect - (const Sphere& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, - p2, n); + bool res = + details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -157,16 +153,14 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Halfspace, Sphere) -template<> -bool GJKSolver::shapeIntersect - (const Box& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Box& s1, const Transform3f& tf1, const Halfspace& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::boxHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, - p2, n); + bool res = + details::boxHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -174,16 +168,14 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Halfspace, Box) -template<> -bool GJKSolver::shapeIntersect - (const Capsule& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::capsuleHalfspaceIntersect - (s1, tf1, s2, tf2, distance, p1, p2, n); + bool res = + details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -191,16 +183,14 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Halfspace, Capsule) -template<> -bool GJKSolver::shapeIntersect - (const Cylinder& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::cylinderHalfspaceIntersect - (s1, tf1, s2, tf2, distance, p1, p2, n); + bool res = details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, + p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -208,16 +198,14 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Halfspace, Cylinder) -template<> -bool GJKSolver::shapeIntersect - (const Cone& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Cone& s1, const Transform3f& tf1, const Halfspace& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::coneHalfspaceIntersect - (s1, tf1, s2, tf2, distance, p1, p2, n); + bool res = + details::coneHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -225,50 +213,45 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Halfspace, Cone) -template<> -bool GJKSolver::shapeIntersect - (const Halfspace& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* /*contact_points*/, Vec3f* /*normal*/) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* /*contact_points*/, + Vec3f* /*normal*/) const { Halfspace s; Vec3f p, d; FCL_REAL depth; int ret; bool res = details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret); - distance = - depth; + distance = -depth; return res; } -template<> -bool GJKSolver::shapeIntersect - (const Plane& s1, const Transform3f& tf1, - const Halfspace& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* /*contact_points*/, Vec3f* /*normal*/) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Plane& s1, const Transform3f& tf1, const Halfspace& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* /*contact_points*/, + Vec3f* /*normal*/) const { Plane pl; Vec3f p, d; FCL_REAL depth; int ret; - bool res = details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); - distance = - depth; + bool res = + details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); + distance = -depth; return res; } SHAPE_INTERSECT_INVERTED(Halfspace, Plane) -template<> -bool GJKSolver::shapeIntersect - (const Sphere& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Sphere& s1, const Transform3f& tf1, const Plane& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::spherePlaneIntersect(s1, tf1, s2, tf2, distance, p1, - p2, n); + bool res = + details::spherePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -276,16 +259,13 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Plane, Sphere) -template<> -bool GJKSolver::shapeIntersect - (const Box& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Box& s1, const Transform3f& tf1, const Plane& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::boxPlaneIntersect - (s1, tf1, s2, tf2, distance, p1, p2, n); + bool res = details::boxPlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -293,16 +273,14 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Plane, Box) -template<> -bool GJKSolver::shapeIntersect - (const Capsule& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Capsule& s1, const Transform3f& tf1, const Plane& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::capsulePlaneIntersect(s1, tf1, s2, tf2, distance, p1, - p2, n); + bool res = + details::capsulePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -310,16 +288,14 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Plane, Capsule) -template<> -bool GJKSolver::shapeIntersect - (const Cylinder& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Cylinder& s1, const Transform3f& tf1, const Plane& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::cylinderPlaneIntersect - (s1, tf1, s2, tf2, distance, p1, p2, n); + bool res = + details::cylinderPlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -327,16 +303,13 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Plane, Cylinder) -template<> -bool GJKSolver::shapeIntersect - (const Cone& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ +template <> +bool GJKSolver::shapeIntersect( + const Cone& s1, const Transform3f& tf1, const Plane& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { Vec3f p1, p2, n; - bool res = details::conePlaneIntersect - (s1, tf1, s2, tf2, distance, p1, p2, n); + bool res = details::conePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n); if (contact_points) *contact_points = p1; if (normal) *normal = n; return res; @@ -344,158 +317,160 @@ bool GJKSolver::shapeIntersect SHAPE_INTERSECT_INVERTED(Plane, Cone) -template<> -bool GJKSolver::shapeIntersect - (const Plane& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - FCL_REAL& distance, bool, - Vec3f* contact_points, Vec3f* normal) const -{ - return details::planeIntersect(s1, tf1, s2, tf2, contact_points, &distance, normal); +template <> +bool GJKSolver::shapeIntersect( + const Plane& s1, const Transform3f& tf1, const Plane& s2, + const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points, + Vec3f* normal) const { + return details::planeIntersect(s1, tf1, s2, tf2, contact_points, &distance, + normal); } -template<> -bool GJKSolver::shapeTriangleInteraction - (const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ - return details::sphereTriangleIntersect - (s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), - distance, p1, p2, normal); +template <> +bool GJKSolver::shapeTriangleInteraction( + const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, + Vec3f& p2, Vec3f& normal) const { + return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), + tf2.transform(P2), tf2.transform(P3), + distance, p1, p2, normal); } -template<> -bool GJKSolver::shapeTriangleInteraction - (const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ - return details::halfspaceTriangleIntersect - (s, tf1, P1, P2, P3, tf2, distance, p1, p2, normal); +template <> +bool GJKSolver::shapeTriangleInteraction( + const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, + const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { + return details::halfspaceTriangleIntersect(s, tf1, P1, P2, P3, tf2, distance, + p1, p2, normal); } -template<> -bool GJKSolver::shapeTriangleInteraction - (const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ - return details::planeTriangleIntersect - (s, tf1, P1, P2, P3, tf2, distance, p1, p2, normal); +template <> +bool GJKSolver::shapeTriangleInteraction(const Plane& s, const Transform3f& tf1, + const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, + const Transform3f& tf2, + FCL_REAL& distance, Vec3f& p1, + Vec3f& p2, Vec3f& normal) const { + return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, distance, p1, + p2, normal); } // Shape distance algorithms not using built-in GJK algorithm // // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | +// | | box | sphere | capsule | cone | cylinder | plane | half-space +// | triangle | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | box | | O | | | | | | | +// | box | | O | | | | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | O | | | | | | +// | sphere |/////| O | O | | | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | capsule |/////|////////| O | | | | | | +// | capsule |/////|////////| O | | | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | cone |/////|////////|/////////| | | | | | +// | cone |/////|////////|/////////| | | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | cylinder |/////|////////|/////////|//////| | | | | +// | cylinder |/////|////////|/////////|//////| | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | plane |/////|////////|/////////|//////|//////////| | | | +// | plane |/////|////////|/////////|//////|//////////| | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | half-space |/////|////////|/////////|//////|//////////|///////| | | +// | half-space |/////|////////|/////////|//////|//////////|///////| | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| | +// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| +// | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -template<> -bool GJKSolver::shapeDistance -(const Sphere& s1, const Transform3f& tf1, - const Capsule& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ +template <> +bool GJKSolver::shapeDistance(const Sphere& s1, + const Transform3f& tf1, + const Capsule& s2, + const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, + Vec3f& p2, Vec3f& normal) const { return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2, normal); } -template<> -bool GJKSolver::shapeDistance -(const Capsule& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ +template <> +bool GJKSolver::shapeDistance(const Capsule& s1, + const Transform3f& tf1, + const Sphere& s2, + const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, + Vec3f& p2, Vec3f& normal) const { return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1, normal); } -template<> -bool GJKSolver::shapeDistance -(const Box & s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ - return !details::boxSphereDistance (s1, tf1, s2, tf2, dist, p1, p2, normal); +template <> +bool GJKSolver::shapeDistance(const Box& s1, + const Transform3f& tf1, + const Sphere& s2, + const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { + return !details::boxSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal); } -template<> -bool GJKSolver::shapeDistance -(const Sphere& s1, const Transform3f& tf1, - const Box & s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ - bool collide = details::boxSphereDistance (s2, tf2, s1, tf1, dist, p2, p1, normal); +template <> +bool GJKSolver::shapeDistance(const Sphere& s1, + const Transform3f& tf1, + const Box& s2, + const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { + bool collide = + details::boxSphereDistance(s2, tf2, s1, tf1, dist, p2, p1, normal); normal *= -1; return !collide; } -template<> -bool GJKSolver::shapeDistance -(const Sphere& s1, const Transform3f& tf1, - const Cylinder& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ - return details::sphereCylinderDistance - (s1, tf1, s2, tf2, dist, p1, p2, normal); +template <> +bool GJKSolver::shapeDistance( + const Sphere& s1, const Transform3f& tf1, const Cylinder& s2, + const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { + return details::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2, + normal); } -template<> -bool GJKSolver::shapeDistance -(const Cylinder& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ - return details::sphereCylinderDistance - (s2, tf2, s1, tf1, dist, p2, p1, normal); +template <> +bool GJKSolver::shapeDistance( + const Cylinder& s1, const Transform3f& tf1, const Sphere& s2, + const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { + return details::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1, + normal); } -template<> -bool GJKSolver::shapeDistance -(const Sphere& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ +template <> +bool GJKSolver::shapeDistance(const Sphere& s1, + const Transform3f& tf1, + const Sphere& s2, + const Transform3f& tf2, + FCL_REAL& dist, Vec3f& p1, + Vec3f& p2, Vec3f& normal) const { return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal); } -template<> -bool GJKSolver::shapeDistance -(const Capsule& /*s1*/, const Transform3f& /*tf1*/, - const Capsule& /*s2*/, const Transform3f& /*tf2*/, - FCL_REAL& /*dist*/, Vec3f& /*p1*/, Vec3f& /*p2*/, Vec3f& /*normal*/) const -{ - abort (); +template <> +bool GJKSolver::shapeDistance( + const Capsule& /*s1*/, const Transform3f& /*tf1*/, const Capsule& /*s2*/, + const Transform3f& /*tf2*/, FCL_REAL& /*dist*/, Vec3f& /*p1*/, + Vec3f& /*p2*/, Vec3f& /*normal*/) const { + abort(); } -template<> -bool GJKSolver::shapeDistance -(const TriangleP& s1, const Transform3f& tf1, - const TriangleP& s2, const Transform3f& tf2, - FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const -{ - const TriangleP - t1 (tf1.transform(s1.a), tf1.transform(s1.b), tf1.transform(s1.c)), - t2 (tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c)); - +template <> +bool GJKSolver::shapeDistance( + const TriangleP& s1, const Transform3f& tf1, const TriangleP& s2, + const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { + const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b), + tf1.transform(s1.c)), + t2(tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c)); + Vec3f guess; support_func_guess_t support_hint; - if(enable_cached_guess) { + if (enable_cached_guess) { guess = cached_guess; support_hint = support_func_cached_guess; } else { @@ -503,36 +478,33 @@ bool GJKSolver::shapeDistance guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3; } bool enable_penetration = true; - + details::MinkowskiDiff shape; - shape.set (&t1, &t2); - - details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance); + shape.set(&t1, &t2); + + details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); - if(enable_cached_guess) { + if (enable_cached_guess) { cached_guess = gjk.getGuessFromSimplex(); support_func_cached_guess = gjk.support_hint; } - - gjk.getClosestPoints (shape, p1, p2); - - if((gjk_status == details::GJK::Valid) || - (gjk_status == details::GJK::Failed)) - { + + gjk.getClosestPoints(shape, p1, p2); + + if ((gjk_status == details::GJK::Valid) || + (gjk_status == details::GJK::Failed)) { // TODO On degenerated case, the closest point may be wrong // (i.e. an object face normal is colinear to gjk.ray // assert (dist == (w0 - w1).norm()); dist = gjk.distance; - + return true; - } - else if (gjk_status == details::GJK::Inside) - { + } else if (gjk_status == details::GJK::Inside) { if (enable_penetration) { - FCL_REAL penetrationDepth = details::computePenetration - (t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal); + FCL_REAL penetrationDepth = details::computePenetration( + t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal); dist = -penetrationDepth; - assert (dist <= 1e-6); + assert(dist <= 1e-6); // GJK says Inside when below GJK.tolerance. So non intersecting // triangle may trigger "Inside" and have no penetration. return penetrationDepth < 0; @@ -540,9 +512,9 @@ bool GJKSolver::shapeDistance dist = 0; return false; } - assert (false && "should not reach this point"); + assert(false && "should not reach this point"); return false; } -} // fcl +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/src/octree.cpp b/src/octree.cpp index 11a5a7697..3d2ff6ea1 100644 --- a/src/octree.cpp +++ b/src/octree.cpp @@ -34,61 +34,32 @@ #include -namespace hpp -{ -namespace fcl -{ -namespace internal -{ -struct Neighbors -{ +namespace hpp { +namespace fcl { +namespace internal { +struct Neighbors { char value; - Neighbors() : value(0){} - bool minusX() const{ - return value & 0x1; - } - bool plusX() const{ - return value & 0x2; - } - bool minusY() const{ - return value & 0x4; - } - bool plusY() const{ - return value & 0x8; - } - bool minusZ() const{ - return value & 0x10; - } - bool plusZ() const{ - return value & 0x20; - } - void hasNeighboordMinusX(){ - value |= 0x1; - } - void hasNeighboordPlusX(){ - value |= 0x2; - } - void hasNeighboordMinusY(){ - value |= 0x4; - } - void hasNeighboordPlusY(){ - value |= 0x8; - } - void hasNeighboordMinusZ(){ - value |= 0x10; - } - void hasNeighboordPlusZ(){ - value |= 0x20; - } -}; //struct neighbors + Neighbors() : value(0) {} + bool minusX() const { return value & 0x1; } + bool plusX() const { return value & 0x2; } + bool minusY() const { return value & 0x4; } + bool plusY() const { return value & 0x8; } + bool minusZ() const { return value & 0x10; } + bool plusZ() const { return value & 0x20; } + void hasNeighboordMinusX() { value |= 0x1; } + void hasNeighboordPlusX() { value |= 0x2; } + void hasNeighboordMinusY() { value |= 0x4; } + void hasNeighboordPlusY() { value |= 0x8; } + void hasNeighboordMinusZ() { value |= 0x10; } + void hasNeighboordPlusZ() { value |= 0x20; } +}; // struct neighbors void computeNeighbors(const std::vector >& boxes, - std::vector& neighbors) -{ + std::vector& neighbors) { typedef std::vector > VectorArray6; FCL_REAL fixedSize = -1; FCL_REAL e(1e-8); - for (std::size_t i=0; i < boxes.size(); ++i){ + for (std::size_t i = 0; i < boxes.size(); ++i) { const boost::array& box(boxes[i]); Neighbors& n(neighbors[i]); FCL_REAL x(box[0]); @@ -100,37 +71,40 @@ void computeNeighbors(const std::vector >& boxes, else assert(s == fixedSize); - for(VectorArray6::const_iterator it = boxes.begin(); - it != boxes.end(); ++it) - { - const boost::array & otherBox = *it; + for (VectorArray6::const_iterator it = boxes.begin(); it != boxes.end(); + ++it) { + const boost::array& otherBox = *it; FCL_REAL xo(otherBox[0]); FCL_REAL yo(otherBox[1]); FCL_REAL zo(otherBox[2]); // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){ // continue; // } - if ((fabs(x-xo-s) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo) < e)){ + if ((fabs(x - xo - s) < e) && (fabs(y - yo) < e) && (fabs(z - zo) < e)) { n.hasNeighboordMinusX(); - } else if((fabs(x-xo+s) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo) < e)){ + } else if ((fabs(x - xo + s) < e) && (fabs(y - yo) < e) && + (fabs(z - zo) < e)) { n.hasNeighboordPlusX(); - } else if ((fabs(x-xo) < e)&&(fabs(y-yo-s) < e)&&(fabs(z-zo) < e)){ + } else if ((fabs(x - xo) < e) && (fabs(y - yo - s) < e) && + (fabs(z - zo) < e)) { n.hasNeighboordMinusY(); - } else if((fabs(x-xo) < e)&&(fabs(y-yo+s) < e)&&(fabs(z-zo) < e)){ + } else if ((fabs(x - xo) < e) && (fabs(y - yo + s) < e) && + (fabs(z - zo) < e)) { n.hasNeighboordPlusY(); - } else if ((fabs(x-xo) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo-s) < e)){ + } else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) && + (fabs(z - zo - s) < e)) { n.hasNeighboordMinusZ(); - } else if((fabs(x-xo) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo+s) < e)){ + } else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) && + (fabs(z - zo + s) < e)) { n.hasNeighboordPlusZ(); } } } } -} // namespace internal +} // namespace internal -void OcTree::exportAsObjFile(const std::string& filename) const -{ +void OcTree::exportAsObjFile(const std::string& filename) const { std::vector > boxes(this->toBoxes()); std::vector neighbors(boxes.size()); internal::computeNeighbors(boxes, neighbors); @@ -143,8 +117,7 @@ void OcTree::exportAsObjFile(const std::string& filename) const typedef std::vector VectorArray4; std::vector faces; - for (std::size_t i=0; i& box(boxes[i]); internal::Neighbors& n(neighbors[i]); @@ -153,87 +126,78 @@ void OcTree::exportAsObjFile(const std::string& filename) const FCL_REAL z(box[2]); FCL_REAL size(box[3]); - vertices.push_back(Vec3f(x-.5*size, y-.5*size, z-.5*size)); - vertices.push_back(Vec3f(x+.5*size, y-.5*size, z-.5*size)); - vertices.push_back(Vec3f(x-.5*size, y+.5*size, z-.5*size)); - vertices.push_back(Vec3f(x+.5*size, y+.5*size, z-.5*size)); - vertices.push_back(Vec3f(x-.5*size, y-.5*size, z+.5*size)); - vertices.push_back(Vec3f(x+.5*size, y-.5*size, z+.5*size)); - vertices.push_back(Vec3f(x-.5*size, y+.5*size, z+.5*size)); - vertices.push_back(Vec3f(x+.5*size, y+.5*size, z+.5*size)); + vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z - .5 * size)); + vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z - .5 * size)); + vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z - .5 * size)); + vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z - .5 * size)); + vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z + .5 * size)); + vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z + .5 * size)); + vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z + .5 * size)); + vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z + .5 * size)); // Add face only if box has no neighbor with the same face - if (!n.minusX()) - { - Array4 a = {{8*i + 1, 8*i + 5, 8*i + 7, 8*i + 3}}; + if (!n.minusX()) { + Array4 a = {{8 * i + 1, 8 * i + 5, 8 * i + 7, 8 * i + 3}}; faces.push_back(a); } - if (!n.plusX()) - { - Array4 a = {{8*i + 2, 8*i + 4, 8*i + 8, 8*i + 6}}; + if (!n.plusX()) { + Array4 a = {{8 * i + 2, 8 * i + 4, 8 * i + 8, 8 * i + 6}}; faces.push_back(a); } - if (!n.minusY()) - { - Array4 a = {{8*i + 1, 8*i + 2, 8*i + 6, 8*i + 5}}; + if (!n.minusY()) { + Array4 a = {{8 * i + 1, 8 * i + 2, 8 * i + 6, 8 * i + 5}}; faces.push_back(a); } - if (!n.plusY()) - { - Array4 a = {{8*i + 4, 8*i + 3, 8*i + 7, 8*i + 8}}; + if (!n.plusY()) { + Array4 a = {{8 * i + 4, 8 * i + 3, 8 * i + 7, 8 * i + 8}}; faces.push_back(a); } - if (!n.minusZ()) - { - Array4 a = {{8*i + 1, 8*i + 2, 8*i + 4, 8*i + 3}}; + if (!n.minusZ()) { + Array4 a = {{8 * i + 1, 8 * i + 2, 8 * i + 4, 8 * i + 3}}; faces.push_back(a); } - if (!n.plusZ()) - { - Array4 a = {{8*i + 5, 8*i + 6, 8*i + 8, 8*i + 7}}; + if (!n.plusZ()) { + Array4 a = {{8 * i + 5, 8 * i + 6, 8 * i + 8, 8 * i + 7}}; faces.push_back(a); } - } // write obj in a file std::ofstream os; os.open(filename); if (!os.is_open()) - throw std::runtime_error(std::string("failed to open file \"")+ - filename + std::string("\"")); + throw std::runtime_error(std::string("failed to open file \"") + filename + + std::string("\"")); // write vertices os << "# list of vertices\n"; - for (VectorVec3f::const_iterator it = vertices.begin(); - it != vertices.end(); ++it) - { - const Vec3f & v = *it; + for (VectorVec3f::const_iterator it = vertices.begin(); it != vertices.end(); + ++it) { + const Vec3f& v = *it; os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n'; } os << "\n# list of faces\n"; - for (VectorArray4::const_iterator it = faces.begin(); - it != faces.end(); ++it) - { - const Array4 & f = *it; + for (VectorArray4::const_iterator it = faces.begin(); it != faces.end(); + ++it) { + const Array4& f = *it; os << "f " << f[0] << " " << f[1] << " " << f[2] << " " << f[3] << '\n'; } } -OcTreePtr_t makeOctree(const Eigen::Matrix& point_cloud, - const FCL_REAL resolution) -{ - typedef Eigen::Matrix InputType; +OcTreePtr_t makeOctree( + const Eigen::Matrix& point_cloud, + const FCL_REAL resolution) { + typedef Eigen::Matrix InputType; typedef InputType::ConstRowXpr RowType; shared_ptr octree(new octomap::OcTree(resolution)); - for(Eigen::DenseIndex row_id = 0; row_id < point_cloud.rows(); ++row_id) - { + for (Eigen::DenseIndex row_id = 0; row_id < point_cloud.rows(); ++row_id) { RowType row = point_cloud.row(row_id); - octomap::point3d p(static_cast(row[0]),static_cast(row[1]),static_cast(row[2])); + octomap::point3d p(static_cast(row[0]), static_cast(row[1]), + static_cast(row[2])); octree->updateNode(p, true); } octree->updateInnerOccupancy(); - return OcTreePtr_t (new OcTree(octree)); -} -} + return OcTreePtr_t(new OcTree(octree)); } +} // namespace fcl +} // namespace hpp diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp index b23a5644e..3750a2a27 100644 --- a/src/shape/convex.cpp +++ b/src/shape/convex.cpp @@ -12,58 +12,58 @@ using orgQhull::Qhull; using orgQhull::QhullFacet; using orgQhull::QhullPoint; +using orgQhull::QhullRidgeSet; using orgQhull::QhullVertex; using orgQhull::QhullVertexList; using orgQhull::QhullVertexSet; -using orgQhull::QhullRidgeSet; #endif namespace hpp { namespace fcl { ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, - bool keepTriangles, const char* qhullCommand) -{ + bool keepTriangles, + const char* qhullCommand) { #ifdef HPP_FCL_HAS_QHULL if (num_points <= 3) { - throw std::invalid_argument("You shouldn't use this function with less than" + throw std::invalid_argument( + "You shouldn't use this function with less than" " 4 points."); } assert(pts[0].data() + 3 == pts[1].data()); Qhull qh; - const char* command = qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : ""); + const char* command = + qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : ""); qh.runQhull("", 3, num_points, pts[0].data(), command); - if (qh.qhullStatus() != qh_ERRnone) - { - if (qh.hasQhullMessage()) - std::cerr << qh.qhullMessage() << std::endl; - throw std::logic_error ("Qhull failed"); + if (qh.qhullStatus() != qh_ERRnone) { + if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; + throw std::logic_error("Qhull failed"); } typedef std::size_t index_type; typedef int size_type; // Map index in pts to index in vertices. -1 means not used - std::vector pts_to_vertices (num_points, -1); + std::vector pts_to_vertices(num_points, -1); // Initialize the vertices int nvertex = qh.vertexCount(); Vec3f* vertices = new Vec3f[nvertex]; - QhullVertexList vertexList (qh.vertexList()); + QhullVertexList vertexList(qh.vertexList()); int i_vertex = 0; for (QhullVertexList::const_iterator v = vertexList.begin(); - v != vertexList.end(); ++v) { - QhullPoint pt ((*v).point()); + v != vertexList.end(); ++v) { + QhullPoint pt((*v).point()); pts_to_vertices[pt.id()] = i_vertex; vertices[i_vertex] = Vec3f(pt[0], pt[1], pt[2]); ++i_vertex; } assert(i_vertex == nvertex); - Convex* convex_tri (NULL); - ConvexBase* convex (NULL); + Convex* convex_tri(NULL); + ConvexBase* convex(NULL); if (keepTriangles) convex = convex_tri = new Convex(); else @@ -72,7 +72,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, // Build the neighbors convex->neighbors = new Neighbors[nvertex]; - std::vector > nneighbors (nvertex); + std::vector > nneighbors(nvertex); if (keepTriangles) { convex_tri->num_polygons = qh.facetCount(); convex_tri->polygons = new Triangle[convex_tri->num_polygons]; @@ -82,36 +82,35 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, unsigned int i_polygon = 0; // Compute the neighbors from the edges of the faces. - for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); facet = facet.next()) { + for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); + facet = facet.next()) { if (facet.isSimplicial()) { // In 3D, simplicial faces have 3 vertices. We mark them as neighbors. - QhullVertexSet f_vertices (facet.vertices()); + QhullVertexSet f_vertices(facet.vertices()); int n = f_vertices.count(); assert(n == 3); - Triangle tri ( - pts_to_vertices[f_vertices[0].point().id()], - pts_to_vertices[f_vertices[1].point().id()], - pts_to_vertices[f_vertices[2].point().id()]); + Triangle tri(pts_to_vertices[f_vertices[0].point().id()], + pts_to_vertices[f_vertices[1].point().id()], + pts_to_vertices[f_vertices[2].point().id()]); if (keepTriangles) convex_tri->polygons[i_polygon++] = tri; - for(size_type j = 0; j < n; ++j) - { - size_type i = (j==0 ) ? n-1 : j-1; - size_type k = (j==n-1) ? 0 : j+1; + for (size_type j = 0; j < n; ++j) { + size_type i = (j == 0) ? n - 1 : j - 1; + size_type k = (j == n - 1) ? 0 : j + 1; // Update neighbors of pj; if (nneighbors[tri[j]].insert(tri[i]).second) c_nneighbors++; if (nneighbors[tri[j]].insert(tri[k]).second) c_nneighbors++; } } else { - if (keepTriangles) { // TODO I think there is a memory leak here. - throw std::invalid_argument("You requested to keep triangles so you " + if (keepTriangles) { // TODO I think there is a memory leak here. + throw std::invalid_argument( + "You requested to keep triangles so you " "must pass option \"Qt\" to qhull via the qhull command argument."); } // Non-simplicial faces have more than 3 vertices and contains a list of // rigdes. Ridges are (3-1)D simplex (i.e. one edge). We mark the two // vertices of each ridge as neighbors. - QhullRidgeSet f_ridges (facet.ridges()); - for(size_type j = 0; j < f_ridges.count(); ++j) - { + QhullRidgeSet f_ridges(facet.ridges()); + for (size_type j = 0; j < f_ridges.count(); ++j) { assert(f_ridges[j].vertices().count() == 2); index_type pi = pts_to_vertices[f_ridges[j].vertices()[0].point().id()], pj = pts_to_vertices[f_ridges[j].vertices()[1].point().id()]; @@ -129,20 +128,22 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, for (int i = 0; i < nvertex; ++i) { Neighbors& n = convex->neighbors[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) - throw std::logic_error ("Too many neighbors."); + throw std::logic_error("Too many neighbors."); n.count_ = (unsigned char)nneighbors[i].size(); - n.n_ = p_nneighbors; - p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors); + n.n_ = p_nneighbors; + p_nneighbors = + std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors); } - assert (p_nneighbors == convex->nneighbors_ + c_nneighbors); + assert(p_nneighbors == convex->nneighbors_ + c_nneighbors); return convex; #else - throw std::logic_error("Library built without qhull. Cannot build object of this type."); + throw std::logic_error( + "Library built without qhull. Cannot build object of this type."); HPP_FCL_UNUSED_VARIABLE(pts); HPP_FCL_UNUSED_VARIABLE(num_points); HPP_FCL_UNUSED_VARIABLE(keepTriangles); HPP_FCL_UNUSED_VARIABLE(qhullCommand); #endif } -} // namespace fcl -} // namespace hpp +} // namespace fcl +} // namespace hpp diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index 3ab39d44a..8c05a2d02 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -35,44 +35,39 @@ /** \author Jia Pan */ - #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -void ConvexBase::initialize(bool own_storage, Vec3f* points_, unsigned int num_points_) -{ - points = points_; - num_points = num_points_; +void ConvexBase::initialize(bool own_storage, Vec3f* points_, + unsigned int num_points_) { + points = points_; + num_points = num_points_; own_storage_ = own_storage; computeCenter(); } -void ConvexBase::set(bool own_storage_, Vec3f* points_, unsigned int num_points_) -{ - if(own_storage_ && points) delete [] points; - initialize(own_storage_,points_,num_points_); +void ConvexBase::set(bool own_storage_, Vec3f* points_, + unsigned int num_points_) { + if (own_storage_ && points) delete[] points; + initialize(own_storage_, points_, num_points_); } -ConvexBase::ConvexBase(const ConvexBase& other) : - ShapeBase (other), - num_points (other.num_points), - center (other.center), - own_storage_ (other.own_storage_) -{ - if (neighbors) delete [] neighbors; - if (nneighbors_) delete [] nneighbors_; +ConvexBase::ConvexBase(const ConvexBase& other) + : ShapeBase(other), + num_points(other.num_points), + center(other.center), + own_storage_(other.own_storage_) { + if (neighbors) delete[] neighbors; + if (nneighbors_) delete[] nneighbors_; if (own_storage_) { - if (own_storage_ && points) delete [] points; + if (own_storage_ && points) delete[] points; points = new Vec3f[num_points]; std::copy(other.points, other.points + num_points, points); - } - else + } else points = other.points; neighbors = new Neighbors[num_points]; @@ -87,124 +82,103 @@ ConvexBase::ConvexBase(const ConvexBase& other) : ShapeBase::operator=(*this); } -ConvexBase::~ConvexBase () -{ - if (neighbors) delete [] neighbors; - if (nneighbors_) delete [] nneighbors_; - if (own_storage_ && points) delete [] points; +ConvexBase::~ConvexBase() { + if (neighbors) delete[] neighbors; + if (nneighbors_) delete[] nneighbors_; + if (own_storage_ && points) delete[] points; } -void ConvexBase::computeCenter() -{ +void ConvexBase::computeCenter() { center.setZero(); - for(std::size_t i = 0; i < num_points; ++i) - center += points[i]; // TODO(jcarpent): vectorization + for (std::size_t i = 0; i < num_points; ++i) + center += points[i]; // TODO(jcarpent): vectorization center /= num_points; } -void Halfspace::unitNormalTest() -{ +void Halfspace::unitNormalTest() { FCL_REAL l = n.norm(); - if(l > 0) - { + if (l > 0) { FCL_REAL inv_l = 1.0 / l; n *= inv_l; d *= inv_l; - } - else - { + } else { n << 1, 0, 0; d = 0; } } -void Plane::unitNormalTest() -{ +void Plane::unitNormalTest() { FCL_REAL l = n.norm(); - if(l > 0) - { + if (l > 0) { FCL_REAL inv_l = 1.0 / l; n *= inv_l; d *= inv_l; - } - else - { + } else { n << 1, 0, 0; d = 0; } } -void Box::computeLocalAABB() -{ +void Box::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -void Sphere::computeLocalAABB() -{ +void Sphere::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = radius; } -void Ellipsoid::computeLocalAABB() -{ +void Ellipsoid::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -void Capsule::computeLocalAABB() -{ +void Capsule::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -void Cone::computeLocalAABB() -{ +void Cone::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -void Cylinder::computeLocalAABB() -{ +void Cylinder::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -void ConvexBase::computeLocalAABB() -{ +void ConvexBase::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -void Halfspace::computeLocalAABB() -{ +void Halfspace::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -void Plane::computeLocalAABB() -{ +void Plane::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -void TriangleP::computeLocalAABB() -{ +void TriangleP::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index a4588b3b6..181487cde 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -35,40 +35,35 @@ /** \author Jia Pan */ - #include #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -namespace details -{ +namespace details { -std::vector getBoundVertices(const Box& box, const Transform3f& tf) -{ +std::vector getBoundVertices(const Box& box, const Transform3f& tf) { std::vector result(8); FCL_REAL a = box.halfSide[0]; FCL_REAL b = box.halfSide[1]; FCL_REAL c = box.halfSide[2]; - result[0] = tf.transform(Vec3f( a, b, c)); - result[1] = tf.transform(Vec3f( a, b, -c)); - result[2] = tf.transform(Vec3f( a, -b, c)); - result[3] = tf.transform(Vec3f( a, -b, -c)); - result[4] = tf.transform(Vec3f(-a, b, c)); - result[5] = tf.transform(Vec3f(-a, b, -c)); - result[6] = tf.transform(Vec3f(-a, -b, c)); + result[0] = tf.transform(Vec3f(a, b, c)); + result[1] = tf.transform(Vec3f(a, b, -c)); + result[2] = tf.transform(Vec3f(a, -b, c)); + result[3] = tf.transform(Vec3f(a, -b, -c)); + result[4] = tf.transform(Vec3f(-a, b, c)); + result[5] = tf.transform(Vec3f(-a, b, -c)); + result[6] = tf.transform(Vec3f(-a, -b, c)); result[7] = tf.transform(Vec3f(-a, -b, -c)); return result; } // we use icosahedron to bound the sphere -std::vector getBoundVertices(const Sphere& sphere, const Transform3f& tf) -{ +std::vector getBoundVertices(const Sphere& sphere, + const Transform3f& tf) { std::vector result(12); const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); @@ -92,14 +87,14 @@ std::vector getBoundVertices(const Sphere& sphere, const Transform3f& tf) } // we use scaled icosahedron to bound the ellipsoid -std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf) -{ +std::vector getBoundVertices(const Ellipsoid& ellipsoid, + const Transform3f& tf) { std::vector result(12); const FCL_REAL phi = (1 + sqrt(5.0)) / 2.0; const FCL_REAL a = sqrt(3.0) / (phi * phi); const FCL_REAL b = phi * a; - + const FCL_REAL& A = ellipsoid.radii[0]; const FCL_REAL& B = ellipsoid.radii[1]; const FCL_REAL& C = ellipsoid.radii[2]; @@ -126,8 +121,8 @@ std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3 return result; } -std::vector getBoundVertices(const Capsule& capsule, const Transform3f& tf) -{ +std::vector getBoundVertices(const Capsule& capsule, + const Transform3f& tf) { std::vector result(36); const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; @@ -137,7 +132,6 @@ std::vector getBoundVertices(const Capsule& capsule, const Transform3f& t FCL_REAL b = m * edge_size; FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0); - result[0] = tf.transform(Vec3f(0, a, b + hl)); result[1] = tf.transform(Vec3f(0, -a, b + hl)); result[2] = tf.transform(Vec3f(0, a, -b + hl)); @@ -183,9 +177,7 @@ std::vector getBoundVertices(const Capsule& capsule, const Transform3f& t return result; } - -std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) -{ +std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) { std::vector result(7); FCL_REAL hl = cone.halfLength; @@ -205,8 +197,8 @@ std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) return result; } -std::vector getBoundVertices(const Cylinder& cylinder, const Transform3f& tf) -{ +std::vector getBoundVertices(const Cylinder& cylinder, + const Transform3f& tf) { std::vector result(12); FCL_REAL hl = cylinder.halfLength; @@ -231,19 +223,18 @@ std::vector getBoundVertices(const Cylinder& cylinder, const Transform3f& return result; } -std::vector getBoundVertices(const ConvexBase& convex, const Transform3f& tf) -{ +std::vector getBoundVertices(const ConvexBase& convex, + const Transform3f& tf) { std::vector result(convex.num_points); - for(std::size_t i = 0; i < convex.num_points; ++i) - { + for (std::size_t i = 0; i < convex.num_points; ++i) { result[i] = tf.transform(convex.points[i]); } return result; } -std::vector getBoundVertices(const TriangleP& triangle, const Transform3f& tf) -{ +std::vector getBoundVertices(const TriangleP& triangle, + const Transform3f& tf) { std::vector result(3); result[0] = tf.transform(triangle.a); result[1] = tf.transform(triangle.b); @@ -252,10 +243,9 @@ std::vector getBoundVertices(const TriangleP& triangle, const Transform3f return result; } -} // end detail +} // namespace details -Halfspace transform(const Halfspace& a, const Transform3f& tf) -{ +Halfspace transform(const Halfspace& a, const Transform3f& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' @@ -268,9 +258,7 @@ Halfspace transform(const Halfspace& a, const Transform3f& tf) return Halfspace(n, d); } - -Plane transform(const Plane& a, const Transform3f& tf) -{ +Plane transform(const Plane& a, const Transform3f& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' @@ -283,22 +271,18 @@ Plane transform(const Plane& a, const Transform3f& tf) return Plane(n, d); } - - -template<> -void computeBV(const Box& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const Box& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); - Vec3f v_delta (R.cwiseAbs() * s.halfSide); + Vec3f v_delta(R.cwiseAbs() * s.halfSide); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } -template<> -void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) { const Vec3f& T = tf.getTranslation(); Vec3f v_delta(Vec3f::Constant(s.radius)); @@ -306,9 +290,9 @@ void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) bv.min_ = T - v_delta; } -template<> -void computeBV(const Ellipsoid& e, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const Ellipsoid& e, const Transform3f& tf, + AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -317,9 +301,9 @@ void computeBV(const Ellipsoid& e, const Transform3f& tf, AABB& bv.min_ = T - v_delta; } -template<> -void computeBV(const Capsule& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const Capsule& s, const Transform3f& tf, + AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -328,45 +312,49 @@ void computeBV(const Capsule& s, const Transform3f& tf, AABB& bv) bv.min_ = T - v_delta; } -template<> -void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); - FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength); - FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength); - FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength); + FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + + fabs(R(0, 2) * s.halfLength); + FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + + fabs(R(1, 2) * s.halfLength); + FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + + fabs(R(2, 2) * s.halfLength); Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } -template<> -void computeBV(const Cylinder& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const Cylinder& s, const Transform3f& tf, + AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); - FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength); - FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength); - FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength); + FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + + fabs(R(0, 2) * s.halfLength); + FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + + fabs(R(1, 2) * s.halfLength); + FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + + fabs(R(2, 2) * s.halfLength); Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } -template<> -void computeBV(const ConvexBase& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const ConvexBase& s, const Transform3f& tf, + AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); AABB bv_; - for(std::size_t i = 0; i < s.num_points; ++i) - { + for (std::size_t i = 0; i < s.num_points; ++i) { Vec3f new_p = R * s.points[i] + T; bv_ += new_p; } @@ -374,16 +362,15 @@ void computeBV(const ConvexBase& s, const Transform3f& tf, AAB bv = bv_; } -template<> -void computeBV(const TriangleP& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const TriangleP& s, const Transform3f& tf, + AABB& bv) { bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c)); } - -template<> -void computeBV(const Halfspace& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const Halfspace& s, const Transform3f& tf, + AABB& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; @@ -391,31 +378,31 @@ void computeBV(const Halfspace& s, const Transform3f& tf, AABB& AABB bv_; bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { + if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { // normal aligned with x axis - if(n[0] < 0) bv_.min_[0] = -d; - else if(n[0] > 0) bv_.max_[0] = d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { + if (n[0] < 0) + bv_.min_[0] = -d; + else if (n[0] > 0) + bv_.max_[0] = d; + } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { // normal aligned with y axis - if(n[1] < 0) bv_.min_[1] = -d; - else if(n[1] > 0) bv_.max_[1] = d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { + if (n[1] < 0) + bv_.min_[1] = -d; + else if (n[1] > 0) + bv_.max_[1] = d; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { // normal aligned with z axis - if(n[2] < 0) bv_.min_[2] = -d; - else if(n[2] > 0) bv_.max_[2] = d; + if (n[2] < 0) + bv_.min_[2] = -d; + else if (n[2] > 0) + bv_.max_[2] = d; } bv = bv_; } -template<> -void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) -{ +template <> +void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; @@ -423,32 +410,34 @@ void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) AABB bv_; bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { + if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { // normal aligned with x axis - if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } - else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { + if (n[0] < 0) { + bv_.min_[0] = bv_.max_[0] = -d; + } else if (n[0] > 0) { + bv_.min_[0] = bv_.max_[0] = d; + } + } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { // normal aligned with y axis - if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } - else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { + if (n[1] < 0) { + bv_.min_[1] = bv_.max_[1] = -d; + } else if (n[1] > 0) { + bv_.min_[1] = bv_.max_[1] = d; + } + } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { // normal aligned with z axis - if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; } - else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; } + if (n[2] < 0) { + bv_.min_[2] = bv_.max_[2] = -d; + } else if (n[2] > 0) { + bv_.min_[2] = bv_.max_[2] = d; + } } bv = bv_; } - -template<> -void computeBV(const Box& s, const Transform3f& tf, OBB& bv) -{ +template <> +void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -457,9 +446,8 @@ void computeBV(const Box& s, const Transform3f& tf, OBB& bv) bv.extent = s.halfSide; } -template<> -void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) -{ +template <> +void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { const Vec3f& T = tf.getTranslation(); bv.To.noalias() = T; @@ -467,9 +455,8 @@ void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) bv.extent.setConstant(s.radius); } -template<> -void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) -{ +template <> +void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -478,9 +465,8 @@ void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) bv.extent << s.radius, s.radius, s.halfLength + s.radius; } -template<> -void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) -{ +template <> +void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -489,9 +475,9 @@ void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) bv.extent << s.radius, s.radius, s.halfLength; } -template<> -void computeBV(const Cylinder& s, const Transform3f& tf, OBB& bv) -{ +template <> +void computeBV(const Cylinder& s, const Transform3f& tf, + OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -500,9 +486,9 @@ void computeBV(const Cylinder& s, const Transform3f& tf, OBB& bv) bv.extent << s.radius, s.radius, s.halfLength; } -template<> -void computeBV(const ConvexBase& s, const Transform3f& tf, OBB& bv) -{ +template <> +void computeBV(const ConvexBase& s, const Transform3f& tf, + OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -513,250 +499,250 @@ void computeBV(const ConvexBase& s, const Transform3f& tf, OBB& bv.To = R * bv.To + T; } -template<> -void computeBV(const Halfspace&, const Transform3f&, OBB& bv) -{ +template <> +void computeBV(const Halfspace&, const Transform3f&, OBB& bv) { /// Half space can only have very rough OBB bv.axes.setIdentity(); bv.To.setZero(); bv.extent.setConstant(((std::numeric_limits::max)())); } -template<> -void computeBV(const Halfspace&, const Transform3f&, RSS& bv) -{ +template <> +void computeBV(const Halfspace&, const Transform3f&, RSS& bv) { /// Half space can only have very rough RSS bv.axes.setIdentity(); bv.Tr.setZero(); - bv.length[0] = bv.length[1] = bv.radius = (std::numeric_limits::max)(); + bv.length[0] = bv.length[1] = bv.radius = + (std::numeric_limits::max)(); } -template<> -void computeBV(const Halfspace& s, const Transform3f& tf, OBBRSS& bv) -{ +template <> +void computeBV(const Halfspace& s, const Transform3f& tf, + OBBRSS& bv) { computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); } -template<> -void computeBV(const Halfspace& s, const Transform3f& tf, kIOS& bv) -{ +template <> +void computeBV(const Halfspace& s, const Transform3f& tf, + kIOS& bv) { bv.num_spheres = 1; computeBV(s, tf, bv.obb); bv.spheres[0].o = Vec3f(); bv.spheres[0].r = (std::numeric_limits::max)(); } -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv) -{ +template <> +void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, + KDOP<16>& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const short D = 8; - for(short i = 0; i < D; ++i) + for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); - for(short i = D; i < 2 * D; ++i) + for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } -} - -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv) -{ + if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D) = d; + else + bv.dist(0) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(D + 1) = d; + else + bv.dist(1) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + if (n[2] > 0) + bv.dist(D + 2) = d; + else + bv.dist(2) = -d; + } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + if (n[0] > 0) + bv.dist(D + 3) = n[0] * d * 2; + else + bv.dist(3) = n[0] * d * 2; + } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + if (n[1] > 0) + bv.dist(D + 4) = n[0] * d * 2; + else + bv.dist(4) = n[0] * d * 2; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + if (n[1] > 0) + bv.dist(D + 5) = n[1] * d * 2; + else + bv.dist(5) = n[1] * d * 2; + } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D + 6) = n[0] * d * 2; + else + bv.dist(6) = n[0] * d * 2; + } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D + 7) = n[0] * d * 2; + else + bv.dist(7) = n[0] * d * 2; + } +} + +template <> +void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, + KDOP<18>& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const short D = 9; - for(short i = 0; i < D; ++i) + for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); - for(short i = D; i < 2 * D; ++i) + for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; - else bv.dist(8) = n[1] * d * 2; - } -} - -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv) -{ + if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D) = d; + else + bv.dist(0) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(D + 1) = d; + else + bv.dist(1) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + if (n[2] > 0) + bv.dist(D + 2) = d; + else + bv.dist(2) = -d; + } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + if (n[0] > 0) + bv.dist(D + 3) = n[0] * d * 2; + else + bv.dist(3) = n[0] * d * 2; + } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + if (n[1] > 0) + bv.dist(D + 4) = n[0] * d * 2; + else + bv.dist(4) = n[0] * d * 2; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + if (n[1] > 0) + bv.dist(D + 5) = n[1] * d * 2; + else + bv.dist(5) = n[1] * d * 2; + } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D + 6) = n[0] * d * 2; + else + bv.dist(6) = n[0] * d * 2; + } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D + 7) = n[0] * d * 2; + else + bv.dist(7) = n[0] * d * 2; + } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(D + 8) = n[1] * d * 2; + else + bv.dist(8) = n[1] * d * 2; + } +} + +template <> +void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, + KDOP<24>& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const short D = 12; - for(short i = 0; i < D; ++i) + for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); - for(short i = D; i < 2 * D; ++i) + for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; - else bv.dist(8) = n[1] * d * 2; - } - else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; - else bv.dist(9) = n[0] * d * 3; - } - else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; - else bv.dist(10) = n[0] * d * 3; - } - else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; - else bv.dist(11) = n[1] * d * 3; - } -} - - - -template<> -void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) -{ + if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D) = d; + else + bv.dist(0) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(D + 1) = d; + else + bv.dist(1) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + if (n[2] > 0) + bv.dist(D + 2) = d; + else + bv.dist(2) = -d; + } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + if (n[0] > 0) + bv.dist(D + 3) = n[0] * d * 2; + else + bv.dist(3) = n[0] * d * 2; + } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + if (n[1] > 0) + bv.dist(D + 4) = n[0] * d * 2; + else + bv.dist(4) = n[0] * d * 2; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + if (n[1] > 0) + bv.dist(D + 5) = n[1] * d * 2; + else + bv.dist(5) = n[1] * d * 2; + } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D + 6) = n[0] * d * 2; + else + bv.dist(6) = n[0] * d * 2; + } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D + 7) = n[0] * d * 2; + else + bv.dist(7) = n[0] * d * 2; + } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(D + 8) = n[1] * d * 2; + else + bv.dist(8) = n[1] * d * 2; + } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D + 9) = n[0] * d * 3; + else + bv.dist(9) = n[0] * d * 3; + } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(D + 10) = n[0] * d * 3; + else + bv.dist(10) = n[0] * d * 3; + } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(D + 11) = n[1] * d * 3; + else + bv.dist(11) = n[1] * d * 3; + } +} + +template <> +void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { Vec3f n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; - bv.extent << 0, (std::numeric_limits::max)(), (std::numeric_limits::max)(); + bv.extent << 0, (std::numeric_limits::max)(), + (std::numeric_limits::max)(); Vec3f p = s.n * s.d; - bv.To = tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T + bv.To = + tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T } -template<> -void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) -{ +template <> +void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { Vec3f n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); @@ -771,296 +757,245 @@ void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) bv.Tr = tf.transform(p); } -template<> -void computeBV(const Plane& s, const Transform3f& tf, OBBRSS& bv) -{ +template <> +void computeBV(const Plane& s, const Transform3f& tf, + OBBRSS& bv) { computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); } -template<> -void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) -{ +template <> +void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) { bv.num_spheres = 1; computeBV(s, tf, bv.obb); bv.spheres[0].o = Vec3f(); bv.spheres[0].r = (std::numeric_limits::max)(); } -template<> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv) -{ +template <> +void computeBV, Plane>(const Plane& s, const Transform3f& tf, + KDOP<16>& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const short D = 8; - for(short i = 0; i < D; ++i) + for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); - for(short i = D; i < 2 * D; ++i) + for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { + if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(0) = bv.dist(D) = d; + else + bv.dist(0) = bv.dist(D) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(1) = bv.dist(D + 1) = d; + else + bv.dist(1) = bv.dist(D + 1) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + if (n[2] > 0) + bv.dist(2) = bv.dist(D + 2) = d; + else + bv.dist(2) = bv.dist(D + 2) = -d; + } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { + } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { + } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { + } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { + } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } } -template<> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv) -{ +template <> +void computeBV, Plane>(const Plane& s, const Transform3f& tf, + KDOP<18>& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const short D = 9; - for(short i = 0; i < D; ++i) + for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); - for(short i = D; i < 2 * D; ++i) + for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { + if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(0) = bv.dist(D) = d; + else + bv.dist(0) = bv.dist(D) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(1) = bv.dist(D + 1) = d; + else + bv.dist(1) = bv.dist(D + 1) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + if (n[2] > 0) + bv.dist(2) = bv.dist(D + 2) = d; + else + bv.dist(2) = bv.dist(D + 2) = -d; + } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { + } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { + } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { + } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { + } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { + } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; } } -template<> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv) -{ +template <> +void computeBV, Plane>(const Plane& s, const Transform3f& tf, + KDOP<24>& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const short D = 12; - for(short i = 0; i < D; ++i) + for (short i = 0; i < D; ++i) bv.dist(i) = -(std::numeric_limits::max)(); - for(short i = D; i < 2 * D; ++i) + for (short i = D; i < 2 * D; ++i) bv.dist(i) = (std::numeric_limits::max)(); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { + if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[0] > 0) + bv.dist(0) = bv.dist(D) = d; + else + bv.dist(0) = bv.dist(D) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] > 0) + bv.dist(1) = bv.dist(D + 1) = d; + else + bv.dist(1) = bv.dist(D + 1) = -d; + } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + if (n[2] > 0) + bv.dist(2) = bv.dist(D + 2) = d; + else + bv.dist(2) = bv.dist(D + 2) = -d; + } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { + } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { + } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { + } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { + } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { + } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; - } - else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { + } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; - } - else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { + } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; - } - else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { + } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; } } - -void constructBox(const AABB& bv, Box& box, Transform3f& tf) -{ +void constructBox(const AABB& bv, Box& box, Transform3f& tf) { box = Box(bv.max_ - bv.min_); tf = Transform3f(bv.center()); } -void constructBox(const OBB& bv, Box& box, Transform3f& tf) -{ +void constructBox(const OBB& bv, Box& box, Transform3f& tf) { box = Box(bv.extent * 2); tf = Transform3f(bv.axes, bv.To); } -void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) -{ +void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) { box = Box(bv.obb.extent * 2); tf = Transform3f(bv.obb.axes, bv.obb.To); } -void constructBox(const kIOS& bv, Box& box, Transform3f& tf) -{ +void constructBox(const kIOS& bv, Box& box, Transform3f& tf) { box = Box(bv.obb.extent * 2); tf = Transform3f(bv.obb.axes, bv.obb.To); } -void constructBox(const RSS& bv, Box& box, Transform3f& tf) -{ +void constructBox(const RSS& bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3f(bv.axes, bv.Tr); } -void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) -{ +void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3f(bv.center()); } -void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) -{ +void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3f(bv.center()); } -void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) -{ +void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3f(bv.center()); } - - -void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) -{ +void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf) { box = Box(bv.max_ - bv.min_); tf = tf_bv * Transform3f(bv.center()); } -void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) -{ +void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf) { box = Box(bv.extent * 2); tf = tf_bv * Transform3f(bv.axes, bv.To); } -void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) -{ +void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf) { box = Box(bv.obb.extent * 2); tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To); } -void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) -{ +void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf) { box = Box(bv.obb.extent * 2); tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To); } -void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) -{ +void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3f(bv.axes, bv.Tr); } -void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) -{ +void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3f(bv.center()); } -void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) -{ +void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3f(bv.center()); } -void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) -{ +void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3f(bv.center()); } +} // namespace fcl - -} - -} // namespace hpp +} // namespace hpp diff --git a/src/traits_traversal.h b/src/traits_traversal.h index f18a5d883..a8d20c68b 100644 --- a/src/traits_traversal.h +++ b/src/traits_traversal.h @@ -13,47 +13,38 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { // TraversalTraitsCollision for collision_func_matrix.cpp template -struct HPP_FCL_LOCAL TraversalTraitsCollision -{ -}; +struct HPP_FCL_LOCAL TraversalTraitsCollision {}; #ifdef HPP_FCL_HAS_OCTOMAP template -struct HPP_FCL_LOCAL TraversalTraitsCollision -{ +struct HPP_FCL_LOCAL TraversalTraitsCollision { typedef ShapeOcTreeCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision -{ +struct HPP_FCL_LOCAL TraversalTraitsCollision { typedef OcTreeShapeCollisionTraversalNode CollisionTraversal_t; }; template <> -struct HPP_FCL_LOCAL TraversalTraitsCollision -{ +struct HPP_FCL_LOCAL TraversalTraitsCollision { typedef OcTreeCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision > -{ +struct HPP_FCL_LOCAL TraversalTraitsCollision > { typedef OcTreeMeshCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision , OcTree> -{ +struct HPP_FCL_LOCAL TraversalTraitsCollision, OcTree> { typedef MeshOcTreeCollisionTraversalNode CollisionTraversal_t; }; @@ -62,45 +53,37 @@ struct HPP_FCL_LOCAL TraversalTraitsCollision , OcTree> // TraversalTraitsDistance for distance_func_matrix.cpp template -struct HPP_FCL_LOCAL TraversalTraitsDistance -{ -}; +struct HPP_FCL_LOCAL TraversalTraitsDistance {}; #ifdef HPP_FCL_HAS_OCTOMAP template -struct HPP_FCL_LOCAL TraversalTraitsDistance -{ +struct HPP_FCL_LOCAL TraversalTraitsDistance { typedef ShapeOcTreeDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance -{ +struct HPP_FCL_LOCAL TraversalTraitsDistance { typedef OcTreeShapeDistanceTraversalNode CollisionTraversal_t; }; template <> -struct HPP_FCL_LOCAL TraversalTraitsDistance -{ +struct HPP_FCL_LOCAL TraversalTraitsDistance { typedef OcTreeDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance > -{ +struct HPP_FCL_LOCAL TraversalTraitsDistance > { typedef OcTreeMeshDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance , OcTree> -{ +struct HPP_FCL_LOCAL TraversalTraitsDistance, OcTree> { typedef MeshOcTreeDistanceTraversalNode CollisionTraversal_t; }; #endif -} - -} //hpp +} // namespace fcl +} // namespace hpp diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp index 8850134fc..fc3020e3d 100644 --- a/src/traversal/traversal_recurse.cpp +++ b/src/traversal/traversal_recurse.cpp @@ -35,79 +35,71 @@ /** \author Jia Pan */ - #include #include -namespace hpp -{ -namespace fcl -{ -void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, unsigned int b2, - BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound) -{ +namespace hpp { +namespace fcl { +void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list, + FCL_REAL& sqrDistLowerBound) { FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0; bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); - if(l1 && l2) - { + if (l1 && l2) { updateFrontList(front_list, b1, b2); - // if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return; + // if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return; node->leafCollides(b1, b2, sqrDistLowerBound); return; } - if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) { + if (node->BVDisjoints(b1, b2, sqrDistLowerBound)) { updateFrontList(front_list, b1, b2); return; } - if(node->firstOverSecond(b1, b2)) - { + if (node->firstOverSecond(b1, b2)) { unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1); unsigned int c2 = (unsigned int)node->getFirstRightChild(b1); collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1); // early stop is disabled is front_list is used - if(node->canStop() && !front_list) return; + if (node->canStop() && !front_list) return; collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2); - sqrDistLowerBound = std::min (sqrDistLowerBound1, sqrDistLowerBound2); - } - else - { + sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2); + } else { unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2); unsigned int c2 = (unsigned int)node->getSecondRightChild(b2); collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1); // early stop is disabled is front_list is used - if(node->canStop() && !front_list) return; + if (node->canStop() && !front_list) return; collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2); - sqrDistLowerBound = std::min (sqrDistLowerBound1, sqrDistLowerBound2); + sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2); } } void collisionNonRecurse(CollisionTraversalNodeBase* node, - BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound) -{ + BVHFrontList* front_list, + FCL_REAL& sqrDistLowerBound) { typedef std::pair BVPair_t; - //typedef std::stack > Stack_t; + // typedef std::stack > Stack_t; typedef std::vector Stack_t; Stack_t pairs; - pairs.reserve (1000); + pairs.reserve(1000); sqrDistLowerBound = std::numeric_limits::infinity(); FCL_REAL sdlb = std::numeric_limits::infinity(); - pairs.push_back (BVPair_t (0, 0)); + pairs.push_back(BVPair_t(0, 0)); while (!pairs.empty()) { - unsigned int a = pairs.back().first, - b = pairs.back().second; + unsigned int a = pairs.back().first, b = pairs.back().second; pairs.pop_back(); bool la = node->isFirstNodeLeaf(a); @@ -118,9 +110,9 @@ void collisionNonRecurse(CollisionTraversalNodeBase* node, updateFrontList(front_list, a, b); // TODO should we test the BVs ? - //if(node->BVDijsoints(a, b, sdlb)) { - //if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb; - //continue; + // if(node->BVDijsoints(a, b, sdlb)) { + // if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb; + // continue; //} node->leafCollides(a, b, sdlb); if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb; @@ -134,39 +126,36 @@ void collisionNonRecurse(CollisionTraversalNodeBase* node, // } // Check the BV - if(node->BVDisjoints(a, b, sdlb)) { + if (node->BVDisjoints(a, b, sdlb)) { if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb; updateFrontList(front_list, a, b); continue; } - if(node->firstOverSecond(a, b)) - { + if (node->firstOverSecond(a, b)) { unsigned int c1 = (unsigned int)node->getFirstLeftChild(a); unsigned int c2 = (unsigned int)node->getFirstRightChild(a); - pairs.push_back (BVPair_t (c2, b)); - pairs.push_back (BVPair_t (c1, b)); - } - else - { + pairs.push_back(BVPair_t(c2, b)); + pairs.push_back(BVPair_t(c1, b)); + } else { unsigned int c1 = (unsigned int)node->getSecondLeftChild(b); unsigned int c2 = (unsigned int)node->getSecondRightChild(b); - pairs.push_back (BVPair_t (a, c2)); - pairs.push_back (BVPair_t (a, c1)); + pairs.push_back(BVPair_t(a, c2)); + pairs.push_back(BVPair_t(a, c1)); } } } /** Recurse function for self collision - * Make sure node is set correctly so that the first and second tree are the same + * Make sure node is set correctly so that the first and second tree are the + * same */ -void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list) -{ +void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); - if(l1 && l2) - { + if (l1 && l2) { updateFrontList(front_list, b1, b2); node->leafComputeDistance(b1, b2); @@ -175,15 +164,12 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned unsigned int a1, a2, c1, c2; - if(node->firstOverSecond(b1, b2)) - { + if (node->firstOverSecond(b1, b2)) { a1 = (unsigned int)node->getFirstLeftChild(b1); a2 = b2; c1 = (unsigned int)node->getFirstRightChild(b1); c2 = b2; - } - else - { + } else { a1 = b1; a2 = (unsigned int)node->getSecondLeftChild(b2); c1 = b1; @@ -193,36 +179,31 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned FCL_REAL d1 = node->BVDistanceLowerBound(a1, a2); FCL_REAL d2 = node->BVDistanceLowerBound(c1, c2); - if(d2 < d1) - { - if(!node->canStop(d2)) + if (d2 < d1) { + if (!node->canStop(d2)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); - if(!node->canStop(d1)) + if (!node->canStop(d1)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); - } - else - { - if(!node->canStop(d1)) + } else { + if (!node->canStop(d1)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); - if(!node->canStop(d2)) + if (!node->canStop(d2)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); } } - /** @brief Bounding volume test structure */ -struct HPP_FCL_LOCAL BVT -{ +struct HPP_FCL_LOCAL BVT { /** @brief distance between bvs */ FCL_REAL d; @@ -231,47 +212,26 @@ struct HPP_FCL_LOCAL BVT }; /** @brief Comparer between two BVT */ -struct HPP_FCL_LOCAL BVT_Comparer -{ - bool operator() (const BVT& lhs, const BVT& rhs) const - { +struct HPP_FCL_LOCAL BVT_Comparer { + bool operator()(const BVT& lhs, const BVT& rhs) const { return lhs.d > rhs.d; } }; -struct HPP_FCL_LOCAL BVTQ -{ +struct HPP_FCL_LOCAL BVTQ { BVTQ() : qsize(2) {} - bool empty() const - { - return pq.empty(); - } + bool empty() const { return pq.empty(); } - size_t size() const - { - return pq.size(); - } + size_t size() const { return pq.size(); } - const BVT& top() const - { - return pq.top(); - } + const BVT& top() const { return pq.top(); } - void push(const BVT& x) - { - pq.push(x); - } + void push(const BVT& x) { pq.push(x); } - void pop() - { - pq.pop(); - } + void pop() { pq.pop(); } - bool full() const - { - return (pq.size() + 1 >= qsize); - } + bool full() const { return (pq.size() + 1 >= qsize); } std::priority_queue, BVT_Comparer> pq; @@ -279,9 +239,9 @@ struct HPP_FCL_LOCAL BVTQ unsigned int qsize; }; - -void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, unsigned int qsize) -{ +void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, + unsigned int b2, BVHFrontList* front_list, + unsigned int qsize) { BVTQ bvtq; bvtq.qsize = qsize; @@ -289,30 +249,23 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsi min_test.b1 = b1; min_test.b2 = b2; - while(1) - { + while (1) { bool l1 = node->isFirstNodeLeaf(min_test.b1); bool l2 = node->isSecondNodeLeaf(min_test.b2); - if(l1 && l2) - { + if (l1 && l2) { updateFrontList(front_list, min_test.b1, min_test.b2); node->leafComputeDistance(min_test.b1, min_test.b2); - } - else if(bvtq.full()) - { + } else if (bvtq.full()) { // queue should not get two more tests, recur distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize); - } - else - { + } else { // queue capacity is not full yet BVT bvt1, bvt2; - if(node->firstOverSecond(min_test.b1, min_test.b2)) - { + if (node->firstOverSecond(min_test.b1, min_test.b2)) { unsigned int c1 = (unsigned int)node->getFirstLeftChild(min_test.b1); unsigned int c2 = (unsigned int)node->getFirstRightChild(min_test.b1); bvt1.b1 = c1; @@ -322,9 +275,7 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsi bvt2.b1 = c2; bvt2.b2 = min_test.b2; bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2); - } - else - { + } else { unsigned int c1 = (unsigned int)node->getSecondLeftChild(min_test.b2); unsigned int c2 = (unsigned int)node->getSecondRightChild(min_test.b2); bvt1.b1 = min_test.b1; @@ -340,15 +291,13 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsi bvtq.push(bvt2); } - if(bvtq.empty()) + if (bvtq.empty()) break; - else - { + else { min_test = bvtq.top(); bvtq.pop(); - if(node->canStop(min_test.d)) - { + if (node->canStop(min_test.d)) { updateFrontList(front_list, min_test.b1, min_test.b2); break; } @@ -356,69 +305,61 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1, unsi } } -void propagateBVHFrontListCollisionRecurse -(CollisionTraversalNodeBase* node, const CollisionRequest& /*request*/, - CollisionResult& result, BVHFrontList* front_list) -{ - FCL_REAL sqrDistLowerBound = -1, - sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0; +void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, + const CollisionRequest& /*request*/, + CollisionResult& result, + BVHFrontList* front_list) { + FCL_REAL sqrDistLowerBound = -1, sqrDistLowerBound1 = 0, + sqrDistLowerBound2 = 0; BVHFrontList::iterator front_iter; BVHFrontList append; - for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter) - { + for (front_iter = front_list->begin(); front_iter != front_list->end(); + ++front_iter) { unsigned int b1 = front_iter->left; unsigned int b2 = front_iter->right; bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); - if(l1 & l2) - { - front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again. + if (l1 & l2) { + front_iter->valid = false; // the front node is no longer valid, in + // collideRecurse will add again. collisionRecurse(node, b1, b2, &append, sqrDistLowerBound); - } - else - { - if(!node->BVDisjoints(b1, b2, sqrDistLowerBound)) { + } else { + if (!node->BVDisjoints(b1, b2, sqrDistLowerBound)) { front_iter->valid = false; - if(node->firstOverSecond(b1, b2)) { + if (node->firstOverSecond(b1, b2)) { unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1); unsigned int c2 = (unsigned int)node->getFirstRightChild(b1); collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1); collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2); - sqrDistLowerBound = std::min (sqrDistLowerBound1, - sqrDistLowerBound2); + sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2); } else { unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2); unsigned int c2 = (unsigned int)node->getSecondRightChild(b2); collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1); collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2); - sqrDistLowerBound = std::min (sqrDistLowerBound1, - sqrDistLowerBound2); + sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2); } } } - result.updateDistanceLowerBound (sqrt (sqrDistLowerBound)); + result.updateDistanceLowerBound(sqrt(sqrDistLowerBound)); } - // clean the old front list (remove invalid node) - for(front_iter = front_list->begin(); front_iter != front_list->end();) - { - if(!front_iter->valid) + for (front_iter = front_list->begin(); front_iter != front_list->end();) { + if (!front_iter->valid) front_iter = front_list->erase(front_iter); else ++front_iter; } - for(front_iter = append.begin(); front_iter != append.end(); ++front_iter) - { + for (front_iter = append.begin(); front_iter != append.end(); ++front_iter) { front_list->push_back(*front_iter); } } +} // namespace fcl -} - -} // namespace hpp +} // namespace hpp diff --git a/test/benchmark.cpp b/test/benchmark.cpp index 6a9f028c3..54a22e5b3 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -24,7 +24,7 @@ #include "utility.h" #include "fcl_resources/config.h" -#define RUN_CASE(BV,tf,models,split) \ +#define RUN_CASE(BV, tf, models, split) \ run(tf, models, split, #BV " - " #split ":\t") using namespace hpp::fcl; @@ -32,52 +32,55 @@ using namespace hpp::fcl; bool verbose = false; FCL_REAL DELTA = 0.001; -template -void makeModel (const std::vector& vertices, const std::vector& triangles, - SplitMethodType split_method, BVHModel& model); +template +void makeModel(const std::vector& vertices, + const std::vector& triangles, + SplitMethodType split_method, BVHModel& model); -template -double distance (const std::vector& tf, - const BVHModel& m1, const BVHModel& m2, - bool verbose); +template +double distance(const std::vector& tf, const BVHModel& m1, + const BVHModel& m2, bool verbose); -template -double collide (const std::vector& tf, - const BVHModel& m1, const BVHModel& m2, - bool verbose); +template +double collide(const std::vector& tf, const BVHModel& m1, + const BVHModel& m2, bool verbose); -template -double run (const std::vector& tf, - const BVHModel (&models)[2][3], int split_method, - const char* sm_name); +template +double run(const std::vector& tf, + const BVHModel (&models)[2][3], int split_method, + const char* sm_name); -template struct traits { -}; +template +struct traits {}; -template <> struct traits { +template <> +struct traits { typedef MeshCollisionTraversalNodeRSS CollisionTraversalNode; - typedef MeshDistanceTraversalNodeRSS DistanceTraversalNode; + typedef MeshDistanceTraversalNodeRSS DistanceTraversalNode; }; -template <> struct traits { +template <> +struct traits { typedef MeshCollisionTraversalNodekIOS CollisionTraversalNode; - typedef MeshDistanceTraversalNodekIOS DistanceTraversalNode; + typedef MeshDistanceTraversalNodekIOS DistanceTraversalNode; }; -template <> struct traits { +template <> +struct traits { typedef MeshCollisionTraversalNodeOBB CollisionTraversalNode; // typedef MeshDistanceTraversalNodeOBB DistanceTraversalNode; }; -template <> struct traits { +template <> +struct traits { typedef MeshCollisionTraversalNodeOBBRSS CollisionTraversalNode; - typedef MeshDistanceTraversalNodeOBBRSS DistanceTraversalNode; + typedef MeshDistanceTraversalNodeOBBRSS DistanceTraversalNode; }; -template -void makeModel (const std::vector& vertices, const std::vector& triangles, - SplitMethodType split_method, BVHModel& model) -{ +template +void makeModel(const std::vector& vertices, + const std::vector& triangles, + SplitMethodType split_method, BVHModel& model) { model.bv_splitter.reset(new BVSplitter(split_method)); model.bv_splitter.reset(new BVSplitter(split_method)); @@ -86,11 +89,9 @@ void makeModel (const std::vector& vertices, const std::vector& model.endModel(); } -template -double distance (const std::vector& tf, - const BVHModel& m1, const BVHModel& m2, - bool verbose) -{ +template +double distance(const std::vector& tf, const BVHModel& m1, + const BVHModel& m2, bool verbose) { Transform3f pose2; DistanceResult local_result; @@ -103,7 +104,7 @@ double distance (const std::vector& tf, timer.start(); for (std::size_t i = 0; i < tf.size(); ++i) { - if(!initialize(node, m1, tf[i], m2, pose2, request, local_result)) + if (!initialize(node, m1, tf[i], m2, pose2, request, local_result)) std::cout << "initialize error" << std::endl; distance(&node, NULL); @@ -112,16 +113,14 @@ double distance (const std::vector& tf, return timer.getElapsedTimeInMicroSec(); } -template -double collide (const std::vector& tf, - const BVHModel& m1, const BVHModel& m2, - bool verbose) -{ +template +double collide(const std::vector& tf, const BVHModel& m1, + const BVHModel& m2, bool verbose) { Transform3f pose2; - CollisionResult local_result; + CollisionResult local_result; CollisionRequest request; - TraversalNode node (request); + TraversalNode node(request); node.enable_statistics = verbose; @@ -129,9 +128,9 @@ double collide (const std::vector& tf, timer.start(); for (std::size_t i = 0; i < tf.size(); ++i) { - bool success (initialize(node, m1, tf[i], m2, pose2, local_result)); + bool success(initialize(node, m1, tf[i], m2, pose2, local_result)); (void)success; - assert (success); + assert(success); CollisionResult result; collide(&node, request, result); @@ -141,36 +140,32 @@ double collide (const std::vector& tf, return timer.getElapsedTimeInMicroSec(); } -template -double run (const std::vector& tf, - const BVHModel (&models)[2][3], int split_method, - const char* prefix) -{ - double col = collide ::CollisionTraversalNode> - (tf, models[0][split_method], models[1][split_method], verbose); - double dist = distance::DistanceTraversalNode> - (tf, models[0][split_method], models[1][split_method], verbose); +template +double run(const std::vector& tf, + const BVHModel (&models)[2][3], int split_method, + const char* prefix) { + double col = collide::CollisionTraversalNode>( + tf, models[0][split_method], models[1][split_method], verbose); + double dist = distance::DistanceTraversalNode>( + tf, models[0][split_method], models[1][split_method], verbose); std::cout << prefix << " (" << col << ", " << dist << ")\n"; return col + dist; } -template<> -double run (const std::vector& tf, - const BVHModel (&models)[2][3], int split_method, - const char* prefix) -{ - double col = collide ::CollisionTraversalNode> - (tf, models[0][split_method], models[1][split_method], verbose); +template <> +double run(const std::vector& tf, + const BVHModel (&models)[2][3], int split_method, + const char* prefix) { + double col = collide::CollisionTraversalNode>( + tf, models[0][split_method], models[1][split_method], verbose); double dist = 0; std::cout << prefix << " (\t" << col << ", \tNaN)\n"; return col + dist; } - -int main (int, char*[]) -{ +int main(int, char*[]) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -179,38 +174,40 @@ int main (int, char*[]) // Make models BVHModel ms_rss[2][3]; - makeModel (p1, t1, SPLIT_METHOD_MEAN , ms_rss[0][SPLIT_METHOD_MEAN ]); - makeModel (p1, t1, SPLIT_METHOD_BV_CENTER, ms_rss[0][SPLIT_METHOD_BV_CENTER]); - makeModel (p1, t1, SPLIT_METHOD_MEDIAN , ms_rss[0][SPLIT_METHOD_MEDIAN ]); - makeModel (p2, t2, SPLIT_METHOD_MEAN , ms_rss[1][SPLIT_METHOD_MEAN ]); - makeModel (p2, t2, SPLIT_METHOD_BV_CENTER, ms_rss[1][SPLIT_METHOD_BV_CENTER]); - makeModel (p2, t2, SPLIT_METHOD_MEDIAN , ms_rss[1][SPLIT_METHOD_MEDIAN ]); + makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_rss[0][SPLIT_METHOD_MEAN]); + makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_rss[0][SPLIT_METHOD_BV_CENTER]); + makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_rss[0][SPLIT_METHOD_MEDIAN]); + makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_rss[1][SPLIT_METHOD_MEAN]); + makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_rss[1][SPLIT_METHOD_BV_CENTER]); + makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_rss[1][SPLIT_METHOD_MEDIAN]); BVHModel ms_kios[2][3]; - makeModel (p1, t1, SPLIT_METHOD_MEAN , ms_kios[0][SPLIT_METHOD_MEAN ]); - makeModel (p1, t1, SPLIT_METHOD_BV_CENTER, ms_kios[0][SPLIT_METHOD_BV_CENTER]); - makeModel (p1, t1, SPLIT_METHOD_MEDIAN , ms_kios[0][SPLIT_METHOD_MEDIAN ]); - makeModel (p2, t2, SPLIT_METHOD_MEAN , ms_kios[1][SPLIT_METHOD_MEAN ]); - makeModel (p2, t2, SPLIT_METHOD_BV_CENTER, ms_kios[1][SPLIT_METHOD_BV_CENTER]); - makeModel (p2, t2, SPLIT_METHOD_MEDIAN , ms_kios[1][SPLIT_METHOD_MEDIAN ]); + makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_kios[0][SPLIT_METHOD_MEAN]); + makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_kios[0][SPLIT_METHOD_BV_CENTER]); + makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_kios[0][SPLIT_METHOD_MEDIAN]); + makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_kios[1][SPLIT_METHOD_MEAN]); + makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_kios[1][SPLIT_METHOD_BV_CENTER]); + makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_kios[1][SPLIT_METHOD_MEDIAN]); BVHModel ms_obb[2][3]; - makeModel (p1, t1, SPLIT_METHOD_MEAN , ms_obb[0][SPLIT_METHOD_MEAN ]); - makeModel (p1, t1, SPLIT_METHOD_BV_CENTER, ms_obb[0][SPLIT_METHOD_BV_CENTER]); - makeModel (p1, t1, SPLIT_METHOD_MEDIAN , ms_obb[0][SPLIT_METHOD_MEDIAN ]); - makeModel (p2, t2, SPLIT_METHOD_MEAN , ms_obb[1][SPLIT_METHOD_MEAN ]); - makeModel (p2, t2, SPLIT_METHOD_BV_CENTER, ms_obb[1][SPLIT_METHOD_BV_CENTER]); - makeModel (p2, t2, SPLIT_METHOD_MEDIAN , ms_obb[1][SPLIT_METHOD_MEDIAN ]); + makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obb[0][SPLIT_METHOD_MEAN]); + makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_obb[0][SPLIT_METHOD_BV_CENTER]); + makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obb[0][SPLIT_METHOD_MEDIAN]); + makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obb[1][SPLIT_METHOD_MEAN]); + makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_obb[1][SPLIT_METHOD_BV_CENTER]); + makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obb[1][SPLIT_METHOD_MEDIAN]); BVHModel ms_obbrss[2][3]; - makeModel (p1, t1, SPLIT_METHOD_MEAN , ms_obbrss[0][SPLIT_METHOD_MEAN ]); - makeModel (p1, t1, SPLIT_METHOD_BV_CENTER, ms_obbrss[0][SPLIT_METHOD_BV_CENTER]); - makeModel (p1, t1, SPLIT_METHOD_MEDIAN , ms_obbrss[0][SPLIT_METHOD_MEDIAN ]); - makeModel (p2, t2, SPLIT_METHOD_MEAN , ms_obbrss[1][SPLIT_METHOD_MEAN ]); - makeModel (p2, t2, SPLIT_METHOD_BV_CENTER, ms_obbrss[1][SPLIT_METHOD_BV_CENTER]); - makeModel (p2, t2, SPLIT_METHOD_MEDIAN , ms_obbrss[1][SPLIT_METHOD_MEDIAN ]); - - std::vector transforms; // t0 + makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obbrss[0][SPLIT_METHOD_MEAN]); + makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, + ms_obbrss[0][SPLIT_METHOD_BV_CENTER]); + makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obbrss[0][SPLIT_METHOD_MEDIAN]); + makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obbrss[1][SPLIT_METHOD_MEAN]); + makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, + ms_obbrss[1][SPLIT_METHOD_BV_CENTER]); + makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]); + + std::vector transforms; // t0 FCL_REAL extents[] = {-3000, -3000, -3000, 3000, 3000, 3000}; std::size_t n = 10000; diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp index 92fa38a13..f262e44cf 100644 --- a/test/box_box_distance.cpp +++ b/test/box_box_distance.cpp @@ -40,7 +40,7 @@ #define BOOST_TEST_MODULE FCL_BOX_BOX #include -#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps)) +#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include #include @@ -52,206 +52,202 @@ #include "utility.h" -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using hpp::fcl::CollisionGeometryPtr_t; using hpp::fcl::CollisionObject; -using hpp::fcl::DistanceResult; using hpp::fcl::DistanceRequest; -using hpp::fcl::CollisionGeometryPtr_t; +using hpp::fcl::DistanceResult; +using hpp::fcl::Transform3f; +using hpp::fcl::Vec3f; -BOOST_AUTO_TEST_CASE(distance_box_box_1) -{ - CollisionGeometryPtr_t s1 (new hpp::fcl::Box (6, 10, 2)); - CollisionGeometryPtr_t s2 (new hpp::fcl::Box (2, 2, 2)); +BOOST_AUTO_TEST_CASE(distance_box_box_1) { + CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2)); + CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2)); Transform3f tf1; - Transform3f tf2 (Vec3f(25, 20, 5)); + Transform3f tf2(Vec3f(25, 20, 5)); - CollisionObject o1 (s1, tf1); - CollisionObject o2 (s2, tf2); + CollisionObject o1(s1, tf1); + CollisionObject o2(s2, tf2); // Enable computation of nearest points - DistanceRequest distanceRequest (true, 0, 0); + DistanceRequest distanceRequest(true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance (&o1, &o2, distanceRequest, distanceResult); + hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl - << " R1 = " << tf1.getRotation () << std::endl - << " T2 = " << tf2.getTranslation() << std::endl - << " R2 = " << tf2.getRotation () << std::endl; - std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0] - << ", p2 = " << distanceResult.nearest_points [1] - << ", distance = " << distanceResult.min_distance << std::endl; + << " R1 = " << tf1.getRotation() << std::endl + << " T2 = " << tf2.getTranslation() << std::endl + << " R2 = " << tf2.getRotation() << std::endl; + std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] + << ", p2 = " << distanceResult.nearest_points[1] + << ", distance = " << distanceResult.min_distance << std::endl; double dx = 25 - 3 - 1; double dy = 20 - 5 - 1; double dz = 5 - 1 - 1; - const Vec3f& p1 = distanceResult.nearest_points [0]; - const Vec3f& p2 = distanceResult.nearest_points [1]; - BOOST_CHECK_CLOSE(distanceResult.min_distance, - sqrt (dx*dx + dy*dy + dz*dz), 1e-4); - - BOOST_CHECK_CLOSE (p1 [0], 3, 1e-6); - BOOST_CHECK_CLOSE (p1 [1], 5, 1e-6); - BOOST_CHECK_CLOSE (p1 [2], 1, 1e-6); - BOOST_CHECK_CLOSE (p2 [0], 24, 1e-6); - BOOST_CHECK_CLOSE (p2 [1], 19, 1e-6); - BOOST_CHECK_CLOSE (p2 [2], 4, 1e-6); + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; + BOOST_CHECK_CLOSE(distanceResult.min_distance, + sqrt(dx * dx + dy * dy + dz * dz), 1e-4); + + BOOST_CHECK_CLOSE(p1[0], 3, 1e-6); + BOOST_CHECK_CLOSE(p1[1], 5, 1e-6); + BOOST_CHECK_CLOSE(p1[2], 1, 1e-6); + BOOST_CHECK_CLOSE(p2[0], 24, 1e-6); + BOOST_CHECK_CLOSE(p2[1], 19, 1e-6); + BOOST_CHECK_CLOSE(p2[2], 4, 1e-6); } -BOOST_AUTO_TEST_CASE(distance_box_box_2) -{ - CollisionGeometryPtr_t s1 (new hpp::fcl::Box (6, 10, 2)); - CollisionGeometryPtr_t s2 (new hpp::fcl::Box (2, 2, 2)); +BOOST_AUTO_TEST_CASE(distance_box_box_2) { + CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2)); + CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2)); static double pi = M_PI; Transform3f tf1; - Transform3f tf2 (hpp::fcl::makeQuat (cos (pi/8), sin(pi/8)/sqrt(3), - sin(pi/8)/sqrt(3), sin(pi/8)/sqrt(3)), - Vec3f(0, 0, 10)); + Transform3f tf2( + hpp::fcl::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), + sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)), + Vec3f(0, 0, 10)); - CollisionObject o1 (s1, tf1); - CollisionObject o2 (s2, tf2); + CollisionObject o1(s1, tf1); + CollisionObject o2(s2, tf2); // Enable computation of nearest points - DistanceRequest distanceRequest (true, 0, 0); + DistanceRequest distanceRequest(true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance (&o1, &o2, distanceRequest, distanceResult); + hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl - << " R1 = " << tf1.getRotation () << std::endl - << " T2 = " << tf2.getTranslation() << std::endl - << " R2 = " << tf2.getRotation () << std::endl; - std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0] - << ", p2 = " << distanceResult.nearest_points [1] - << ", distance = " << distanceResult.min_distance << std::endl; - - const Vec3f& p1 = distanceResult.nearest_points [0]; - const Vec3f& p2 = distanceResult.nearest_points [1]; + << " R1 = " << tf1.getRotation() << std::endl + << " T2 = " << tf2.getTranslation() << std::endl + << " R2 = " << tf2.getRotation() << std::endl; + std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] + << ", p2 = " << distanceResult.nearest_points[1] + << ", distance = " << distanceResult.min_distance << std::endl; + + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; double distance = -1.62123444 + 10 - 1; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - BOOST_CHECK_CLOSE (p1 [0], 0.60947571, 1e-4); - BOOST_CHECK_CLOSE (p1 [1], 0.01175873, 1e-4); - BOOST_CHECK_CLOSE (p1 [2], 1, 1e-6); - BOOST_CHECK_CLOSE (p2 [0], 0.60947571, 1e-4); - BOOST_CHECK_CLOSE (p2 [1], 0.01175873, 1e-4); - BOOST_CHECK_CLOSE (p2 [2], -1.62123444 + 10, 1e-4); + BOOST_CHECK_CLOSE(p1[0], 0.60947571, 1e-4); + BOOST_CHECK_CLOSE(p1[1], 0.01175873, 1e-4); + BOOST_CHECK_CLOSE(p1[2], 1, 1e-6); + BOOST_CHECK_CLOSE(p2[0], 0.60947571, 1e-4); + BOOST_CHECK_CLOSE(p2[1], 0.01175873, 1e-4); + BOOST_CHECK_CLOSE(p2[2], -1.62123444 + 10, 1e-4); } -BOOST_AUTO_TEST_CASE(distance_box_box_3) -{ - CollisionGeometryPtr_t s1 (new hpp::fcl::Box (1, 1, 1)); - CollisionGeometryPtr_t s2 (new hpp::fcl::Box (1, 1, 1)); +BOOST_AUTO_TEST_CASE(distance_box_box_3) { + CollisionGeometryPtr_t s1(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t s2(new hpp::fcl::Box(1, 1, 1)); static double pi = M_PI; - Transform3f tf1 (hpp::fcl::makeQuat (cos (pi/8), 0, 0, sin (pi/8)), - Vec3f (-2, 1, .5)); - Transform3f tf2 (hpp::fcl::makeQuat (cos (pi/8), 0, sin(pi/8),0), - Vec3f (2, .5, .5)); + Transform3f tf1(hpp::fcl::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), + Vec3f(-2, 1, .5)); + Transform3f tf2(hpp::fcl::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), + Vec3f(2, .5, .5)); - CollisionObject o1 (s1, tf1); - CollisionObject o2 (s2, tf2); + CollisionObject o1(s1, tf1); + CollisionObject o2(s2, tf2); // Enable computation of nearest points - DistanceRequest distanceRequest (true, 0, 0); + DistanceRequest distanceRequest(true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance (&o1, &o2, distanceRequest, distanceResult); + hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl - << " R1 = " << tf1.getRotation () << std::endl - << " T2 = " << tf2.getTranslation() << std::endl - << " R2 = " << tf2.getRotation () << std::endl; - std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0] - << ", p2 = " << distanceResult.nearest_points [1] - << ", distance = " << distanceResult.min_distance << std::endl; - - const Vec3f& p1 = distanceResult.nearest_points [0]; - const Vec3f& p2 = distanceResult.nearest_points [1]; - double distance = 4 - sqrt (2); + << " R1 = " << tf1.getRotation() << std::endl + << " T2 = " << tf2.getTranslation() << std::endl + << " R2 = " << tf2.getRotation() << std::endl; + std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] + << ", p2 = " << distanceResult.nearest_points[1] + << ", distance = " << distanceResult.min_distance << std::endl; + + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; + double distance = 4 - sqrt(2); BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - const Vec3f p1Ref (sqrt(2)/2 - 2, 1, .5); - const Vec3f p2Ref (2 - sqrt(2)/2, 1, .5); - BOOST_CHECK_CLOSE (p1 [0], p1Ref [0], 1e-4); - BOOST_CHECK_CLOSE (p1 [1], p1Ref [1], 1e-4); - BOOST_CHECK_CLOSE (p1 [2], p1Ref [2], 1e-4); - BOOST_CHECK_CLOSE (p2 [0], p2Ref [0], 1e-4); - BOOST_CHECK_CLOSE (p2 [1], p2Ref [1], 1e-4); - BOOST_CHECK_CLOSE (p2 [2], p2Ref [2], 1e-4); + const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5); + const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5); + BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4); + BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4); + BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4); + BOOST_CHECK_CLOSE(p2[0], p2Ref[0], 1e-4); + BOOST_CHECK_CLOSE(p2[1], p2Ref[1], 1e-4); + BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4); // Apply the same global transform to both objects and recompute - Transform3f tf3 (hpp::fcl::makeQuat (0.435952844074,-0.718287018243, - 0.310622451066, 0.444435113443), - Vec3f (4, 5, 6)); - tf1 = tf3*tf1; - tf2 = tf3*tf2; - o1 = CollisionObject (s1, tf1); - o2 = CollisionObject (s2, tf2); + Transform3f tf3(hpp::fcl::makeQuat(0.435952844074, -0.718287018243, + 0.310622451066, 0.444435113443), + Vec3f(4, 5, 6)); + tf1 = tf3 * tf1; + tf2 = tf3 * tf2; + o1 = CollisionObject(s1, tf1); + o2 = CollisionObject(s2, tf2); - distanceResult.clear (); - hpp::fcl::distance (&o1, &o2, distanceRequest, distanceResult); + distanceResult.clear(); + hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl - << " R1 = " << tf1.getRotation () << std::endl - << " T2 = " << tf2.getTranslation() << std::endl - << " R2 = " << tf2.getRotation () << std::endl; - std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0] - << ", p2 = " << distanceResult.nearest_points [1] - << ", distance = " << distanceResult.min_distance << std::endl; + << " R1 = " << tf1.getRotation() << std::endl + << " T2 = " << tf2.getTranslation() << std::endl + << " R2 = " << tf2.getRotation() << std::endl; + std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] + << ", p2 = " << distanceResult.nearest_points[1] + << ", distance = " << distanceResult.min_distance << std::endl; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - const Vec3f p1Moved = tf3.transform (p1Ref); - const Vec3f p2Moved = tf3.transform (p2Ref); - BOOST_CHECK_CLOSE (p1 [0], p1Moved [0], 1e-4); - BOOST_CHECK_CLOSE (p1 [1], p1Moved [1], 1e-4); - BOOST_CHECK_CLOSE (p1 [2], p1Moved [2], 1e-4); - BOOST_CHECK_CLOSE (p2 [0], p2Moved [0], 1e-4); - BOOST_CHECK_CLOSE (p2 [1], p2Moved [1], 1e-4); - BOOST_CHECK_CLOSE (p2 [2], p2Moved [2], 1e-4); - + const Vec3f p1Moved = tf3.transform(p1Ref); + const Vec3f p2Moved = tf3.transform(p2Ref); + BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4); + BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4); + BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4); + BOOST_CHECK_CLOSE(p2[0], p2Moved[0], 1e-4); + BOOST_CHECK_CLOSE(p2[1], p2Moved[1], 1e-4); + BOOST_CHECK_CLOSE(p2[2], p2Moved[2], 1e-4); } -BOOST_AUTO_TEST_CASE(distance_box_box_4) -{ - hpp::fcl::Box s1 (1, 1, 1); - hpp::fcl::Box s2 (1, 1, 1); +BOOST_AUTO_TEST_CASE(distance_box_box_4) { + hpp::fcl::Box s1(1, 1, 1); + hpp::fcl::Box s2(1, 1, 1); // Enable computation of nearest points - DistanceRequest distanceRequest (true, 0, 0); + DistanceRequest distanceRequest(true, 0, 0); DistanceResult distanceResult; double distance; - Transform3f tf1 (Vec3f (2, 0, 0)); + Transform3f tf1(Vec3f(2, 0, 0)); Transform3f tf2; - hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 1.; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - tf1.setTranslation(Vec3f (1.01, 0, 0)); + tf1.setTranslation(Vec3f(1.01, 0, 0)); distanceResult.clear(); - hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 0.01; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); - tf1.setTranslation(Vec3f (0.99, 0, 0)); + tf1.setTranslation(Vec3f(0.99, 0, 0)); distanceResult.clear(); - hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = -0.01; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); - tf1.setTranslation(Vec3f (0, 0, 0)); + tf1.setTranslation(Vec3f(0, 0, 0)); distanceResult.clear(); - hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = -1; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); diff --git a/test/broadphase.cpp b/test/broadphase.cpp index ed14aa0ea..1eddb29b3 100644 --- a/test/broadphase.cpp +++ b/test/broadphase.cpp @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #define BOOST_TEST_MODULE FCL_BROADPHASE #include @@ -57,85 +56,98 @@ using namespace hpp::fcl; -struct TStruct -{ +struct TStruct { std::vector records; double overall_time; TStruct() { overall_time = 0; } - - void push_back(double t) - { + + void push_back(double t) { records.push_back(t); overall_time += t; } }; -/// @brief Generate environment with 3 * n objects: n boxes, n spheres and n cylinders. -void generateEnvironments(std::vector& env, double env_scale, std::size_t n); +/// @brief Generate environment with 3 * n objects: n boxes, n spheres and n +/// cylinders. +void generateEnvironments(std::vector& env, double env_scale, + std::size_t n); -/// @brief Generate environment with 3 * n objects for self distance, so we try to make sure none of them collide with each other. -void generateSelfDistanceEnvironments(std::vector& env, double env_scale, std::size_t n); +/// @brief Generate environment with 3 * n objects for self distance, so we try +/// to make sure none of them collide with each other. +void generateSelfDistanceEnvironments(std::vector& env, + double env_scale, std::size_t n); /// @brief Generate environment with 3 * n objects, but all in meshes. -void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n); +void generateEnvironmentsMesh(std::vector& env, + double env_scale, std::size_t n); -/// @brief Generate environment with 3 * n objects for self distance, but all in meshes. -void generateSelfDistanceEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n); +/// @brief Generate environment with 3 * n objects for self distance, but all in +/// meshes. +void generateSelfDistanceEnvironmentsMesh(std::vector& env, + double env_scale, std::size_t n); /// @brief test for broad phase collision and self collision -void broad_phase_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); +void broad_phase_collision_test(double env_scale, std::size_t env_size, + std::size_t query_size, + std::size_t num_max_contacts = 1, + bool exhaustive = false, bool use_mesh = false); /// @brief test for broad phase distance -void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh = false); +void broad_phase_distance_test(double env_scale, std::size_t env_size, + std::size_t query_size, bool use_mesh = false); /// @brief test for broad phase self distance -void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh = false); +void broad_phase_self_distance_test(double env_scale, std::size_t env_size, + bool use_mesh = false); /// @brief test for broad phase update -void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); +void broad_phase_update_collision_test(double env_scale, std::size_t env_size, + std::size_t query_size, + std::size_t num_max_contacts = 1, + bool exhaustive = false, + bool use_mesh = false); FCL_REAL DELTA = 0.01; - #if USE_GOOGLEHASH -template -struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; - -template -struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > -{ - GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() - { +template +struct GoogleSparseHashTable + : public google::sparse_hash_map, + std::equal_to > {}; + +template +struct GoogleDenseHashTable + : public google::dense_hash_map, + std::equal_to > { + GoogleDenseHashTable() + : google::dense_hash_map, + std::equal_to >() { this->set_empty_key(NULL); } }; #endif /// check the update, only return collision or not -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_binary) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_binary) { broad_phase_update_collision_test(2000, 100, 1000, 1, false); broad_phase_update_collision_test(2000, 1000, 1000, 1, false); } /// check the update, return 10 contacts -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision) { broad_phase_update_collision_test(2000, 100, 1000, 10, false); broad_phase_update_collision_test(2000, 1000, 1000, 10, false); } /// check the update, exhaustive -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_exhaustive) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_exhaustive) { broad_phase_update_collision_test(2000, 100, 1000, 1, true); broad_phase_update_collision_test(2000, 1000, 1000, 1, true); } /// check broad phase distance -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance) { broad_phase_distance_test(200, 100, 100); broad_phase_distance_test(200, 1000, 100); broad_phase_distance_test(2000, 100, 100); @@ -143,16 +155,14 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance) } /// check broad phase self distance -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance) { broad_phase_self_distance_test(200, 512); broad_phase_self_distance_test(200, 1000); broad_phase_self_distance_test(200, 5000); } /// check broad phase collision for empty collision object set and queries -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) -{ +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); @@ -171,8 +181,7 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) } /// check broad phase collision and self collision, only return collision or not -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_collision_test(2000, 10, 100, 1, false); broad_phase_collision_test(2000, 100, 100, 1, false); @@ -187,22 +196,20 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) } /// check broad phase collision and self collision, return 10 contacts -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) { broad_phase_collision_test(2000, 100, 1000, 10, false); broad_phase_collision_test(2000, 1000, 1000, 10, false); } /// check broad phase update, in mesh, only return collision or not -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_binary) -{ +BOOST_AUTO_TEST_CASE( + test_core_mesh_bf_broad_phase_update_collision_mesh_binary) { broad_phase_update_collision_test(2000, 100, 1000, false, true); broad_phase_update_collision_test(2000, 1000, 1000, false, true); } /// check broad phase update, in mesh, return 10 contacts -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh) -{ +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_update_collision_test(200, 10, 100, 10, false, true); broad_phase_update_collision_test(200, 100, 100, 10, false, true); @@ -213,8 +220,8 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh) } /// check broad phase update, in mesh, exhaustive -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) -{ +BOOST_AUTO_TEST_CASE( + test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_update_collision_test(2000, 10, 100, 1, true, true); broad_phase_update_collision_test(2000, 100, 100, 1, true, true); @@ -225,8 +232,7 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_exhaust } /// check broad phase distance -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh) -{ +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_distance_test(200, 10, 10, true); broad_phase_distance_test(200, 100, 10, true); @@ -241,16 +247,15 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh) } /// check broad phase self distance -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_self_distance_mesh) -{ +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_self_distance_mesh) { broad_phase_self_distance_test(200, 512, true); broad_phase_self_distance_test(200, 1000, true); broad_phase_self_distance_test(200, 5000, true); } -/// check broad phase collision and self collision, return only collision or not, in mesh -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) -{ +/// check broad phase collision and self collision, return only collision or +/// not, in mesh +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_collision_test(2000, 10, 100, 1, false, true); broad_phase_collision_test(2000, 100, 100, 1, false, true); @@ -261,8 +266,7 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) } /// check broad phase collision and self collision, return 10 contacts, in mesh -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) -{ +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_collision_test(2000, 10, 100, 10, false, true); broad_phase_collision_test(2000, 100, 100, 10, false, true); @@ -273,8 +277,7 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) } /// check broad phase collision and self collision, exhaustive, in mesh -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) -{ +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_collision_test(2000, 10, 100, 1, true, true); broad_phase_collision_test(2000, 100, 100, 1, true, true); @@ -284,139 +287,144 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) #endif } -void generateEnvironments(std::vector& env, double env_scale, std::size_t n) -{ - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; +void generateEnvironments(std::vector& env, double env_scale, + std::size_t n) { + FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, + env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { Box* box = new Box(5, 10, 20); - env.push_back(new CollisionObject(shared_ptr(box), transforms[i])); + env.push_back( + new CollisionObject(shared_ptr(box), transforms[i])); } generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { Sphere* sphere = new Sphere(30); - env.push_back(new CollisionObject(shared_ptr(sphere), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(sphere), + transforms[i])); } generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { Cylinder* cylinder = new Cylinder(10, 40); - env.push_back(new CollisionObject(shared_ptr(cylinder), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(cylinder), + transforms[i])); } } -void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) -{ - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; +void generateEnvironmentsMesh(std::vector& env, + double env_scale, std::size_t n) { + FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, + env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Box box(5, 10, 20); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); - env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(model), + transforms[i])); } generateRandomTransforms(extents, transforms, n); Sphere sphere(30); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f(), 16, 16); - env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(model), + transforms[i])); } generateRandomTransforms(extents, transforms, n); Cylinder cylinder(10, 40); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f(), 16, 16); - env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(model), + transforms[i])); } } -void generateSelfDistanceEnvironments(std::vector& env, double env_scale, std::size_t n) -{ - unsigned int n_edge = std::floor(std::pow(n, 1/3.0)); +void generateSelfDistanceEnvironments(std::vector& env, + double env_scale, std::size_t n) { + unsigned int n_edge = std::floor(std::pow(n, 1 / 3.0)); FCL_REAL step_size = env_scale * 2 / n_edge; FCL_REAL delta_size = step_size * 0.05; FCL_REAL single_size = step_size - 2 * delta_size; - + unsigned int i = 0; - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { + for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Box* box = new Box(single_size, single_size, single_size); - env.push_back(new CollisionObject(shared_ptr(box), - Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale)))); + env.push_back(new CollisionObject( + shared_ptr(box), + Transform3f(Vec3f( + x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale)))); } - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { + for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Sphere* sphere = new Sphere(single_size / 2); - env.push_back(new CollisionObject(shared_ptr(sphere), - Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale)))); + env.push_back(new CollisionObject( + shared_ptr(sphere), + Transform3f(Vec3f( + x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale)))); } - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { + for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cylinder* cylinder = new Cylinder(single_size / 2, single_size); - env.push_back(new CollisionObject(shared_ptr(cylinder), - Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale)))); + env.push_back(new CollisionObject( + shared_ptr(cylinder), + Transform3f(Vec3f( + x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale)))); } - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { + for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cone* cone = new Cone(single_size / 2, single_size); - env.push_back(new CollisionObject(shared_ptr(cone), - Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale)))); + env.push_back(new CollisionObject( + shared_ptr(cone), + Transform3f(Vec3f( + x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale)))); } } -void generateSelfDistanceEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) -{ - unsigned int n_edge = std::floor(std::pow(n, 1/3.0)); +void generateSelfDistanceEnvironmentsMesh(std::vector& env, + double env_scale, std::size_t n) { + unsigned int n_edge = std::floor(std::pow(n, 1 / 3.0)); FCL_REAL step_size = env_scale * 2 / n_edge; FCL_REAL delta_size = step_size * 0.05; FCL_REAL single_size = step_size - 2 * delta_size; - + std::size_t i = 0; - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { + for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; @@ -424,14 +432,15 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Box box(single_size, single_size, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); - env.push_back(new CollisionObject(shared_ptr(model), - Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale)))); + env.push_back(new CollisionObject( + shared_ptr(model), + Transform3f(Vec3f( + x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale)))); } - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { + for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; @@ -439,14 +448,15 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Sphere sphere(single_size / 2); BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f(), 16, 16); - env.push_back(new CollisionObject(shared_ptr(model), - Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale)))); + env.push_back(new CollisionObject( + shared_ptr(model), + Transform3f(Vec3f( + x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale)))); } - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { + for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; @@ -454,14 +464,15 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Cylinder cylinder(single_size / 2, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f(), 16, 16); - env.push_back(new CollisionObject(shared_ptr(model), - Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale)))); + env.push_back(new CollisionObject( + shared_ptr(model), + Transform3f(Vec3f( + x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale)))); } - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { + for (; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; @@ -469,48 +480,63 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Cone cone(single_size / 2, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, cone, Transform3f(), 16, 16); - env.push_back(new CollisionObject(shared_ptr(model), - Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale)))); + env.push_back(new CollisionObject( + shared_ptr(model), + Transform3f(Vec3f( + x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale)))); } } - -void broad_phase_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) -{ +void broad_phase_collision_test(double env_scale, std::size_t env_size, + std::size_t query_size, + std::size_t num_max_contacts, bool exhaustive, + bool use_mesh) { std::vector ts; std::vector timers; std::vector env; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(query, env_scale, query_size); else generateEnvironments(query, env_scale, query_size); std::vector managers; - + managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - + Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); // FCL_REAL ncell_per_axis = std::pow((FCL_REAL)env_size / 10, 1 / 3.0); FCL_REAL ncell_per_axis = 20; - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis); - // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + FCL_REAL cell_size = + std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, + (upper_limit[1] - lower_limit[1]) / ncell_per_axis), + (upper_limit[2] - lower_limit[2]) / ncell_per_axis); + // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, + // lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager< + SparseHashTable >( + cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); @@ -523,7 +549,8 @@ void broad_phase_collision_test(double env_scale, std::size_t env_size, std::siz } { - DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + DynamicAABBTreeCollisionManager_Array* m = + new DynamicAABBTreeCollisionManager_Array(); m->tree_init_level = 2; managers.push_back(m); } @@ -531,16 +558,14 @@ void broad_phase_collision_test(double env_scale, std::size_t env_size, std::siz ts.resize(managers.size()); timers.resize(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); @@ -548,90 +573,82 @@ void broad_phase_collision_test(double env_scale, std::size_t env_size, std::siz } std::vector self_data(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { - if(exhaustive) self_data[i].request.num_max_contacts = 100000; - else self_data[i].request.num_max_contacts = num_max_contacts; + for (size_t i = 0; i < managers.size(); ++i) { + if (exhaustive) + self_data[i].request.num_max_contacts = 100000; + else + self_data[i].request.num_max_contacts = num_max_contacts; } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->collide(&self_data[i], defaultCollisionFunction); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - for(size_t i = 0; i < managers.size(); ++i) + for (size_t i = 0; i < managers.size(); ++i) std::cout << self_data[i].result.numContacts() << " "; std::cout << std::endl; - if(exhaustive) - { - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); - } - else - { + if (exhaustive) { + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(self_data[i].result.numContacts() == + self_data[0].result.numContacts()); + } else { std::vector self_res(managers.size()); - for(size_t i = 0; i < self_res.size(); ++i) + for (size_t i = 0; i < self_res.size(); ++i) self_res[i] = (self_data[i].result.numContacts() > 0); - - for(size_t i = 1; i < self_res.size(); ++i) + + for (size_t i = 1; i < self_res.size(); ++i) BOOST_CHECK(self_res[0] == self_res[i]); - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(self_data[i].result.numContacts() == + self_data[0].result.numContacts()); } - - for(size_t i = 0; i < query.size(); ++i) - { + for (size_t i = 0; i < query.size(); ++i) { std::vector query_data(managers.size()); - for(size_t j = 0; j < query_data.size(); ++j) - { - if(exhaustive) query_data[j].request.num_max_contacts = 100000; - else query_data[j].request.num_max_contacts = num_max_contacts; + for (size_t j = 0; j < query_data.size(); ++j) { + if (exhaustive) + query_data[j].request.num_max_contacts = 100000; + else + query_data[j].request.num_max_contacts = num_max_contacts; } - for(size_t j = 0; j < query_data.size(); ++j) - { + for (size_t j = 0; j < query_data.size(); ++j) { timers[j].start(); managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); } - // for(size_t j = 0; j < managers.size(); ++j) // std::cout << query_data[j].result.numContacts() << " "; // std::cout << std::endl; - - if(exhaustive) - { - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts()); - } - else - { + + if (exhaustive) { + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(query_data[j].result.numContacts() == + query_data[0].result.numContacts()); + } else { std::vector query_res(managers.size()); - for(size_t j = 0; j < query_res.size(); ++j) + for (size_t j = 0; j < query_res.size(); ++j) query_res[j] = (query_data[j].result.numContacts() > 0); - for(size_t j = 1; j < query_res.size(); ++j) + for (size_t j = 1; j < query_res.size(); ++j) BOOST_CHECK(query_res[0] == query_res[j]); - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts()); + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(query_data[j].result.numContacts() == + query_data[0].result.numContacts()); } } - for(size_t i = 0; i < env.size(); ++i) - delete env[i]; - for(size_t i = 0; i < query.size(); ++i) - delete query[i]; + for (size_t i = 0; i < env.size(); ++i) delete env[i]; + for (size_t i = 0; i < query.size(); ++i) delete query[i]; - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; + for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); size_t w = 7; @@ -639,65 +656,72 @@ void broad_phase_collision_test(double env_scale, std::size_t env_size, std::siz std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { + for (size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; - for(size_t j = 3; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; + for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; - std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; - } -void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh) -{ +void broad_phase_self_distance_test(double env_scale, std::size_t env_size, + bool use_mesh) { std::vector ts; std::vector timers; std::vector env; - if(use_mesh) + if (use_mesh) generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size); else generateSelfDistanceEnvironments(env, env_scale, env_size); - + std::vector managers; - + managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - + Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, (upper_limit[1] - lower_limit[1]) / 5), (upper_limit[2] - lower_limit[2]) / 5); - // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, + (upper_limit[1] - lower_limit[1]) / 5), + (upper_limit[2] - lower_limit[2]) / 5); + // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, + // lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager< + SparseHashTable >( + cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeCollisionManager_Array()); @@ -709,7 +733,8 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool } { - DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + DynamicAABBTreeCollisionManager_Array* m = + new DynamicAABBTreeCollisionManager_Array(); m->tree_init_level = 2; managers.push_back(m); } @@ -717,27 +742,23 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool ts.resize(managers.size()); timers.resize(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - std::vector self_data(managers.size()); - - for(size_t i = 0; i < self_data.size(); ++i) - { + + for (size_t i = 0; i < self_data.size(); ++i) { timers[i].start(); managers[i]->distance(&self_data[i], defaultDistanceFunction); timers[i].stop(); @@ -746,15 +767,17 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool } // std::cout << std::endl; - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) < DELTA || - fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) / fabs(self_data[0].result.min_distance) < DELTA); + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(fabs(self_data[0].result.min_distance - + self_data[i].result.min_distance) < DELTA || + fabs(self_data[0].result.min_distance - + self_data[i].result.min_distance) / + fabs(self_data[0].result.min_distance) < + DELTA); - for(size_t i = 0; i < env.size(); ++i) - delete env[i]; + for (size_t i = 0; i < env.size(); ++i) delete env[i]; - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; + for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); size_t w = 7; @@ -762,35 +785,34 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool std::cout << "self distance timing summary" << std::endl; std::cout << env.size() << " objs" << std::endl; std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "self distance time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } - -void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh) -{ +void broad_phase_distance_test(double env_scale, std::size_t env_size, + std::size_t query_size, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); @@ -798,30 +820,27 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size std::vector query; BroadPhaseCollisionManager* manager = new NaiveCollisionManager(); - for(std::size_t i = 0; i < env.size(); ++i) - manager->registerObject(env[i]); + for (std::size_t i = 0; i < env.size(); ++i) manager->registerObject(env[i]); manager->setup(); - while(1) - { + while (1) { std::vector candidates; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(candidates, env_scale, query_size); else generateEnvironments(candidates, env_scale, query_size); - for(std::size_t i = 0; i < candidates.size(); ++i) - { + for (std::size_t i = 0; i < candidates.size(); ++i) { CollisionData query_data; manager->collide(candidates[i], &query_data, defaultCollisionFunction); - if(query_data.result.numContacts() == 0) + if (query_data.result.numContacts() == 0) query.push_back(candidates[i]); else delete candidates[i]; - if(query.size() == query_size) break; + if (query.size() == query_size) break; } - if(query.size() == query_size) break; + if (query.size() == query_size) break; } delete manager; @@ -832,15 +851,27 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - + Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); - // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + FCL_REAL cell_size = + std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, + (upper_limit[1] - lower_limit[1]) / 20), + (upper_limit[2] - lower_limit[2]) / 20); + // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, + // lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager< + SparseHashTable >( + cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeCollisionManager_Array()); @@ -852,7 +883,8 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size } { - DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + DynamicAABBTreeCollisionManager_Array* m = + new DynamicAABBTreeCollisionManager_Array(); m->tree_init_level = 2; managers.push_back(m); } @@ -860,28 +892,23 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size ts.resize(managers.size()); timers.resize(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - - for(size_t i = 0; i < query.size(); ++i) - { + for (size_t i = 0; i < query.size(); ++i) { std::vector query_data(managers.size()); - for(size_t j = 0; j < managers.size(); ++j) - { + for (size_t j = 0; j < managers.size(); ++j) { timers[j].start(); managers[j]->distance(query[i], &query_data[j], defaultDistanceFunction); timers[j].stop(); @@ -890,20 +917,19 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size } // std::cout << std::endl; - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) < DELTA || - fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) / fabs(query_data[0].result.min_distance) < DELTA); + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(fabs(query_data[0].result.min_distance - + query_data[j].result.min_distance) < DELTA || + fabs(query_data[0].result.min_distance - + query_data[j].result.min_distance) / + fabs(query_data[0].result.min_distance) < + DELTA); } + for (std::size_t i = 0; i < env.size(); ++i) delete env[i]; + for (std::size_t i = 0; i < query.size(); ++i) delete query[i]; - for(std::size_t i = 0; i < env.size(); ++i) - delete env[i]; - for(std::size_t i = 0; i < query.size(); ++i) - delete query[i]; - - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; - + for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); size_t w = 7; @@ -911,67 +937,77 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size std::cout << "distance timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "distance time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { + for (size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; - for(size_t j = 2; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; + for (size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } - -void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) -{ +void broad_phase_update_collision_test(double env_scale, std::size_t env_size, + std::size_t query_size, + std::size_t num_max_contacts, + bool exhaustive, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; - if(use_mesh) - generateEnvironmentsMesh(query, env_scale, query_size); + if (use_mesh) + generateEnvironmentsMesh(query, env_scale, query_size); else - generateEnvironments(query, env_scale, query_size); + generateEnvironments(query, env_scale, query_size); std::vector managers; - + managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); - managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - + Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); - // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + FCL_REAL cell_size = + std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, + (upper_limit[1] - lower_limit[1]) / 20), + (upper_limit[2] - lower_limit[2]) / 20); + // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, + // lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager< + SparseHashTable >( + cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeCollisionManager_Array()); @@ -983,7 +1019,8 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s } { - DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + DynamicAABBTreeCollisionManager_Array* m = + new DynamicAABBTreeCollisionManager_Array(); m->tree_init_level = 2; managers.push_back(m); } @@ -991,16 +1028,14 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s ts.resize(managers.size()); timers.resize(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - - for(size_t i = 0; i < managers.size(); ++i) - { + + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); @@ -1008,16 +1043,22 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s } // update the environment - FCL_REAL delta_angle_max = 10 / 360.0 * 2 * boost::math::constants::pi(); + FCL_REAL delta_angle_max = + 10 / 360.0 * 2 * boost::math::constants::pi(); FCL_REAL delta_trans_max = 0.01 * env_scale; - for(size_t i = 0; i < env.size(); ++i) - { - FCL_REAL rand_angle_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + for (size_t i = 0; i < env.size(); ++i) { + FCL_REAL rand_angle_x = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_x = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + FCL_REAL rand_angle_y = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_y = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + FCL_REAL rand_angle_z = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_z = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; Quaternion3f q1, q2, q3; q1.fromAxisAngle(Vec3f(1, 0, 0), rand_angle_x); @@ -1027,15 +1068,14 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s Matrix3f dR; q.toRotation(dR); Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); - + Matrix3f R = env[i]->getRotation(); Vec3f T = env[i]->getTranslation(); env[i]->setTransform(dR * R, dR * T + dT); env[i]->computeAABB(); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->update(); timers[i].stop(); @@ -1043,93 +1083,82 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s } std::vector self_data(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { - if(exhaustive) self_data[i].request.num_max_contacts = 100000; - else self_data[i].request.num_max_contacts = num_max_contacts; + for (size_t i = 0; i < managers.size(); ++i) { + if (exhaustive) + self_data[i].request.num_max_contacts = 100000; + else + self_data[i].request.num_max_contacts = num_max_contacts; } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->collide(&self_data[i], defaultCollisionFunction); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - - for(size_t i = 0; i < managers.size(); ++i) + for (size_t i = 0; i < managers.size(); ++i) std::cout << self_data[i].result.numContacts() << " "; std::cout << std::endl; - if(exhaustive) - { - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); - } - else - { + if (exhaustive) { + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(self_data[i].result.numContacts() == + self_data[0].result.numContacts()); + } else { std::vector self_res(managers.size()); - for(size_t i = 0; i < self_res.size(); ++i) + for (size_t i = 0; i < self_res.size(); ++i) self_res[i] = (self_data[i].result.numContacts() > 0); - - for(size_t i = 1; i < self_res.size(); ++i) + + for (size_t i = 1; i < self_res.size(); ++i) BOOST_CHECK(self_res[0] == self_res[i]); - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(self_data[i].result.numContacts() == + self_data[0].result.numContacts()); } - - for(size_t i = 0; i < query.size(); ++i) - { + for (size_t i = 0; i < query.size(); ++i) { std::vector query_data(managers.size()); - for(size_t j = 0; j < query_data.size(); ++j) - { - if(exhaustive) query_data[j].request.num_max_contacts = 100000; - else query_data[j].request.num_max_contacts = num_max_contacts; + for (size_t j = 0; j < query_data.size(); ++j) { + if (exhaustive) + query_data[j].request.num_max_contacts = 100000; + else + query_data[j].request.num_max_contacts = num_max_contacts; } - for(size_t j = 0; j < query_data.size(); ++j) - { + for (size_t j = 0; j < query_data.size(); ++j) { timers[j].start(); managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); } - // for(size_t j = 0; j < managers.size(); ++j) // std::cout << query_data[j].result.numContacts() << " "; // std::cout << std::endl; - - if(exhaustive) - { - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts()); - } - else - { + + if (exhaustive) { + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(query_data[j].result.numContacts() == + query_data[0].result.numContacts()); + } else { std::vector query_res(managers.size()); - for(size_t j = 0; j < query_res.size(); ++j) + for (size_t j = 0; j < query_res.size(); ++j) query_res[j] = (query_data[j].result.numContacts() > 0); - for(size_t j = 1; j < query_res.size(); ++j) + for (size_t j = 1; j < query_res.size(); ++j) BOOST_CHECK(query_res[0] == query_res[j]); - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts()); + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(query_data[j].result.numContacts() == + query_data[0].result.numContacts()); } } + for (size_t i = 0; i < env.size(); ++i) delete env[i]; + for (size_t i = 0; i < query.size(); ++i) delete query[i]; - for(size_t i = 0; i < env.size(); ++i) - delete env[i]; - for(size_t i = 0; i < query.size(); ++i) - delete query[i]; - - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; - + for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); size_t w = 7; @@ -1137,42 +1166,36 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "update time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[3] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { + for (size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; - for(size_t j = 4; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; + for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; - std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } - - - diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp index 46a004530..4798e66d0 100644 --- a/test/broadphase_collision_1.cpp +++ b/test/broadphase_collision_1.cpp @@ -65,20 +65,29 @@ using namespace hpp::fcl; /// @brief make sure if broadphase algorithms doesn't check twice for the same /// collision object pair -void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, bool verbose = false); +void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, + bool verbose = false); /// @brief test for broad phase update -void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); +void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, + std::size_t query_size, + std::size_t num_max_contacts = 1, + bool exhaustive = false, + bool use_mesh = false); #if USE_GOOGLEHASH -template -struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; - -template -struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > -{ - GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() - { +template +struct GoogleSparseHashTable + : public google::sparse_hash_map, + std::equal_to> {}; + +template +struct GoogleDenseHashTable + : public google::dense_hash_map, + std::equal_to> { + GoogleDenseHashTable() + : google::dense_hash_map, + std::equal_to>() { this->set_empty_key(nullptr); } }; @@ -86,8 +95,7 @@ struct GoogleDenseHashTable : public google::dense_hash_map> checkedPairs; - bool checkUniquenessAndAddPair(CollisionObject* o1, CollisionObject* o2) - { + bool checkUniquenessAndAddPair(CollisionObject* o1, CollisionObject* o2) { auto search = checkedPairs.find(std::make_pair(o1, o2)); - if (search != checkedPairs.end()) - return false; + if (search != checkedPairs.end()) return false; checkedPairs.emplace(o1, o2); @@ -186,21 +187,18 @@ struct CollisionDataForUniquenessChecking }; //============================================================================== -struct CollisionFunctionForUniquenessChecking -: CollisionCallBackBase -{ - bool collide(CollisionObject* o1, CollisionObject* o2) - { +struct CollisionFunctionForUniquenessChecking : CollisionCallBackBase { + bool collide(CollisionObject* o1, CollisionObject* o2) { BOOST_CHECK(data.checkUniquenessAndAddPair(o1, o2)); return false; } - + CollisionDataForUniquenessChecking data; }; //============================================================================== -void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, bool verbose) -{ +void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, + bool verbose) { std::vector ts; std::vector timers; @@ -214,11 +212,23 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, managers.push_back(new IntervalTreeCollisionManager()); Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + FCL_REAL cell_size = + std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, + (upper_limit[1] - lower_limit[1]) / 20), + (upper_limit[2] - lower_limit[2]) / 20); + managers.push_back( + new SpatialHashingCollisionManager< + detail::SparseHashTable>( + cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager>( + cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager>( + cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeArrayCollisionManager()); @@ -230,7 +240,8 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, } { - DynamicAABBTreeArrayCollisionManager* m = new DynamicAABBTreeArrayCollisionManager(); + DynamicAABBTreeArrayCollisionManager* m = + new DynamicAABBTreeArrayCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } @@ -238,16 +249,14 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, ts.resize(managers.size()); timers.resize(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); @@ -255,21 +264,26 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, } // update the environment - FCL_REAL delta_angle_max = 10 / 360.0 * 2 * boost::math::constants::pi(); + FCL_REAL delta_angle_max = + 10 / 360.0 * 2 * boost::math::constants::pi(); FCL_REAL delta_trans_max = 0.01 * env_scale; - for(size_t i = 0; i < env.size(); ++i) - { - FCL_REAL rand_angle_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - - Matrix3f dR( - Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) - * Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) - * Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); + for (size_t i = 0; i < env.size(); ++i) { + FCL_REAL rand_angle_x = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_x = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + FCL_REAL rand_angle_y = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_y = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + FCL_REAL rand_angle_z = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_z = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + + Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * + Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * + Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); Matrix3f R = env[i]->getRotation(); @@ -278,8 +292,7 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, env[i]->computeAABB(); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->update(); timers[i].stop(); @@ -288,8 +301,7 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, std::vector self_data(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { CollisionFunctionForUniquenessChecking callback; timers[i].start(); managers[i]->collide(&callback); @@ -297,11 +309,9 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, ts[i].push_back(timers[i].getElapsedTime()); } - for (auto obj : env) - delete obj; + for (auto obj : env) delete obj; - if (!verbose) - return; + if (!verbose) return; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); int w = 7; @@ -309,60 +319,55 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs" << std::endl; std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "update time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[3] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { + for (size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; - for(size_t j = 4; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; + for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } -void broad_phase_update_collision_test(FCL_REAL env_scale, - std::size_t env_size, +void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, - bool exhaustive, - bool use_mesh) -{ + bool exhaustive, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(query, env_scale, query_size); else generateEnvironments(query, env_scale, query_size); @@ -372,18 +377,30 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); - managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); - // managers.push_back(new SpatialHashingCollisionManager(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + FCL_REAL cell_size = + std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, + (upper_limit[1] - lower_limit[1]) / 20), + (upper_limit[2] - lower_limit[2]) / 20); + // managers.push_back(new SpatialHashingCollisionManager(cell_size, + // lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager< + detail::SparseHashTable>( + cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager>( + cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager>( + cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeArrayCollisionManager()); @@ -395,7 +412,8 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, } { - DynamicAABBTreeArrayCollisionManager* m = new DynamicAABBTreeArrayCollisionManager(); + DynamicAABBTreeArrayCollisionManager* m = + new DynamicAABBTreeArrayCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } @@ -403,16 +421,14 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, ts.resize(managers.size()); timers.resize(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); @@ -420,21 +436,26 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, } // update the environment - FCL_REAL delta_angle_max = 10 / 360.0 * 2 * boost::math::constants::pi(); + FCL_REAL delta_angle_max = + 10 / 360.0 * 2 * boost::math::constants::pi(); FCL_REAL delta_trans_max = 0.01 * env_scale; - for(size_t i = 0; i < env.size(); ++i) - { - FCL_REAL rand_angle_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - - Matrix3f dR( - Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) - * Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) - * Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); + for (size_t i = 0; i < env.size(); ++i) { + FCL_REAL rand_angle_x = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_x = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + FCL_REAL rand_angle_y = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_y = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + FCL_REAL rand_angle_z = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; + FCL_REAL rand_trans_z = + 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + + Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * + Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * + Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); Matrix3f R = env[i]->getRotation(); @@ -443,8 +464,7 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, env[i]->computeAABB(); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->update(); timers[i].stop(); @@ -452,14 +472,14 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, } std::vector self_data(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { - if(exhaustive) self_data[i].request.num_max_contacts = 100000; - else self_data[i].request.num_max_contacts = num_max_contacts; + for (size_t i = 0; i < managers.size(); ++i) { + if (exhaustive) + self_data[i].request.num_max_contacts = 100000; + else + self_data[i].request.num_max_contacts = num_max_contacts; } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { CollisionCallBackDefault callback; timers[i].start(); managers[i]->collide(&callback); @@ -467,80 +487,69 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, ts[i].push_back(timers[i].getElapsedTime()); } - - for(size_t i = 0; i < managers.size(); ++i) + for (size_t i = 0; i < managers.size(); ++i) std::cout << self_data[i].result.numContacts() << " "; std::cout << std::endl; - if(exhaustive) - { - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); - } - else - { + if (exhaustive) { + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(self_data[i].result.numContacts() == + self_data[0].result.numContacts()); + } else { std::vector self_res(managers.size()); - for(size_t i = 0; i < self_res.size(); ++i) + for (size_t i = 0; i < self_res.size(); ++i) self_res[i] = (self_data[i].result.numContacts() > 0); - for(size_t i = 1; i < self_res.size(); ++i) + for (size_t i = 1; i < self_res.size(); ++i) BOOST_CHECK(self_res[0] == self_res[i]); - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(self_data[i].result.numContacts() == + self_data[0].result.numContacts()); } - - for(size_t i = 0; i < query.size(); ++i) - { + for (size_t i = 0; i < query.size(); ++i) { std::vector query_callbacks(managers.size()); - - for(size_t j = 0; j < query_callbacks.size(); ++j) - { - if(exhaustive) query_callbacks[j].data.request.num_max_contacts = 100000; - else query_callbacks[j].data.request.num_max_contacts = num_max_contacts; + + for (size_t j = 0; j < query_callbacks.size(); ++j) { + if (exhaustive) + query_callbacks[j].data.request.num_max_contacts = 100000; + else + query_callbacks[j].data.request.num_max_contacts = num_max_contacts; } - for(size_t j = 0; j < query_callbacks.size(); ++j) - { + for (size_t j = 0; j < query_callbacks.size(); ++j) { timers[j].start(); managers[j]->collide(query[i], &query_callbacks[j]); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); } - // for(size_t j = 0; j < managers.size(); ++j) // std::cout << query_callbacks[j].result.numContacts() << " "; // std::cout << std::endl; - if(exhaustive) - { - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(query_callbacks[j].data.result.numContacts() == query_callbacks[0].data.result.numContacts()); - } - else - { + if (exhaustive) { + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(query_callbacks[j].data.result.numContacts() == + query_callbacks[0].data.result.numContacts()); + } else { std::vector query_res(managers.size()); - for(size_t j = 0; j < query_res.size(); ++j) + for (size_t j = 0; j < query_res.size(); ++j) query_res[j] = (query_callbacks[j].data.result.numContacts() > 0); - for(size_t j = 1; j < query_res.size(); ++j) + for (size_t j = 1; j < query_res.size(); ++j) BOOST_CHECK(query_res[0] == query_res[j]); - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(query_callbacks[j].data.result.numContacts() == query_callbacks[0].data.result.numContacts()); + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(query_callbacks[j].data.result.numContacts() == + query_callbacks[0].data.result.numContacts()); } } + for (size_t i = 0; i < env.size(); ++i) delete env[i]; + for (size_t i = 0; i < query.size(); ++i) delete query[i]; - for(size_t i = 0; i < env.size(); ++i) - delete env[i]; - for(size_t i = 0; i < query.size(); ++i) - delete query[i]; - - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; - + for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); int w = 7; @@ -548,38 +557,35 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "update time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[3] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { + for (size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; - for(size_t j = 4; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; + for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; - std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp index 34829f37a..a6a667465 100644 --- a/test/broadphase_collision_2.cpp +++ b/test/broadphase_collision_2.cpp @@ -62,25 +62,31 @@ using namespace hpp::fcl; /// @brief test for broad phase collision and self collision -void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); +void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, + std::size_t query_size, + std::size_t num_max_contacts = 1, + bool exhaustive = false, bool use_mesh = false); #if USE_GOOGLEHASH -template -struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; - -template -struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > -{ - GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() - { +template +struct GoogleSparseHashTable + : public google::sparse_hash_map, + std::equal_to > {}; + +template +struct GoogleDenseHashTable + : public google::dense_hash_map, + std::equal_to > { + GoogleDenseHashTable() + : google::dense_hash_map, + std::equal_to >() { this->set_empty_key(nullptr); } }; #endif /// check broad phase collision for empty collision object set and queries -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) { #ifdef NDEBUG broad_phase_collision_test(2000, 0, 0, 10, false, false); broad_phase_collision_test(2000, 0, 1000, 10, false, false); @@ -117,8 +123,7 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) } /// check broad phase collision and self collision, only return collision or not -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 1, false); broad_phase_collision_test(2000, 1000, 1000, 1, false); @@ -133,8 +138,7 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) } /// check broad phase collision and self collision, return 10 contacts -BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) -{ +BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 10, false); broad_phase_collision_test(2000, 1000, 1000, 10, false); @@ -144,9 +148,9 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) #endif } -/// check broad phase collision and self collision, return only collision or not, in mesh -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) -{ +/// check broad phase collision and self collision, return only collision or +/// not, in mesh +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 1, false, true); broad_phase_collision_test(2000, 1000, 1000, 1, false, true); @@ -157,8 +161,7 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) } /// check broad phase collision and self collision, return 10 contacts, in mesh -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) -{ +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 10, false, true); broad_phase_collision_test(2000, 1000, 1000, 10, false, true); @@ -169,8 +172,7 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) } /// check broad phase collision and self collision, exhaustive, in mesh -BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) -{ +BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) { #ifdef NDEBUG broad_phase_collision_test(2000, 100, 1000, 1, true, true); broad_phase_collision_test(2000, 1000, 1000, 1, true, true); @@ -180,24 +182,21 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) #endif } -void broad_phase_collision_test(FCL_REAL env_scale, - std::size_t env_size, +void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, std::size_t query_size, - std::size_t num_max_contacts, - bool exhaustive, - bool use_mesh) -{ + std::size_t num_max_contacts, bool exhaustive, + bool use_mesh) { std::vector ts; std::vector timers; std::vector env; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; - if(use_mesh) + if (use_mesh) generateEnvironmentsMesh(query, env_scale, query_size); else generateEnvironments(query, env_scale, query_size); @@ -213,12 +212,24 @@ void broad_phase_collision_test(FCL_REAL env_scale, SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); // FCL_REAL ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0); FCL_REAL ncell_per_axis = 20; - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis); - // managers.push_back(new SpatialHashingCollisionManager(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + FCL_REAL cell_size = + std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, + (upper_limit[1] - lower_limit[1]) / ncell_per_axis), + (upper_limit[2] - lower_limit[2]) / ncell_per_axis); + // managers.push_back(new SpatialHashingCollisionManager(cell_size, + // lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); + managers.push_back( + new SpatialHashingCollisionManager >( + cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); @@ -231,7 +242,8 @@ void broad_phase_collision_test(FCL_REAL env_scale, } { - DynamicAABBTreeArrayCollisionManager* m = new DynamicAABBTreeArrayCollisionManager(); + DynamicAABBTreeArrayCollisionManager* m = + new DynamicAABBTreeArrayCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } @@ -239,16 +251,14 @@ void broad_phase_collision_test(FCL_REAL env_scale, ts.resize(managers.size()); timers.resize(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); @@ -256,90 +266,82 @@ void broad_phase_collision_test(FCL_REAL env_scale, } std::vector callbacks(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { - if(exhaustive) callbacks[i].data.request.num_max_contacts = 100000; - else callbacks[i].data.request.num_max_contacts = num_max_contacts; + for (size_t i = 0; i < managers.size(); ++i) { + if (exhaustive) + callbacks[i].data.request.num_max_contacts = 100000; + else + callbacks[i].data.request.num_max_contacts = num_max_contacts; } - for(size_t i = 0; i < managers.size(); ++i) - { + for (size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->collide(&callbacks[i]); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } - for(size_t i = 0; i < managers.size(); ++i) + for (size_t i = 0; i < managers.size(); ++i) std::cout << callbacks[i].data.result.numContacts() << " "; std::cout << std::endl; - if(exhaustive) - { - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(callbacks[i].data.result.numContacts() == callbacks[0].data.result.numContacts()); - } - else - { + if (exhaustive) { + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(callbacks[i].data.result.numContacts() == + callbacks[0].data.result.numContacts()); + } else { std::vector self_res(managers.size()); - for(size_t i = 0; i < self_res.size(); ++i) + for (size_t i = 0; i < self_res.size(); ++i) self_res[i] = (callbacks[i].data.result.numContacts() > 0); - for(size_t i = 1; i < self_res.size(); ++i) + for (size_t i = 1; i < self_res.size(); ++i) BOOST_CHECK(self_res[0] == self_res[i]); - for(size_t i = 1; i < managers.size(); ++i) - BOOST_CHECK(callbacks[i].data.result.numContacts() == callbacks[0].data.result.numContacts()); + for (size_t i = 1; i < managers.size(); ++i) + BOOST_CHECK(callbacks[i].data.result.numContacts() == + callbacks[0].data.result.numContacts()); } - - for(size_t i = 0; i < query.size(); ++i) - { + for (size_t i = 0; i < query.size(); ++i) { std::vector callbacks(managers.size()); - for(size_t j = 0; j < managers.size(); ++j) - { - if(exhaustive) callbacks[j].data.request.num_max_contacts = 100000; - else callbacks[j].data.request.num_max_contacts = num_max_contacts; + for (size_t j = 0; j < managers.size(); ++j) { + if (exhaustive) + callbacks[j].data.request.num_max_contacts = 100000; + else + callbacks[j].data.request.num_max_contacts = num_max_contacts; } - for(size_t j = 0; j < managers.size(); ++j) - { + for (size_t j = 0; j < managers.size(); ++j) { timers[j].start(); managers[j]->collide(query[i], &callbacks[j]); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); } - // for(size_t j = 0; j < managers.size(); ++j) // std::cout << callbacks[i].data.result.numContacts() << " "; // std::cout << std::endl; - if(exhaustive) - { - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(callbacks[j].data.result.numContacts() == callbacks[0].data.result.numContacts()); - } - else - { + if (exhaustive) { + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(callbacks[j].data.result.numContacts() == + callbacks[0].data.result.numContacts()); + } else { std::vector query_res(managers.size()); - for(size_t j = 0; j < query_res.size(); ++j) + for (size_t j = 0; j < query_res.size(); ++j) query_res[j] = (callbacks[j].data.result.numContacts() > 0); - for(size_t j = 1; j < query_res.size(); ++j) + for (size_t j = 1; j < query_res.size(); ++j) BOOST_CHECK(query_res[0] == query_res[j]); - for(size_t j = 1; j < managers.size(); ++j) - BOOST_CHECK(callbacks[j].data.result.numContacts() == callbacks[0].data.result.numContacts()); + for (size_t j = 1; j < managers.size(); ++j) + BOOST_CHECK(callbacks[j].data.result.numContacts() == + callbacks[0].data.result.numContacts()); } } - for(size_t i = 0; i < env.size(); ++i) - delete env[i]; - for(size_t i = 0; i < query.size(); ++i) - delete query[i]; + for (size_t i = 0; i < env.size(); ++i) delete env[i]; + for (size_t i = 0; i < query.size(); ++i) delete query[i]; - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; + for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); int w = 7; @@ -347,35 +349,31 @@ void broad_phase_collision_test(FCL_REAL env_scale, std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { + for (size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; - for(size_t j = 3; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; + for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; - std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) + for (size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; - } diff --git a/test/broadphase_dynamic_AABB_tree.cpp b/test/broadphase_dynamic_AABB_tree.cpp index 13dcf7068..a809be6be 100644 --- a/test/broadphase_dynamic_AABB_tree.cpp +++ b/test/broadphase_dynamic_AABB_tree.cpp @@ -49,8 +49,7 @@ using namespace hpp::fcl; // Pack the data for callback function. -struct CallBackData -{ +struct CallBackData { bool expect_object0_then_object1; std::vector* objects; }; @@ -62,18 +61,13 @@ struct CallBackData // FCL_REAL& parameter, which specifies the distance beyond which the // pair of objects will be skipped. -struct DistanceCallBackDerived -: DistanceCallBackBase -{ - - bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL & dist) - { - return distance_callback(o1,o2,&data,dist); +struct DistanceCallBackDerived : DistanceCallBackBase { + bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist) { + return distance_callback(o1, o2, &data, dist); } bool distance_callback(CollisionObject* a, CollisionObject*, - void* callback_data, FCL_REAL&) - { + void* callback_data, FCL_REAL&) { // Unpack the data. CallBackData* data = static_cast(callback_data); const std::vector& objects = *(data->objects); @@ -91,8 +85,6 @@ struct DistanceCallBackDerived CallBackData data; }; - - // Tests repeatability of a dynamic tree of two spheres when we call update() // and distance() again and again without changing the poses of the objects. // We only use the distance() method to invoke a hierarchy traversal. @@ -104,8 +96,7 @@ struct DistanceCallBackDerived // repeatability problem as mentioned in: // https://github.com/flexible-collision-library/fcl/issues/368 // -BOOST_AUTO_TEST_CASE(DynamicAABBTreeCollisionManager_class) -{ +BOOST_AUTO_TEST_CASE(DynamicAABBTreeCollisionManager_class) { CollisionGeometryPtr_t sphere0 = make_shared(0.1); CollisionGeometryPtr_t sphere1 = make_shared(0.2); CollisionObject object0(sphere0); diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index 513f0facf..38bac56cb 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -35,7 +35,6 @@ /** \author Jeongseok Lee */ - #define BOOST_TEST_MODULE FCL_BVH_MODELS #include #include @@ -54,32 +53,28 @@ using namespace hpp::fcl; -template -void testBVHModelPointCloud() -{ - - Box box (Vec3f::Ones()); +template +void testBVHModelPointCloud() { + Box box(Vec3f::Ones()); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; std::vector points(8); - points[0] << a, -b, c; - points[1] << a, b, c; - points[2] << -a, b, c; - points[3] << -a, -b, c; - points[4] << a, -b, -c; - points[5] << a, b, -c; - points[6] << -a, b, -c; + points[0] << a, -b, c; + points[1] << a, b, c; + points[2] << -a, b, c; + points[3] << -a, -b, c; + points[4] << a, -b, -c; + points[5] << a, b, -c; + points[6] << -a, b, -c; points[7] << -a, -b, -c; { shared_ptr > model(new BVHModel); - if (model->getNodeType() != BV_AABB - && model->getNodeType() != BV_KDOP16 - && model->getNodeType() != BV_KDOP18 - && model->getNodeType() != BV_KDOP24) - { + 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; @@ -91,8 +86,7 @@ void testBVHModelPointCloud() result = model->beginModel(); BOOST_CHECK_EQUAL(result, BVH_OK); - for (std::size_t i = 0; i < points.size(); ++i) - { + for (std::size_t i = 0; i < points.size(); ++i) { result = model->addVertex(points[i]); BOOST_CHECK_EQUAL(result, BVH_OK); } @@ -110,19 +104,17 @@ void testBVHModelPointCloud() { shared_ptr > model(new BVHModel); - if (model->getNodeType() != BV_AABB - && model->getNodeType() != BV_KDOP16 - && model->getNodeType() != BV_KDOP18 - && model->getNodeType() != BV_KDOP24) - { + 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; } - Matrixx3f all_points((Eigen::DenseIndex)points.size(),3); - for(size_t k = 0; k < points.size(); ++k) + Matrixx3f all_points((Eigen::DenseIndex)points.size(), 3); + for (size_t k = 0; k < points.size(); ++k) all_points.row((Eigen::DenseIndex)k) = points[k].transpose(); int result; @@ -143,25 +135,24 @@ void testBVHModelPointCloud() } } -template -void testBVHModelTriangles() -{ +template +void testBVHModelTriangles() { shared_ptr > model(new BVHModel); - Box box (Vec3f::Ones()); - AABB aabb (Vec3f(-1,0,-1), Vec3f(1,1,1)); + Box box(Vec3f::Ones()); + AABB aabb(Vec3f(-1, 0, -1), Vec3f(1, 1, 1)); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; std::vector points(8); std::vector tri_indices(12); - points[0] << a, -b, c; - points[1] << a, b, c; - points[2] << -a, b, c; - points[3] << -a, -b, c; - points[4] << a, -b, -c; - points[5] << a, b, -c; - points[6] << -a, b, -c; + points[0] << a, -b, c; + points[1] << a, b, c; + points[2] << -a, b, c; + points[3] << -a, -b, c; + points[4] << a, -b, -c; + points[5] << a, b, -c; + points[6] << -a, b, -c; points[7] << -a, -b, -c; tri_indices[0].set(0, 4, 1); @@ -182,9 +173,10 @@ void testBVHModelTriangles() 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]]); + 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); } @@ -204,53 +196,52 @@ void testBVHModelTriangles() BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(Vec3f(0,1,0)); + pose.setTranslation(Vec3f(0, 1, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(Vec3f(0,0,0)); - FCL_REAL sqrt2_2 = std::sqrt(2)/2; - pose.setQuatRotation(Quaternion3f(sqrt2_2,sqrt2_2,0,0)); + pose.setTranslation(Vec3f(0, 0, 0)); + FCL_REAL sqrt2_2 = std::sqrt(2) / 2; + pose.setQuatRotation(Quaternion3f(sqrt2_2, sqrt2_2, 0, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(-Vec3f(1,1,1)); + pose.setTranslation(-Vec3f(1, 1, 1)); pose.setQuatRotation(Quaternion3f::Identity()); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_CHECK(!cropped); - aabb = AABB(Vec3f(-0.1,-0.1,-0.1), Vec3f(0.1,0.1,0.1)); - pose.setTranslation(Vec3f(-0.5,-0.5,0)); + aabb = AABB(Vec3f(-0.1, -0.1, -0.1), Vec3f(0.1, 0.1, 0.1)); + pose.setTranslation(Vec3f(-0.5, -0.5, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK_EQUAL(cropped->num_tris, 2); BOOST_CHECK_EQUAL(cropped->num_vertices, 6); } -template -void testBVHModelSubModel() -{ +template +void testBVHModelSubModel() { shared_ptr > model(new BVHModel); - Box box (Vec3f::Ones()); + Box box(Vec3f::Ones()); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; std::vector points(8); std::vector tri_indices(12); - points[0] << a, -b, c; - points[1] << a, b, c; - points[2] << -a, b, c; - points[3] << -a, -b, c; - points[4] << a, -b, -c; - points[5] << a, b, -c; - points[6] << -a, b, -c; + points[0] << a, -b, c; + points[1] << a, b, c; + points[2] << -a, b, c; + points[3] << -a, -b, c; + points[4] << a, -b, -c; + points[5] << a, b, -c; + points[6] << -a, b, -c; points[7] << -a, -b, -c; tri_indices[0].set(0, 4, 1); @@ -284,16 +275,14 @@ void testBVHModelSubModel() BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); } -template -void testBVHModel() -{ +template +void testBVHModel() { testBVHModelTriangles(); testBVHModelPointCloud(); testBVHModelSubModel(); } -BOOST_AUTO_TEST_CASE(building_bvh_models) -{ +BOOST_AUTO_TEST_CASE(building_bvh_models) { testBVHModel(); testBVHModel(); testBVHModel(); @@ -304,62 +293,59 @@ BOOST_AUTO_TEST_CASE(building_bvh_models) testBVHModel >(); } -template -void testLoadPolyhedron () -{ +template +void testLoadPolyhedron() { boost::filesystem::path path(TEST_RESOURCES_DIR); std::string env = (path / "env.obj").string(), rob = (path / "rob.obj").string(); typedef BVHModel Polyhedron_t; - typedef shared_ptr PolyhedronPtr_t; - PolyhedronPtr_t P1 (new Polyhedron_t), P2; + typedef shared_ptr PolyhedronPtr_t; + PolyhedronPtr_t P1(new Polyhedron_t), P2; Vec3f scale; - scale.setConstant (1); - loadPolyhedronFromResource (env, scale, P1); + scale.setConstant(1); + loadPolyhedronFromResource(env, scale, P1); - scale.setConstant (-1); - CachedMeshLoader loader (P1->getNodeType()); - CollisionGeometryPtr_t geom = loader.load (env, scale); - P2 = dynamic_pointer_cast (geom); - BOOST_REQUIRE (P2); + scale.setConstant(-1); + CachedMeshLoader loader(P1->getNodeType()); + CollisionGeometryPtr_t geom = loader.load(env, scale); + P2 = dynamic_pointer_cast(geom); + BOOST_REQUIRE(P2); - BOOST_CHECK_EQUAL(P1->num_tris , P2->num_tris); - BOOST_CHECK_EQUAL(P1->num_vertices , P2->num_vertices); - BOOST_CHECK_EQUAL(P1->getNumBVs() , P2->getNumBVs()); + BOOST_CHECK_EQUAL(P1->num_tris, P2->num_tris); + BOOST_CHECK_EQUAL(P1->num_vertices, P2->num_vertices); + BOOST_CHECK_EQUAL(P1->getNumBVs(), P2->getNumBVs()); - CollisionGeometryPtr_t geom2 = loader.load (env, scale); - BOOST_CHECK_EQUAL (geom, geom2); + CollisionGeometryPtr_t geom2 = loader.load(env, scale); + BOOST_CHECK_EQUAL(geom, geom2); } -template -void testLoadGerardBauzil () -{ +template +void testLoadGerardBauzil() { boost::filesystem::path path(TEST_RESOURCES_DIR); std::string env = (path / "staircases_koroibot_hr.dae").string(); typedef BVHModel Polyhedron_t; - typedef shared_ptr PolyhedronPtr_t; - PolyhedronPtr_t P1 (new Polyhedron_t), P2; + typedef shared_ptr PolyhedronPtr_t; + PolyhedronPtr_t P1(new Polyhedron_t), P2; Vec3f scale; - scale.setConstant (1); - loadPolyhedronFromResource (env, scale, P1); - CollisionGeometryPtr_t cylinder (new Cylinder(.27, .27)); - Transform3f pos (Vec3f (-1.33, 1.36, .14)); - CollisionObject obj (cylinder, pos); - CollisionObject stairs (P1); + scale.setConstant(1); + loadPolyhedronFromResource(env, scale, P1); + CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27)); + Transform3f pos(Vec3f(-1.33, 1.36, .14)); + CollisionObject obj(cylinder, pos); + CollisionObject stairs(P1); CollisionRequest request; CollisionResult result; - collide (&stairs, &obj, request, result); - BOOST_CHECK (result.isCollision ()); + collide(&stairs, &obj, request, result); + BOOST_CHECK(result.isCollision()); } -BOOST_AUTO_TEST_CASE(load_polyhedron) -{ +BOOST_AUTO_TEST_CASE(load_polyhedron) { testLoadPolyhedron(); testLoadPolyhedron(); testLoadPolyhedron(); @@ -370,17 +356,14 @@ BOOST_AUTO_TEST_CASE(load_polyhedron) testLoadPolyhedron >(); } -BOOST_AUTO_TEST_CASE (gerard_bauzil) -{ +BOOST_AUTO_TEST_CASE(gerard_bauzil) { testLoadGerardBauzil(); testLoadGerardBauzil(); testLoadGerardBauzil(); testLoadGerardBauzil(); } - -BOOST_AUTO_TEST_CASE(load_illformated_mesh) -{ +BOOST_AUTO_TEST_CASE(load_illformated_mesh) { boost::filesystem::path path(TEST_RESOURCES_DIR); const std::string filename = (path / "illformated_mesh.dae").string(); diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp index d079ac29d..e86e5328f 100644 --- a/test/capsule_box_1.cpp +++ b/test/capsule_box_1.cpp @@ -34,11 +34,10 @@ /** \author Florent Lamiraux */ - #define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES #include -#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps)) +#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include #include @@ -49,70 +48,69 @@ #include "utility.h" -BOOST_AUTO_TEST_CASE(distance_capsule_box) -{ +BOOST_AUTO_TEST_CASE(distance_capsule_box) { using hpp::fcl::CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 - CollisionGeometryPtr_t capsuleGeometry (new hpp::fcl::Capsule (2., 4.)); + CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.)); // Box of size 1 by 2 by 4 - CollisionGeometryPtr_t boxGeometry (new hpp::fcl::Box (1., 2., 4.)); + CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.)); // Enable computation of nearest points - hpp::fcl::DistanceRequest distanceRequest (true, 0, 0); + hpp::fcl::DistanceRequest distanceRequest(true, 0, 0); hpp::fcl::DistanceResult distanceResult; - - hpp::fcl::Transform3f tf1 (hpp::fcl::Vec3f (3., 0, 0)); + + hpp::fcl::Transform3f tf1(hpp::fcl::Vec3f(3., 0, 0)); hpp::fcl::Transform3f tf2; - hpp::fcl::CollisionObject capsule (capsuleGeometry, tf1); - hpp::fcl::CollisionObject box (boxGeometry, tf2); + hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1); + hpp::fcl::CollisionObject box(boxGeometry, tf2); // test distance - hpp::fcl::distance (&capsule, &box, distanceRequest, distanceResult); + hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); // Nearest point on capsule - hpp::fcl::Vec3f o1 (distanceResult.nearest_points [0]); + hpp::fcl::Vec3f o1(distanceResult.nearest_points[0]); // Nearest point on box - hpp::fcl::Vec3f o2 (distanceResult.nearest_points [1]); - BOOST_CHECK_CLOSE (distanceResult.min_distance, 0.5, 1e-1); - BOOST_CHECK_CLOSE (o1 [0], 1.0, 1e-1); - CHECK_CLOSE_TO_0 (o1 [1], 1e-1); - BOOST_CHECK_CLOSE (o2 [0], 0.5, 1e-1); - CHECK_CLOSE_TO_0 (o2 [1], 1e-1); + hpp::fcl::Vec3f o2(distanceResult.nearest_points[1]); + BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.5, 1e-1); + BOOST_CHECK_CLOSE(o1[0], 1.0, 1e-1); + CHECK_CLOSE_TO_0(o1[1], 1e-1); + BOOST_CHECK_CLOSE(o2[0], 0.5, 1e-1); + CHECK_CLOSE_TO_0(o2[1], 1e-1); // Move capsule above box - tf1 = hpp::fcl::Transform3f (hpp::fcl::Vec3f (0., 0., 8.)); - capsule.setTransform (tf1); + tf1 = hpp::fcl::Transform3f(hpp::fcl::Vec3f(0., 0., 8.)); + capsule.setTransform(tf1); // test distance - distanceResult.clear (); - hpp::fcl::distance (&capsule, &box, distanceRequest, distanceResult); - o1 = distanceResult.nearest_points [0]; - o2 = distanceResult.nearest_points [1]; + distanceResult.clear(); + hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + o1 = distanceResult.nearest_points[0]; + o2 = distanceResult.nearest_points[1]; - BOOST_CHECK_CLOSE (distanceResult.min_distance, 2.0, 1e-1); - CHECK_CLOSE_TO_0 (o1 [0], 1e-1); - CHECK_CLOSE_TO_0 (o1 [1], 1e-1); - BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-1); + BOOST_CHECK_CLOSE(distanceResult.min_distance, 2.0, 1e-1); + CHECK_CLOSE_TO_0(o1[0], 1e-1); + CHECK_CLOSE_TO_0(o1[1], 1e-1); + BOOST_CHECK_CLOSE(o1[2], 4.0, 1e-1); - CHECK_CLOSE_TO_0 (o2 [0], 1e-1); - CHECK_CLOSE_TO_0 (o2 [1], 1e-1); - BOOST_CHECK_CLOSE (o2 [2], 2.0, 1e-1); + CHECK_CLOSE_TO_0(o2[0], 1e-1); + CHECK_CLOSE_TO_0(o2[1], 1e-1); + BOOST_CHECK_CLOSE(o2[2], 2.0, 1e-1); // Rotate capsule around y axis by pi/2 and move it behind box - tf1.setTranslation (hpp::fcl::Vec3f (-10., 0., 0.)); - tf1.setQuatRotation (hpp::fcl::makeQuat (sqrt(2)/2, 0, sqrt(2)/2, 0)); - capsule.setTransform (tf1); + tf1.setTranslation(hpp::fcl::Vec3f(-10., 0., 0.)); + tf1.setQuatRotation(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0)); + capsule.setTransform(tf1); // test distance - distanceResult.clear (); - hpp::fcl::distance (&capsule, &box, distanceRequest, distanceResult); - o1 = distanceResult.nearest_points [0]; - o2 = distanceResult.nearest_points [1]; - - BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-1); - BOOST_CHECK_CLOSE (o1 [0], -6, 1e-2); - CHECK_CLOSE_TO_0 (o1 [1], 1e-1); - CHECK_CLOSE_TO_0 (o1 [2], 1e-1); - BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-2); - CHECK_CLOSE_TO_0 (o2 [1], 1e-1); - CHECK_CLOSE_TO_0 (o2 [2], 1e-1); + distanceResult.clear(); + hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + o1 = distanceResult.nearest_points[0]; + o2 = distanceResult.nearest_points[1]; + + BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-1); + BOOST_CHECK_CLOSE(o1[0], -6, 1e-2); + CHECK_CLOSE_TO_0(o1[1], 1e-1); + CHECK_CLOSE_TO_0(o1[2], 1e-1); + BOOST_CHECK_CLOSE(o2[0], -0.5, 1e-2); + CHECK_CLOSE_TO_0(o2[1], 1e-1); + CHECK_CLOSE_TO_0(o2[2], 1e-1); } diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp index 34a232bc7..add71efd6 100644 --- a/test/capsule_box_2.cpp +++ b/test/capsule_box_2.cpp @@ -34,11 +34,10 @@ /** \author Florent Lamiraux */ - #define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES #include -#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps)) +#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include "utility.h" @@ -49,36 +48,36 @@ #include #include -BOOST_AUTO_TEST_CASE(distance_capsule_box) -{ - typedef hpp::fcl::shared_ptr CollisionGeometryPtr_t; +BOOST_AUTO_TEST_CASE(distance_capsule_box) { + typedef hpp::fcl::shared_ptr + CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 - CollisionGeometryPtr_t capsuleGeometry (new hpp::fcl::Capsule (2., 4.)); + CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.)); // Box of size 1 by 2 by 4 - CollisionGeometryPtr_t boxGeometry (new hpp::fcl::Box (1., 2., 4.)); + CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.)); // Enable computation of nearest points - hpp::fcl::DistanceRequest distanceRequest (true, 0, 0); + hpp::fcl::DistanceRequest distanceRequest(true, 0, 0); hpp::fcl::DistanceResult distanceResult; // Rotate capsule around y axis by pi/2 and move it behind box - hpp::fcl::Transform3f tf1 (hpp::fcl::makeQuat (sqrt(2)/2, 0, sqrt(2)/2, 0), - hpp::fcl::Vec3f (-10., 0.8, 1.5)); + hpp::fcl::Transform3f tf1(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), + hpp::fcl::Vec3f(-10., 0.8, 1.5)); hpp::fcl::Transform3f tf2; - hpp::fcl::CollisionObject capsule (capsuleGeometry, tf1); - hpp::fcl::CollisionObject box (boxGeometry, tf2); + hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1); + hpp::fcl::CollisionObject box(boxGeometry, tf2); // test distance - distanceResult.clear (); - hpp::fcl::distance (&capsule, &box, distanceRequest, distanceResult); - hpp::fcl::Vec3f o1 = distanceResult.nearest_points [0]; - hpp::fcl::Vec3f o2 = distanceResult.nearest_points [1]; + distanceResult.clear(); + hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + hpp::fcl::Vec3f o1 = distanceResult.nearest_points[0]; + hpp::fcl::Vec3f o2 = distanceResult.nearest_points[1]; - BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-2); - BOOST_CHECK_CLOSE (o1 [0], -6, 1e-2); - BOOST_CHECK_CLOSE (o1 [1], 0.8, 1e-1); - BOOST_CHECK_CLOSE (o1 [2], 1.5, 1e-2); - BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-2); - BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-1); - BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-2); + BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-2); + BOOST_CHECK_CLOSE(o1[0], -6, 1e-2); + BOOST_CHECK_CLOSE(o1[1], 0.8, 1e-1); + BOOST_CHECK_CLOSE(o1[2], 1.5, 1e-2); + BOOST_CHECK_CLOSE(o2[0], -0.5, 1e-2); + BOOST_CHECK_CLOSE(o2[1], 0.8, 1e-1); + BOOST_CHECK_CLOSE(o2[2], 1.5, 1e-2); } diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp index d2b70ff14..25d95455d 100644 --- a/test/capsule_capsule.cpp +++ b/test/capsule_capsule.cpp @@ -38,7 +38,7 @@ #define BOOST_TEST_MODULE FCL_CAPSULE_CAPSULE #include -#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps)) +#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include #include @@ -53,286 +53,304 @@ using namespace hpp::fcl; -BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) -{ +BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) { const double radius = 1.; - - CollisionGeometryPtr_t c1 (new Capsule (radius, 0.)); - CollisionGeometryPtr_t c2 (new Capsule (radius, 0.)); - - CollisionGeometryPtr_t s1 (new Sphere (radius)); - CollisionGeometryPtr_t s2 (new Sphere (radius)); - - #ifndef NDEBUG - int num_tests = 1e3; - #else - int num_tests = 1e6; - #endif - + + CollisionGeometryPtr_t c1(new Capsule(radius, 0.)); + CollisionGeometryPtr_t c2(new Capsule(radius, 0.)); + + CollisionGeometryPtr_t s1(new Sphere(radius)); + CollisionGeometryPtr_t s2(new Sphere(radius)); + +#ifndef NDEBUG + int num_tests = 1e3; +#else + int num_tests = 1e6; +#endif + Transform3f tf1; Transform3f tf2; - - for(int i = 0; i < num_tests; ++i) - { - Eigen::Vector3d p1 = Eigen::Vector3d::Random()*(2.*radius); - Eigen::Vector3d p2 = Eigen::Vector3d::Random()*(2.*radius); - - Eigen::Matrix3d rot1 = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix(); - Eigen::Matrix3d rot2 = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix(); - - tf1.setTranslation(p1); tf1.setRotation(rot1); - tf2.setTranslation(p2); tf2.setRotation(rot2); - - CollisionObject capsule_o1 (c1, tf1); - CollisionObject capsule_o2 (c2, tf2); - - CollisionObject sphere_o1 (s1, tf1); - CollisionObject sphere_o2 (s2, tf2); - + + for (int i = 0; i < num_tests; ++i) { + Eigen::Vector3d p1 = Eigen::Vector3d::Random() * (2. * radius); + Eigen::Vector3d p2 = Eigen::Vector3d::Random() * (2. * radius); + + Eigen::Matrix3d rot1 = + Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) + .toRotationMatrix(); + Eigen::Matrix3d rot2 = + Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) + .toRotationMatrix(); + + tf1.setTranslation(p1); + tf1.setRotation(rot1); + tf2.setTranslation(p2); + tf2.setRotation(rot2); + + CollisionObject capsule_o1(c1, tf1); + CollisionObject capsule_o2(c2, tf2); + + CollisionObject sphere_o1(s1, tf1); + CollisionObject sphere_o2(s2, tf2); + // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult, sphere_collisionResult; - - size_t sphere_num_collisions = collide(&sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult); - size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); - + + size_t sphere_num_collisions = collide( + &sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult); + size_t capsule_num_collisions = collide( + &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); + BOOST_CHECK_EQUAL(sphere_num_collisions, capsule_num_collisions); if (sphere_num_collisions == 0 && capsule_num_collisions == 0) BOOST_CHECK_CLOSE(sphere_collisionResult.distance_lower_bound, - capsule_collisionResult.distance_lower_bound, - 1e-6); + capsule_collisionResult.distance_lower_bound, 1e-6); } } -BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) -{ +BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { const double radius = 0.01; const double length = 0.2; - - CollisionGeometryPtr_t c1 (new Capsule (radius, length)); - CollisionGeometryPtr_t c2 (new Capsule (radius, length)); + + CollisionGeometryPtr_t c1(new Capsule(radius, length)); + CollisionGeometryPtr_t c2(new Capsule(radius, length)); #ifndef NDEBUG int num_tests = 1e3; #else int num_tests = 1e6; #endif - + Transform3f tf1; Transform3f tf2; - + Eigen::Vector3d p1 = Eigen::Vector3d::Zero(); - Eigen::Vector3d p2_no_collision = Eigen::Vector3d(0.,0.,2*(length/2. + radius) + 1e-3); // because capsule are along the Z axis - - for(int i = 0; i < num_tests; ++i) - { - Eigen::Matrix3d rot = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix(); - - tf1.setTranslation(p1); tf1.setRotation(rot); - tf2.setTranslation(p2_no_collision); tf2.setRotation(rot); - - CollisionObject capsule_o1 (c1, tf1); - CollisionObject capsule_o2 (c2, tf2); - + Eigen::Vector3d p2_no_collision = + Eigen::Vector3d(0., 0., + 2 * (length / 2. + radius) + + 1e-3); // because capsule are along the Z axis + + for (int i = 0; i < num_tests; ++i) { + Eigen::Matrix3d rot = + Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) + .toRotationMatrix(); + + tf1.setTranslation(p1); + tf1.setRotation(rot); + tf2.setTranslation(p2_no_collision); + tf2.setRotation(rot); + + CollisionObject capsule_o1(c1, tf1); + CollisionObject capsule_o2(c2, tf2); + // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult; - - size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); - + + size_t capsule_num_collisions = collide( + &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); + BOOST_CHECK(capsule_num_collisions == 0); } - - Eigen::Vector3d p2_with_collision = Eigen::Vector3d(0.,0.,std::min(length/2.,radius)*(1.-1e-2)); - for(int i = 0; i < num_tests; ++i) - { - Eigen::Matrix3d rot = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix(); - - tf1.setTranslation(p1); tf1.setRotation(rot); - tf2.setTranslation(p2_with_collision); tf2.setRotation(rot); - - CollisionObject capsule_o1 (c1, tf1); - CollisionObject capsule_o2 (c2, tf2); - + + Eigen::Vector3d p2_with_collision = + Eigen::Vector3d(0., 0., std::min(length / 2., radius) * (1. - 1e-2)); + for (int i = 0; i < num_tests; ++i) { + Eigen::Matrix3d rot = + Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) + .toRotationMatrix(); + + tf1.setTranslation(p1); + tf1.setRotation(rot); + tf2.setTranslation(p2_with_collision); + tf2.setRotation(rot); + + CollisionObject capsule_o1(c1, tf1); + CollisionObject capsule_o2(c2, tf2); + // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult; - - size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); - + + size_t capsule_num_collisions = collide( + &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); + BOOST_CHECK(capsule_num_collisions > 0); } - - p2_no_collision = Eigen::Vector3d(0.,0.,2*(length/2. + radius) + 1e-3); - - Transform3f geom1_placement(Eigen::Matrix3d::Identity(),Eigen::Vector3d::Zero()); - Transform3f geom2_placement(Eigen::Matrix3d::Identity(),p2_no_collision); - - for(int i = 0; i < num_tests; ++i) - { - Eigen::Matrix3d rot = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix(); + + p2_no_collision = Eigen::Vector3d(0., 0., 2 * (length / 2. + radius) + 1e-3); + + Transform3f geom1_placement(Eigen::Matrix3d::Identity(), + Eigen::Vector3d::Zero()); + Transform3f geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision); + + for (int i = 0; i < num_tests; ++i) { + Eigen::Matrix3d rot = + Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) + .toRotationMatrix(); Eigen::Vector3d trans = Eigen::Vector3d::Random(); - Transform3f displacement(rot,trans); + Transform3f displacement(rot, trans); Transform3f tf1 = displacement * geom1_placement; Transform3f tf2 = displacement * geom2_placement; - - CollisionObject capsule_o1 (c1, tf1); - CollisionObject capsule_o2 (c2, tf2); - + + CollisionObject capsule_o1(c1, tf1); + CollisionObject capsule_o2(c2, tf2); + // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult; - - size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); - + + size_t capsule_num_collisions = collide( + &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); + BOOST_CHECK(capsule_num_collisions == 0); } - -// p2_with_collision = Eigen::Vector3d(0.,0.,std::min(length/2.,radius)*(1.-1e-2)); - p2_with_collision = Eigen::Vector3d(0.,0.,0.01); + + // p2_with_collision = + // Eigen::Vector3d(0.,0.,std::min(length/2.,radius)*(1.-1e-2)); + p2_with_collision = Eigen::Vector3d(0., 0., 0.01); geom2_placement.setTranslation(p2_with_collision); - - for(int i = 0; i < num_tests; ++i) - { - Eigen::Matrix3d rot = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix(); + + for (int i = 0; i < num_tests; ++i) { + Eigen::Matrix3d rot = + Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) + .toRotationMatrix(); Eigen::Vector3d trans = Eigen::Vector3d::Random(); - Transform3f displacement(rot,trans); + Transform3f displacement(rot, trans); Transform3f tf1 = displacement * geom1_placement; Transform3f tf2 = displacement * geom2_placement; - - CollisionObject capsule_o1 (c1, tf1); - CollisionObject capsule_o2 (c2, tf2); - + + CollisionObject capsule_o1(c1, tf1); + CollisionObject capsule_o2(c2, tf2); + // Enable computation of nearest points CollisionRequest collisionRequest; CollisionResult capsule_collisionResult; - - size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); - + + size_t capsule_num_collisions = collide( + &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); + BOOST_CHECK(capsule_num_collisions > 0); } } -BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) -{ - CollisionGeometryPtr_t s1 (new Capsule (5, 10)); - CollisionGeometryPtr_t s2 (new Capsule (5, 10)); +BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) { + CollisionGeometryPtr_t s1(new Capsule(5, 10)); + CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3f tf1; - Transform3f tf2 (Vec3f(20.1, 0,0)); + Transform3f tf2(Vec3f(20.1, 0, 0)); - CollisionObject o1 (s1, tf1); - CollisionObject o2 (s2, tf2); + CollisionObject o1(s1, tf1); + CollisionObject o2(s2, tf2); // Enable computation of nearest points - DistanceRequest distanceRequest (true); + DistanceRequest distanceRequest(true); DistanceResult distanceResult; - distance (&o1, &o2, distanceRequest, distanceResult); + distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied translation on two capsules"; std::cerr << " T1 = " << tf1.getTranslation() - << ", T2 = " << tf2.getTranslation() << std::endl; - std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0] - << ", p2 = " << distanceResult.nearest_points [1] - << ", distance = " << distanceResult.min_distance << std::endl; + << ", T2 = " << tf2.getTranslation() << std::endl; + std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] + << ", p2 = " << distanceResult.nearest_points[1] + << ", distance = " << distanceResult.min_distance << std::endl; BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6); } -BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) -{ - CollisionGeometryPtr_t s1 (new Capsule (5, 10)); - CollisionGeometryPtr_t s2 (new Capsule (5, 10)); +BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { + CollisionGeometryPtr_t s1(new Capsule(5, 10)); + CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3f tf1; - Transform3f tf2 (Vec3f(20, 20,0)); + Transform3f tf2(Vec3f(20, 20, 0)); - CollisionObject o1 (s1, tf1); - CollisionObject o2 (s2, tf2); + CollisionObject o1(s1, tf1); + CollisionObject o2(s2, tf2); // Enable computation of nearest points - DistanceRequest distanceRequest (true); + DistanceRequest distanceRequest(true); DistanceResult distanceResult; - distance (&o1, &o2, distanceRequest, distanceResult); + distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied translation on two capsules"; std::cerr << " T1 = " << tf1.getTranslation() - << ", T2 = " << tf2.getTranslation() << std::endl; - std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0] - << ", p2 = " << distanceResult.nearest_points [1] - << ", distance = " << distanceResult.min_distance << std::endl; + << ", T2 = " << tf2.getTranslation() << std::endl; + std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] + << ", p2 = " << distanceResult.nearest_points[1] + << ", distance = " << distanceResult.min_distance << std::endl; FCL_REAL expected = sqrt(800) - 10; BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6); } -BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) -{ - CollisionGeometryPtr_t s1 (new Capsule (5, 10)); - CollisionGeometryPtr_t s2 (new Capsule (5, 10)); +BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) { + CollisionGeometryPtr_t s1(new Capsule(5, 10)); + CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3f tf1; - Transform3f tf2 (Vec3f(0,0,20.1)); + Transform3f tf2(Vec3f(0, 0, 20.1)); - CollisionObject o1 (s1, tf1); - CollisionObject o2 (s2, tf2); + CollisionObject o1(s1, tf1); + CollisionObject o2(s2, tf2); // Enable computation of nearest points - DistanceRequest distanceRequest (true); + DistanceRequest distanceRequest(true); DistanceResult distanceResult; - distance (&o1, &o2, distanceRequest, distanceResult); + distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied translation on two capsules"; std::cerr << " T1 = " << tf1.getTranslation() - << ", T2 = " << tf2.getTranslation() << std::endl; - std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0] - << ", p2 = " << distanceResult.nearest_points [1] - << ", distance = " << distanceResult.min_distance << std::endl; + << ", T2 = " << tf2.getTranslation() << std::endl; + std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] + << ", p2 = " << distanceResult.nearest_points[1] + << ", distance = " << distanceResult.min_distance << std::endl; BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.1, 1e-6); } - -BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) -{ - CollisionGeometryPtr_t s1 (new Capsule (5, 10)); - CollisionGeometryPtr_t s2 (new Capsule (5, 10)); +BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { + CollisionGeometryPtr_t s1(new Capsule(5, 10)); + CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3f tf1; - Transform3f tf2 (makeQuat (sqrt (2)/2, 0, sqrt (2)/2, 0), - Vec3f(0,0,25.1)); + Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3f(0, 0, 25.1)); - CollisionObject o1 (s1, tf1); - CollisionObject o2 (s2, tf2); + CollisionObject o1(s1, tf1); + CollisionObject o2(s2, tf2); // Enable computation of nearest points - DistanceRequest distanceRequest (true); + DistanceRequest distanceRequest(true); DistanceResult distanceResult; - distance (&o1, &o2, distanceRequest, distanceResult); + distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied rotation and translation on two capsules" << std::endl; - std::cerr << "R1 = " << tf1.getRotation () << std::endl + std::cerr << "R1 = " << tf1.getRotation() << std::endl << "T1 = " << tf1.getTranslation().transpose() << std::endl - << "R2 = " << tf2.getRotation () << std::endl + << "R2 = " << tf2.getRotation() << std::endl << "T2 = " << tf2.getTranslation().transpose() << std::endl; std::cerr << "Closest points:" << std::endl - << "p1 = " << distanceResult.nearest_points[0].transpose() << std::endl - << "p2 = " << distanceResult.nearest_points[1].transpose() << std::endl - << "distance = " << distanceResult.min_distance << std::endl; + << "p1 = " << distanceResult.nearest_points[0].transpose() + << std::endl + << "p2 = " << distanceResult.nearest_points[1].transpose() + << std::endl + << "distance = " << distanceResult.min_distance << std::endl; - const Vec3f& p1 = distanceResult.nearest_points [0]; - const Vec3f& p2 = distanceResult.nearest_points [1]; + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6); - CHECK_CLOSE_TO_0 (p1 [0], 1e-4); - CHECK_CLOSE_TO_0 (p1 [1], 1e-4); - BOOST_CHECK_CLOSE (p1 [2], 10, 1e-4); - CHECK_CLOSE_TO_0 (p2 [0], 1e-4); - CHECK_CLOSE_TO_0 (p2 [1], 1e-4); - BOOST_CHECK_CLOSE (p2 [2], 20.1, 1e-4); + CHECK_CLOSE_TO_0(p1[0], 1e-4); + CHECK_CLOSE_TO_0(p1[1], 1e-4); + BOOST_CHECK_CLOSE(p1[2], 10, 1e-4); + CHECK_CLOSE_TO_0(p2[0], 1e-4); + CHECK_CLOSE_TO_0(p2[1], 1e-4); + BOOST_CHECK_CLOSE(p2[2], 20.1, 1e-4); } diff --git a/test/collision.cpp b/test/collision.cpp index 0f099d8a0..1ad249848 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -65,12 +65,11 @@ namespace utf = boost::unit_test::framework; int num_max_contacts = (std::numeric_limits::max)(); -BOOST_AUTO_TEST_CASE(OBB_Box_test) -{ +BOOST_AUTO_TEST_CASE(OBB_Box_test) { FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); - + AABB aabb1; aabb1.min_ = Vec3f(-600, -600, -600); aabb1.max_ = Vec3f(600, 600, 600); @@ -87,15 +86,14 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) std::vector transforms; generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { AABB aabb; aabb.min_ = aabb1.min_ * 0.5; - aabb.max_ = aabb1.max_ * 0.5; + aabb.max_ = aabb1.max_ * 0.5; OBB obb2; convertBV(aabb, transforms[i], obb2); - + Box box2; Transform3f box2_tf; constructBox(aabb, transforms[i], box2, box2_tf); @@ -104,18 +102,18 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) FCL_REAL distance; bool overlap_obb = obb1.overlap(obb2); - bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, distance, false, NULL, NULL); - + bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, + distance, false, NULL, NULL); + BOOST_CHECK(overlap_obb == overlap_box); } } -BOOST_AUTO_TEST_CASE(OBB_shape_test) -{ +BOOST_AUTO_TEST_CASE(OBB_shape_test) { FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); - + AABB aabb1; aabb1.min_ = Vec3f(-600, -600, -600); aabb1.max_ = Vec3f(600, 600, 600); @@ -132,53 +130,55 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) std::vector transforms; generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; OBB obb2; GJKSolver solver; FCL_REAL distance; - - { + + { Sphere sphere(len); computeBV(sphere, transforms[i], obb2); - + bool overlap_obb = obb1.overlap(obb2); - bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], distance, false, NULL, NULL); + bool overlap_sphere = solver.shapeIntersect( + box1, box1_tf, sphere, transforms[i], distance, false, NULL, NULL); BOOST_CHECK(overlap_obb >= overlap_sphere); } { Capsule capsule(len, 2 * len); computeBV(capsule, transforms[i], obb2); - + bool overlap_obb = obb1.overlap(obb2); - bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], distance, false, NULL, NULL); + bool overlap_capsule = solver.shapeIntersect( + box1, box1_tf, capsule, transforms[i], distance, false, NULL, NULL); BOOST_CHECK(overlap_obb >= overlap_capsule); } { Cone cone(len, 2 * len); computeBV(cone, transforms[i], obb2); - + bool overlap_obb = obb1.overlap(obb2); - bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], distance, false, NULL, NULL); + bool overlap_cone = solver.shapeIntersect( + box1, box1_tf, cone, transforms[i], distance, false, NULL, NULL); BOOST_CHECK(overlap_obb >= overlap_cone); } { Cylinder cylinder(len, 2 * len); computeBV(cylinder, transforms[i], obb2); - + bool overlap_obb = obb1.overlap(obb2); - bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], distance, false, NULL, NULL); + bool overlap_cylinder = solver.shapeIntersect( + box1, box1_tf, cylinder, transforms[i], distance, false, NULL, NULL); BOOST_CHECK(overlap_obb >= overlap_cylinder); } } } -BOOST_AUTO_TEST_CASE(OBB_AABB_test) -{ +BOOST_AUTO_TEST_CASE(OBB_AABB_test) { FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; @@ -188,29 +188,29 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) AABB aabb1; aabb1.min_ = Vec3f(-600, -600, -600); aabb1.max_ = Vec3f(600, 600, 600); - + OBB obb1; convertBV(aabb1, Transform3f(), obb1); - - for(std::size_t i = 0; i < transforms.size(); ++i) - { + + for (std::size_t i = 0; i < transforms.size(); ++i) { AABB aabb; aabb.min_ = aabb1.min_ * 0.5; - aabb.max_ = aabb1.max_ * 0.5; + aabb.max_ = aabb1.max_ * 0.5; AABB aabb2 = translate(aabb, transforms[i].getTranslation()); - + OBB obb2; convertBV(aabb, Transform3f(transforms[i].getTranslation()), obb2); bool overlap_aabb = aabb1.overlap(aabb2); bool overlap_obb = obb1.overlap(obb2); - if(overlap_aabb != overlap_obb) - { + if (overlap_aabb != overlap_obb) { std::cout << aabb1.min_ << " " << aabb1.max_ << std::endl; std::cout << aabb2.min_ << " " << aabb2.max_ << std::endl; - std::cout << obb1.To << " " << obb1.extent << " " << obb1.axes << std::endl; - std::cout << obb2.To << " " << obb2.extent << " " << obb2.axes << std::endl; + std::cout << obb1.To << " " << obb1.extent << " " << obb1.axes + << std::endl; + std::cout << obb2.To << " " << obb2.extent << " " << obb2.axes + << std::endl; } BOOST_CHECK(overlap_aabb == overlap_obb); @@ -221,20 +221,38 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) std::ostream* bench_stream = NULL; bool bs_nl = true; bool bs_hp = false; -#define BENCHMARK(stream) if (bench_stream!=NULL) { *bench_stream << (bs_nl ? "" : ", ") << stream; bs_nl = false; } -#define BENCHMARK_HEADER(stream) if (!bs_hp) { BENCHMARK(stream) } -#define BENCHMARK_NEXT() if (bench_stream!=NULL && !bs_nl) { *bench_stream << '\n'; bs_nl = true; bs_hp = true; } +#define BENCHMARK(stream) \ + if (bench_stream != NULL) { \ + *bench_stream << (bs_nl ? "" : ", ") << stream; \ + bs_nl = false; \ + } +#define BENCHMARK_HEADER(stream) \ + if (!bs_hp) { \ + BENCHMARK(stream) \ + } +#define BENCHMARK_NEXT() \ + if (bench_stream != NULL && !bs_nl) { \ + *bench_stream << '\n'; \ + bs_nl = true; \ + bs_hp = true; \ + } typedef std::vector Contacts_t; -typedef boost::mpl::vector, KDOP<18>, KDOP<16>, kIOS, OBBRSS> BVs_t; -std::vector splitMethods = boost::assign::list_of (SPLIT_METHOD_MEAN)(SPLIT_METHOD_MEDIAN)(SPLIT_METHOD_BV_CENTER); +typedef boost::mpl::vector, KDOP<18>, KDOP<16>, kIOS, OBBRSS> + BVs_t; +std::vector splitMethods = boost::assign::list_of( + SPLIT_METHOD_MEAN)(SPLIT_METHOD_MEDIAN)(SPLIT_METHOD_BV_CENTER); typedef boost::chrono::high_resolution_clock clock_type; typedef clock_type::duration duration_type; #define BV_STR_SPECIALIZATION(bv) \ - template <> const char* str< bv > () { return #bv; } -template const char* str(); + template <> \ + const char* str() { \ + return #bv; \ + } +template +const char* str(); BV_STR_SPECIALIZATION(AABB) BV_STR_SPECIALIZATION(OBB) BV_STR_SPECIALIZATION(RSS) @@ -244,12 +262,11 @@ BV_STR_SPECIALIZATION(KDOP<16>) BV_STR_SPECIALIZATION(kIOS) BV_STR_SPECIALIZATION(OBBRSS) -template struct wrap {}; +template +struct wrap {}; -struct base_traits -{ - enum { IS_IMPLEMENTED = true - }; +struct base_traits { + enum { IS_IMPLEMENTED = true }; }; enum { @@ -259,29 +276,23 @@ enum { NonRecursive = false }; -template -struct traits : base_traits -{}; +template +struct traits : base_traits {}; -template -struct traits, Oriented, recursive> : base_traits -{ - enum { IS_IMPLEMENTED = false - }; +template +struct traits, Oriented, recursive> : base_traits { + enum { IS_IMPLEMENTED = false }; }; -struct mesh_mesh_run_test -{ - mesh_mesh_run_test (const std::vector& _transforms, - const CollisionRequest _request - ) : - transforms (_transforms), - request (_request), - enable_statistics (false), - benchmark (false), - isInit (false), - indent (0) - {} +struct mesh_mesh_run_test { + mesh_mesh_run_test(const std::vector& _transforms, + const CollisionRequest _request) + : transforms(_transforms), + request(_request), + enable_statistics(false), + benchmark(false), + isInit(false), + indent(0) {} const std::vector& transforms; const CollisionRequest request; @@ -292,22 +303,24 @@ struct mesh_mesh_run_test int indent; - const char* getindent() - { - assert (indent < 9); - static const char* t[] = { "", "\t", "\t\t", "\t\t\t", "\t\t\t\t", "\t\t\t\t\t", - "\t\t\t\t\t\t", "\t\t\t\t\t\t\t", "\t\t\t\t\t\t\t\t" }; + const char* getindent() { + assert(indent < 9); + static const char* t[] = {"", + "\t", + "\t\t", + "\t\t\t", + "\t\t\t\t", + "\t\t\t\t\t", + "\t\t\t\t\t\t", + "\t\t\t\t\t\t\t", + "\t\t\t\t\t\t\t\t"}; return t[indent]; } - template - void query ( - const std::vector& transforms, - SplitMethodType splitMethod, - const CollisionRequest request, - std::vector& contacts - ) - { + template + void query(const std::vector& transforms, + SplitMethodType splitMethod, const CollisionRequest request, + std::vector& contacts) { BENCHMARK_HEADER("BV"); BENCHMARK_HEADER("oriented"); BENCHMARK_HEADER("Split method"); @@ -323,37 +336,36 @@ struct mesh_mesh_run_test typedef BVHModel BVH_t; typedef shared_ptr BVHPtr_t; - BVHPtr_t model1 (new BVH_t), model2 (new BVH_t); + BVHPtr_t model1(new BVH_t), model2(new BVH_t); model1->bv_splitter.reset(new BVSplitter(splitMethod)); model2->bv_splitter.reset(new BVSplitter(splitMethod)); - loadPolyhedronFromResource (TEST_RESOURCES_DIR "/env.obj", Vec3f::Ones(), model1); - loadPolyhedronFromResource (TEST_RESOURCES_DIR "/rob.obj", Vec3f::Ones(), model2); + loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3f::Ones(), + model1); + loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3f::Ones(), + model2); clock_type::time_point start, end; const Transform3f tf2; const std::size_t N = transforms.size(); - contacts.resize (3*N); + contacts.resize(3 * N); - if (traits::IS_IMPLEMENTED) - { - BOOST_TEST_MESSAGE (getindent() << "BV: " << str() << " oriented"); + if (traits::IS_IMPLEMENTED) { + BOOST_TEST_MESSAGE(getindent() << "BV: " << str() << " oriented"); ++indent; - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { start = clock_type::now(); const Transform3f& tf1 = transforms[i]; CollisionResult local_result; - MeshCollisionTraversalNode node (request); + MeshCollisionTraversalNode node(request); node.enable_statistics = enable_statistics; - bool success = initialize (node, - *model1, tf1, *model2, tf2, - local_result); - BOOST_REQUIRE (success); + bool success = + initialize(node, *model1, tf1, *model2, tf2, local_result); + BOOST_REQUIRE(success); collide(&node, request, local_result); @@ -363,8 +375,10 @@ struct mesh_mesh_run_test BENCHMARK(1); BENCHMARK(splitMethod); if (enable_statistics) { - BOOST_TEST_MESSAGE (getindent() << "statistics: " << node.num_bv_tests << " " << node.num_leaf_tests); - BOOST_TEST_MESSAGE (getindent() << "nb contacts: " << local_result.numContacts()); + BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests + << " " << node.num_leaf_tests); + BOOST_TEST_MESSAGE(getindent() + << "nb contacts: " << local_result.numContacts()); BENCHMARK(node.num_bv_tests); BENCHMARK(node.num_leaf_tests); } @@ -373,8 +387,7 @@ struct mesh_mesh_run_test BENCHMARK((end - start).count()); BENCHMARK_NEXT(); - if(local_result.numContacts() > 0) - { + if (local_result.numContacts() > 0) { local_result.getContacts(contacts[i]); std::sort(contacts[i].begin(), contacts[i].end()); } @@ -382,18 +395,17 @@ struct mesh_mesh_run_test --indent; } - if (traits::IS_IMPLEMENTED) - { - BOOST_TEST_MESSAGE (getindent() << "BV: " << str()); + if (traits::IS_IMPLEMENTED) { + BOOST_TEST_MESSAGE(getindent() << "BV: " << str()); ++indent; - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { start = clock_type::now(); const Transform3f tf1 = transforms[i]; CollisionResult local_result; - MeshCollisionTraversalNode node (request); + MeshCollisionTraversalNode node( + request); node.enable_statistics = enable_statistics; BVH_t* model1_tmp = new BVH_t(*model1); @@ -401,10 +413,9 @@ struct mesh_mesh_run_test BVH_t* model2_tmp = new BVH_t(*model2); Transform3f tf2_tmp = tf2; - bool success = initialize (node, - *model1_tmp, tf1_tmp, *model2_tmp, tf2_tmp, - local_result, true, true); - BOOST_REQUIRE (success); + bool success = initialize(node, *model1_tmp, tf1_tmp, *model2_tmp, + tf2_tmp, local_result, true, true); + BOOST_REQUIRE(success); collide(&node, request, local_result); delete model1_tmp; @@ -415,8 +426,10 @@ struct mesh_mesh_run_test BENCHMARK(2); BENCHMARK(splitMethod); if (enable_statistics) { - BOOST_TEST_MESSAGE (getindent() << "statistics: " << node.num_bv_tests << " " << node.num_leaf_tests); - BOOST_TEST_MESSAGE (getindent() << "nb contacts: " << local_result.numContacts()); + BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests + << " " << node.num_leaf_tests); + BOOST_TEST_MESSAGE(getindent() + << "nb contacts: " << local_result.numContacts()); BENCHMARK(node.num_bv_tests); BENCHMARK(node.num_leaf_tests); } @@ -425,33 +438,30 @@ struct mesh_mesh_run_test BENCHMARK((end - start).count()); BENCHMARK_NEXT(); - if(local_result.numContacts() > 0) - { - local_result.getContacts(contacts[i+N]); - std::sort(contacts[i+N].begin(), contacts[i+N].end()); + if (local_result.numContacts() > 0) { + local_result.getContacts(contacts[i + N]); + std::sort(contacts[i + N].begin(), contacts[i + N].end()); } } --indent; } - if (traits::IS_IMPLEMENTED) - { - BOOST_TEST_MESSAGE (getindent() << "BV: " << str() << " oriented non-recursive"); + if (traits::IS_IMPLEMENTED) { + BOOST_TEST_MESSAGE(getindent() + << "BV: " << str() << " oriented non-recursive"); ++indent; - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { start = clock_type::now(); const Transform3f tf1 = transforms[i]; CollisionResult local_result; - MeshCollisionTraversalNode node (request); + MeshCollisionTraversalNode node(request); node.enable_statistics = enable_statistics; - bool success = initialize (node, - *model1, tf1, *model2, tf2, - local_result); - BOOST_REQUIRE (success); + bool success = + initialize(node, *model1, tf1, *model2, tf2, local_result); + BOOST_REQUIRE(success); collide(&node, request, local_result, NULL, false); @@ -460,8 +470,10 @@ struct mesh_mesh_run_test BENCHMARK(0); BENCHMARK(splitMethod); if (enable_statistics) { - BOOST_TEST_MESSAGE (getindent() << "statistics: " << node.num_bv_tests << " " << node.num_leaf_tests); - BOOST_TEST_MESSAGE (getindent() << "nb contacts: " << local_result.numContacts()); + BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests + << " " << node.num_leaf_tests); + BOOST_TEST_MESSAGE(getindent() + << "nb contacts: " << local_result.numContacts()); BENCHMARK(node.num_bv_tests); BENCHMARK(node.num_leaf_tests); } @@ -470,98 +482,95 @@ struct mesh_mesh_run_test BENCHMARK((end - start).count()); BENCHMARK_NEXT(); - if(local_result.numContacts() > 0) - { - local_result.getContacts(contacts[i+2*N]); - std::sort(contacts[i+2*N].begin(), contacts[i+2*N].end()); + if (local_result.numContacts() > 0) { + local_result.getContacts(contacts[i + 2 * N]); + std::sort(contacts[i + 2 * N].begin(), contacts[i + 2 * N].end()); } } --indent; } } - void check_contacts (std::size_t i0, std::size_t i1, bool warn) - { - for(std::size_t i = i0; i < i1; ++i) { + void check_contacts(std::size_t i0, std::size_t i1, bool warn) { + for (std::size_t i = i0; i < i1; ++i) { Contacts_t in_ref_but_not_in_i; - std::set_difference ( - contacts_ref[i].begin(), contacts_ref[i].end(), - contacts [i].begin(), contacts [i].end(), + std::set_difference( + contacts_ref[i].begin(), contacts_ref[i].end(), contacts[i].begin(), + contacts[i].end(), std::inserter(in_ref_but_not_in_i, in_ref_but_not_in_i.begin())); - if(!in_ref_but_not_in_i.empty()) { - for(std::size_t j = 0; j < in_ref_but_not_in_i.size(); ++j) { + if (!in_ref_but_not_in_i.empty()) { + for (std::size_t j = 0; j < in_ref_but_not_in_i.size(); ++j) { if (warn) { - BOOST_WARN_MESSAGE (false, "Missed contacts: " - << in_ref_but_not_in_i[j].b1 << ", " - << in_ref_but_not_in_i[j].b2); + BOOST_WARN_MESSAGE( + false, "Missed contacts: " << in_ref_but_not_in_i[j].b1 << ", " + << in_ref_but_not_in_i[j].b2); } else { - BOOST_CHECK_MESSAGE(false, "Missed contacts: " - << in_ref_but_not_in_i[j].b1 << ", " - << in_ref_but_not_in_i[j].b2); + BOOST_CHECK_MESSAGE( + false, "Missed contacts: " << in_ref_but_not_in_i[j].b1 << ", " + << in_ref_but_not_in_i[j].b2); } } } Contacts_t in_i_but_not_in_ref; - std::set_difference ( - contacts [i].begin(), contacts [i].end(), - contacts_ref[i].begin(), contacts_ref[i].end(), + std::set_difference( + contacts[i].begin(), contacts[i].end(), contacts_ref[i].begin(), + contacts_ref[i].end(), std::inserter(in_i_but_not_in_ref, in_i_but_not_in_ref.begin())); - if(!in_i_but_not_in_ref.empty()) { - for(std::size_t j = 0; j < in_i_but_not_in_ref.size(); ++j) { + if (!in_i_but_not_in_ref.empty()) { + for (std::size_t j = 0; j < in_i_but_not_in_ref.size(); ++j) { if (warn) { - BOOST_WARN_MESSAGE (false, "False contacts: " - << in_i_but_not_in_ref[j].b1 << ", " - << in_i_but_not_in_ref[j].b2); + BOOST_WARN_MESSAGE( + false, "False contacts: " << in_i_but_not_in_ref[j].b1 << ", " + << in_i_but_not_in_ref[j].b2); } else { - BOOST_CHECK_MESSAGE(false, "False contacts: " - << in_i_but_not_in_ref[j].b1 << ", " - << in_i_but_not_in_ref[j].b2); + BOOST_CHECK_MESSAGE( + false, "False contacts: " << in_i_but_not_in_ref[j].b1 << ", " + << in_i_but_not_in_ref[j].b2); } } } } } - template - void check () - { + template + void check() { if (benchmark) return; const std::size_t N = transforms.size(); - BOOST_REQUIRE_EQUAL(contacts.size(), 3*N); + BOOST_REQUIRE_EQUAL(contacts.size(), 3 * N); BOOST_REQUIRE_EQUAL(contacts.size(), contacts_ref.size()); if (traits::IS_IMPLEMENTED) { - BOOST_TEST_MESSAGE (getindent() << "BV: " << str() << " oriented"); + BOOST_TEST_MESSAGE(getindent() << "BV: " << str() << " oriented"); ++indent; - check_contacts (0, N, false); + check_contacts(0, N, false); --indent; } if (traits::IS_IMPLEMENTED) { - BOOST_TEST_MESSAGE (getindent() << "BV: " << str()); + BOOST_TEST_MESSAGE(getindent() << "BV: " << str()); ++indent; - check_contacts (N, 2*N, true); + check_contacts(N, 2 * N, true); --indent; } if (traits::IS_IMPLEMENTED) { - BOOST_TEST_MESSAGE (getindent() << "BV: " << str() << " oriented non-recursive"); + BOOST_TEST_MESSAGE(getindent() + << "BV: " << str() << " oriented non-recursive"); ++indent; - check_contacts (2*N, 3*N, false); + check_contacts(2 * N, 3 * N, false); --indent; } } - template - void operator() (wrap) - { - for (std::size_t i = 0; i < splitMethods.size(); ++i) - { - BOOST_TEST_MESSAGE (getindent() << "splitMethod: " << splitMethods[i]); + template + void operator()(wrap) { + for (std::size_t i = 0; i < splitMethods.size(); ++i) { + BOOST_TEST_MESSAGE(getindent() << "splitMethod: " << splitMethods[i]); ++indent; - query (transforms, splitMethods[i], request, (isInit ? contacts : contacts_ref)); - if (isInit) check (); + query(transforms, splitMethods[i], request, + (isInit ? contacts : contacts_ref)); + if (isInit) check(); isInit = true; --indent; } @@ -584,11 +593,10 @@ struct mesh_mesh_run_test // - collide_Test2 that moves all vertices of object1 in pose tf and that // calls function collide with identity for both object poses, // -BOOST_AUTO_TEST_CASE(mesh_mesh) -{ +BOOST_AUTO_TEST_CASE(mesh_mesh) { std::vector transforms; FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; -#ifndef NDEBUG // if debug mode +#ifndef NDEBUG // if debug mode std::size_t n = 1; #else std::size_t n = 10; @@ -597,22 +605,21 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) generateRandomTransforms(extents, transforms, n); - Eigen::IOFormat f (Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")"); - for(std::size_t i = 0; i < transforms.size(); ++i) - { - BOOST_TEST_MESSAGE("q" << i << "=" - << transforms [i].getTranslation () .format (f) << "+" - << transforms [i].getQuatRotation ().coeffs ().format (f)); + Eigen::IOFormat f(Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")"); + for (std::size_t i = 0; i < transforms.size(); ++i) { + BOOST_TEST_MESSAGE( + "q" << i << "=" << transforms[i].getTranslation().format(f) << "+" + << transforms[i].getQuatRotation().coeffs().format(f)); } // Request all contacts and check that all methods give the same result. - mesh_mesh_run_test runner (transforms, CollisionRequest (CONTACT, (size_t)num_max_contacts)); + mesh_mesh_run_test runner( + transforms, CollisionRequest(CONTACT, (size_t)num_max_contacts)); runner.enable_statistics = true; - boost::mpl::for_each > (runner); + boost::mpl::for_each >(runner); } -BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) -{ +BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) { std::vector transforms; FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG @@ -624,31 +631,33 @@ BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) generateRandomTransforms(extents, transforms, n); - Eigen::IOFormat f (Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")"); - for(std::size_t i = 0; i < transforms.size(); ++i) - { - BOOST_TEST_MESSAGE("q" << i << "=" - << transforms [i].getTranslation () .format (f) << "+" - << transforms [i].getQuatRotation ().coeffs ().format (f)); + Eigen::IOFormat f(Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")"); + for (std::size_t i = 0; i < transforms.size(); ++i) { + BOOST_TEST_MESSAGE( + "q" << i << "=" << transforms[i].getTranslation().format(f) << "+" + << transforms[i].getQuatRotation().coeffs().format(f)); } // Request all contacts and check that all methods give the same result. - typedef boost::mpl::vector, KDOP<18>, KDOP<16>, kIOS, OBBRSS> BVs_t; + typedef boost::mpl::vector, KDOP<18>, KDOP<16>, kIOS, + OBBRSS> + BVs_t; - std::ofstream ofs ("./collision.benchmark.csv", std::ofstream::out); + std::ofstream ofs("./collision.benchmark.csv", std::ofstream::out); bench_stream = &ofs; // without lower bound. - mesh_mesh_run_test runner1 (transforms, CollisionRequest ()); + mesh_mesh_run_test runner1(transforms, CollisionRequest()); runner1.enable_statistics = false; runner1.benchmark = true; - boost::mpl::for_each > (runner1); + boost::mpl::for_each >(runner1); // with lower bound. - mesh_mesh_run_test runner2 (transforms, CollisionRequest (DISTANCE_LOWER_BOUND, 1)); + mesh_mesh_run_test runner2(transforms, + CollisionRequest(DISTANCE_LOWER_BOUND, 1)); runner2.enable_statistics = false; runner2.benchmark = true; - boost::mpl::for_each > (runner2); + boost::mpl::for_each >(runner2); bench_stream = NULL; ofs.close(); diff --git a/test/convex.cpp b/test/convex.cpp index 3e3967e25..c7e15d217 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -34,7 +34,6 @@ /** \author Joseph Mirabel */ - #define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES #include @@ -46,38 +45,36 @@ using namespace hpp::fcl; -Convex buildBox (FCL_REAL l, FCL_REAL w, FCL_REAL d) -{ +Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) { Vec3f* pts = new Vec3f[8]; - pts[0] = Vec3f( l, w, d); - pts[1] = Vec3f( l, w,-d); - pts[2] = Vec3f( l,-w, d); - pts[3] = Vec3f( l,-w,-d); + pts[0] = Vec3f(l, w, d); + pts[1] = Vec3f(l, w, -d); + pts[2] = Vec3f(l, -w, d); + pts[3] = Vec3f(l, -w, -d); pts[4] = Vec3f(-l, w, d); - pts[5] = Vec3f(-l, w,-d); - pts[6] = Vec3f(-l,-w, d); - pts[7] = Vec3f(-l,-w,-d); + pts[5] = Vec3f(-l, w, -d); + pts[6] = Vec3f(-l, -w, d); + pts[7] = Vec3f(-l, -w, -d); Quadrilateral* polygons = new Quadrilateral[6]; - polygons[0].set(0, 2, 3, 1); // x+ side - polygons[1].set(2, 6, 7, 3); // y- side - polygons[2].set(4, 5, 7, 6); // x- side - polygons[3].set(0, 1, 5, 4); // y+ side - polygons[4].set(1, 3, 7, 5); // z- side - polygons[5].set(0, 2, 6, 4); // z+ side - - return Convex (true, - pts, // points - 8, // num points - polygons, - 6 // number of polygons - ); + polygons[0].set(0, 2, 3, 1); // x+ side + polygons[1].set(2, 6, 7, 3); // y- side + polygons[2].set(4, 5, 7, 6); // x- side + polygons[3].set(0, 1, 5, 4); // y+ side + polygons[4].set(1, 3, 7, 5); // z- side + polygons[5].set(0, 2, 6, 4); // z+ side + + return Convex(true, + pts, // points + 8, // num points + polygons, + 6 // number of polygons + ); } -BOOST_AUTO_TEST_CASE(convex) -{ +BOOST_AUTO_TEST_CASE(convex) { FCL_REAL l = 1, w = 1, d = 1; - Convex box (buildBox (l, w, d)); + Convex box(buildBox(l, w, d)); // Check neighbors for (int i = 0; i < 8; ++i) { @@ -116,12 +113,11 @@ BOOST_AUTO_TEST_CASE(convex) BOOST_CHECK_EQUAL(box.neighbors[7][2], 6); } -template void compareShapeIntersection ( - const Sa& sa, const Sb& sb, - const Transform3f& tf1, const Transform3f& tf2, - FCL_REAL tol = 1e-9) -{ - CollisionRequest request (CONTACT | DISTANCE_LOWER_BOUND, 1); +template +void compareShapeIntersection(const Sa& sa, const Sb& sb, + const Transform3f& tf1, const Transform3f& tf2, + FCL_REAL tol = 1e-9) { + CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); CollisionResult resA, resB; collide(&sa, tf1, &sa, tf2, request, resA); @@ -131,112 +127,106 @@ template void compareShapeIntersection ( BOOST_CHECK_EQUAL(resA.numContacts(), resB.numContacts()); if (resA.isCollision() && resB.isCollision()) { - Contact cA = resA.getContact(0), - cB = resB.getContact(0); - - BOOST_TEST_MESSAGE( - tf1 << '\n' - << cA.pos.format(pyfmt) << '\n' - << '\n' - << tf2 << '\n' - << cB.pos.format(pyfmt) << '\n' - ); + Contact cA = resA.getContact(0), cB = resB.getContact(0); + + BOOST_TEST_MESSAGE(tf1 << '\n' + << cA.pos.format(pyfmt) << '\n' + << '\n' + << tf2 << '\n' + << cB.pos.format(pyfmt) << '\n'); // Only warnings because there are still some bugs. - BOOST_WARN_SMALL((cA.pos - cB.pos ).squaredNorm(), tol); + BOOST_WARN_SMALL((cA.pos - cB.pos).squaredNorm(), tol); BOOST_WARN_SMALL((cA.normal - cB.normal).squaredNorm(), tol); } else { - BOOST_CHECK_CLOSE(resA.distance_lower_bound, resB.distance_lower_bound, tol); // distances should be same + BOOST_CHECK_CLOSE(resA.distance_lower_bound, resB.distance_lower_bound, + tol); // distances should be same } } -template void compareShapeDistance ( - const Sa& sa, const Sb& sb, - const Transform3f& tf1, const Transform3f& tf2, - FCL_REAL tol = 1e-9) -{ - DistanceRequest request (true); +template +void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1, + const Transform3f& tf2, FCL_REAL tol = 1e-9) { + DistanceRequest request(true); DistanceResult resA, resB; distance(&sa, tf1, &sa, tf2, request, resA); distance(&sb, tf1, &sb, tf2, request, resB); - BOOST_TEST_MESSAGE( - tf1 << '\n' - << resA.normal.format(pyfmt) << '\n' - << resA.nearest_points[0].format(pyfmt) << '\n' - << resA.nearest_points[1].format(pyfmt) << '\n' - << '\n' - << tf2 << '\n' - << resB.normal.format(pyfmt) << '\n' - << resB.nearest_points[0].format(pyfmt) << '\n' - << resB.nearest_points[1].format(pyfmt) << '\n' - ); - // TODO in one case, there is a mismatch between the distances and I cannot say - // which one is correct. To visualize the case, use script test/geometric_shapes.py + BOOST_TEST_MESSAGE(tf1 << '\n' + << resA.normal.format(pyfmt) << '\n' + << resA.nearest_points[0].format(pyfmt) << '\n' + << resA.nearest_points[1].format(pyfmt) << '\n' + << '\n' + << tf2 << '\n' + << resB.normal.format(pyfmt) << '\n' + << resB.nearest_points[0].format(pyfmt) << '\n' + << resB.nearest_points[1].format(pyfmt) << '\n'); + // TODO in one case, there is a mismatch between the distances and I cannot + // say which one is correct. To visualize the case, use script + // test/geometric_shapes.py BOOST_WARN_CLOSE(resA.min_distance, resB.min_distance, tol); - //BOOST_CHECK_CLOSE(resA.min_distance, resB.min_distance, tol); + // BOOST_CHECK_CLOSE(resA.min_distance, resB.min_distance, tol); // Only warnings because there are still some bugs. BOOST_WARN_SMALL((resA.normal - resA.normal).squaredNorm(), tol); - BOOST_WARN_SMALL((resA.nearest_points[0] - resB.nearest_points[0]).squaredNorm(), tol); - BOOST_WARN_SMALL((resA.nearest_points[1] - resB.nearest_points[1]).squaredNorm(), tol); + BOOST_WARN_SMALL( + (resA.nearest_points[0] - resB.nearest_points[0]).squaredNorm(), tol); + BOOST_WARN_SMALL( + (resA.nearest_points[1] - resB.nearest_points[1]).squaredNorm(), tol); } -BOOST_AUTO_TEST_CASE(compare_convex_box) -{ - FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10}; +BOOST_AUTO_TEST_CASE(compare_convex_box) { + FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; FCL_REAL l = 1, w = 1, d = 1, eps = 1e-4; - Box box(l*2, w*2, d*2); - Convex convex_box (buildBox (l, w, d)); + Box box(l * 2, w * 2, d * 2); + Convex convex_box(buildBox(l, w, d)); Transform3f tf1; Transform3f tf2; - tf2.setTranslation (Vec3f (3, 0, 0)); + tf2.setTranslation(Vec3f(3, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); - compareShapeDistance (box, convex_box, tf1, tf2, eps); + compareShapeDistance(box, convex_box, tf1, tf2, eps); - tf2.setTranslation (Vec3f (0, 0, 0)); + tf2.setTranslation(Vec3f(0, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); - compareShapeDistance (box, convex_box, tf1, tf2, eps); + compareShapeDistance(box, convex_box, tf1, tf2, eps); for (int i = 0; i < 1000; ++i) { generateRandomTransform(extents, tf2); compareShapeIntersection(box, convex_box, tf1, tf2, eps); - compareShapeDistance (box, convex_box, tf1, tf2, eps); + compareShapeDistance(box, convex_box, tf1, tf2, eps); } } #ifdef HPP_FCL_HAS_QHULL -BOOST_AUTO_TEST_CASE(convex_hull_throw) -{ - std::vector points ({ - Vec3f ( 1, 1, 1), - Vec3f ( 0, 0, 0), - Vec3f ( 1, 0, 0), - }); +BOOST_AUTO_TEST_CASE(convex_hull_throw) { + std::vector points({ + Vec3f(1, 1, 1), + Vec3f(0, 0, 0), + Vec3f(1, 0, 0), + }); BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 0, false, NULL), - std::invalid_argument); + std::invalid_argument); BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 1, false, NULL), - std::invalid_argument); + std::invalid_argument); BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 2, false, NULL), - std::invalid_argument); + std::invalid_argument); BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 3, false, NULL), - std::invalid_argument); + std::invalid_argument); } -BOOST_AUTO_TEST_CASE(convex_hull_quad) -{ - std::vector points ({ - Vec3f ( 1, 1, 1), - Vec3f ( 0, 0, 0), - Vec3f ( 1, 0, 0), - Vec3f ( 0, 0, 1), - }); +BOOST_AUTO_TEST_CASE(convex_hull_quad) { + std::vector points({ + Vec3f(1, 1, 1), + Vec3f(0, 0, 0), + Vec3f(1, 0, 0), + Vec3f(0, 0, 1), + }); - ConvexBase* convexHull = ConvexBase::convexHull(points.data(), (int)points.size(), - false, NULL); + ConvexBase* convexHull = + ConvexBase::convexHull(points.data(), (int)points.size(), false, NULL); BOOST_REQUIRE_EQUAL(convexHull->num_points, 4); BOOST_CHECK_EQUAL(convexHull->neighbors[0].count(), 3); @@ -245,41 +235,38 @@ BOOST_AUTO_TEST_CASE(convex_hull_quad) delete convexHull; } -BOOST_AUTO_TEST_CASE(convex_hull_box_like) -{ - std::vector points ({ - Vec3f ( 1, 1, 1), - Vec3f ( 1, 1,-1), - Vec3f ( 1,-1, 1), - Vec3f ( 1,-1,-1), - Vec3f (-1, 1, 1), - Vec3f (-1, 1,-1), - Vec3f (-1,-1, 1), - Vec3f (-1,-1,-1), - Vec3f ( 0, 0, 0 ), - Vec3f ( 0, 0, 0.99), - }); - - ConvexBase* convexHull = ConvexBase::convexHull(points.data(), (int)points.size(), - false, NULL); +BOOST_AUTO_TEST_CASE(convex_hull_box_like) { + std::vector points({ + Vec3f(1, 1, 1), + Vec3f(1, 1, -1), + Vec3f(1, -1, 1), + Vec3f(1, -1, -1), + Vec3f(-1, 1, 1), + Vec3f(-1, 1, -1), + Vec3f(-1, -1, 1), + Vec3f(-1, -1, -1), + Vec3f(0, 0, 0), + Vec3f(0, 0, 0.99), + }); + + ConvexBase* convexHull = + ConvexBase::convexHull(points.data(), (int)points.size(), false, NULL); BOOST_REQUIRE_EQUAL(8, convexHull->num_points); - for (int i = 0; i < 8; ++i) - { - BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1,1,1)); + for (int i = 0; i < 8; ++i) { + BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1, 1, 1)); BOOST_CHECK_EQUAL(convexHull->neighbors[i].count(), 3); } delete convexHull; - convexHull = ConvexBase::convexHull(points.data(), (int)points.size(), - true, NULL); - Convex* convex_tri = dynamic_cast*> (convexHull); + convexHull = + ConvexBase::convexHull(points.data(), (int)points.size(), true, NULL); + Convex* convex_tri = dynamic_cast*>(convexHull); BOOST_CHECK(convex_tri != NULL); BOOST_REQUIRE_EQUAL(8, convexHull->num_points); - for (int i = 0; i < 8; ++i) - { - BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1,1,1)); + for (int i = 0; i < 8; ++i) { + BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1, 1, 1)); BOOST_CHECK(convexHull->neighbors[i].count() >= 3); BOOST_CHECK(convexHull->neighbors[i].count() <= 6); } diff --git a/test/distance.cpp b/test/distance.cpp index d4a23af63..ed365417a 100644 --- a/test/distance.cpp +++ b/test/distance.cpp @@ -55,48 +55,47 @@ namespace utf = boost::unit_test::framework; bool verbose = false; FCL_REAL DELTA = 0.001; - -template -void distance_Test(const Transform3f& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, - unsigned int qsize, - DistanceRes& distance_result, - bool verbose = true); +template +void distance_Test(const Transform3f& tf, const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, unsigned int qsize, + DistanceRes& distance_result, bool verbose = true); bool collide_Test_OBB(const Transform3f& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); - + const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, bool verbose); -template +template void distance_Test_Oriented(const Transform3f& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, - unsigned int qsize, - DistanceRes& distance_result, - bool verbose = true); - -inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) -{ - if(fabs(a[0] - b[0]) > DELTA) return false; - if(fabs(a[1] - b[1]) > DELTA) return false; - if(fabs(a[2] - b[2]) > DELTA) return false; + const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, unsigned int qsize, + DistanceRes& distance_result, bool verbose = true); + +inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) { + if (fabs(a[0] - b[0]) > DELTA) return false; + if (fabs(a[1] - b[1]) > DELTA) return false; + if (fabs(a[2] - b[2]) > DELTA) return false; return true; } - -BOOST_AUTO_TEST_CASE(mesh_distance) -{ +BOOST_AUTO_TEST_CASE(mesh_distance) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); - std::vector transforms; // t0 + std::vector transforms; // t0 FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; -#ifndef NDEBUG // if debug mode +#ifndef NDEBUG // if debug mode std::size_t n = 1; #else std::size_t n = 10; @@ -109,243 +108,378 @@ BOOST_AUTO_TEST_CASE(mesh_distance) double col_time = 0; DistanceRes res, res_now; - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { auto timer_col = std::chrono::high_resolution_clock::now(); collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - col_time += std::chrono::duration_cast>(std::chrono::high_resolution_clock::now() - timer_col).count(); + col_time += std::chrono::duration_cast>( + std::chrono::high_resolution_clock::now() - timer_col) + .count(); auto timer_dist = std::chrono::high_resolution_clock::now(); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose); - dis_time += std::chrono::duration_cast>(std::chrono::high_resolution_clock::now() - timer_dist).count(); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose); + dis_time += std::chrono::duration_cast>( + std::chrono::high_resolution_clock::now() - timer_dist) + .count(); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, + verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, + verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, + verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, + verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); - - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, + verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, + verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, + verbose); + BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, + verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, + verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, + verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); - BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, + verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + distance_Test_Oriented( + transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, + verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, + res_now, verbose); + BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, + res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, + 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); - BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); + + distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + 20, res_now, verbose); + BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); + BOOST_CHECK(fabs(res.distance) < DELTA || + (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && + nearlyEqual(res.p2, res_now.p2))); } BOOST_TEST_MESSAGE("distance timing: " << dis_time << " sec"); BOOST_TEST_MESSAGE("collision timing: " << col_time << " sec"); } -template +template void distance_Test_Oriented(const Transform3f& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, - unsigned int qsize, - DistanceRes& distance_result, - bool verbose) -{ + const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, unsigned int qsize, + DistanceRes& distance_result, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); - m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); @@ -356,7 +490,8 @@ void distance_Test_Oriented(const Transform3f& tf, DistanceResult local_result; TraversalNode node; - if(!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, Transform3f(), DistanceRequest(true), local_result)) + if (!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, + Transform3f(), DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -367,13 +502,11 @@ void distance_Test_Oriented(const Transform3f& tf, Vec3f p1 = local_result.nearest_points[0]; Vec3f p2 = local_result.nearest_points[1]; - distance_result.distance = local_result.min_distance; distance_result.p1 = p1; distance_result.p2 = p2; - if(verbose) - { + if (verbose) { std::cout << "distance " << local_result.min_distance << std::endl; std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl; @@ -382,20 +515,18 @@ void distance_Test_Oriented(const Transform3f& tf, } } -template -void distance_Test(const Transform3f& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, - unsigned int qsize, - DistanceRes& distance_result, - bool verbose) -{ +template +void distance_Test(const Transform3f& tf, const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, unsigned int qsize, + DistanceRes& distance_result, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); - m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); @@ -409,7 +540,8 @@ void distance_Test(const Transform3f& tf, DistanceResult local_result; MeshDistanceTraversalNode node; - if(!initialize(node, m1, pose1, m2, pose2, DistanceRequest(true), local_result)) + if (!initialize(node, m1, pose1, m2, pose2, DistanceRequest(true), + local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -420,21 +552,25 @@ void distance_Test(const Transform3f& tf, distance_result.p1 = local_result.nearest_points[0]; distance_result.p2 = local_result.nearest_points[1]; - if(verbose) - { + if (verbose) { std::cout << "distance " << local_result.min_distance << std::endl; - std::cout << local_result.nearest_points[0][0] << " " << local_result.nearest_points[0][1] << " " << local_result.nearest_points[0][2] << std::endl; - std::cout << local_result.nearest_points[1][0] << " " << local_result.nearest_points[1][1] << " " << local_result.nearest_points[1][2] << std::endl; + std::cout << local_result.nearest_points[0][0] << " " + << local_result.nearest_points[0][1] << " " + << local_result.nearest_points[0][2] << std::endl; + std::cout << local_result.nearest_points[1][0] << " " + << local_result.nearest_points[1][1] << " " + << local_result.nearest_points[1][2] << std::endl; std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; } } - bool collide_Test_OBB(const Transform3f& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) -{ + const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -449,21 +585,19 @@ bool collide_Test_OBB(const Transform3f& tf, m2.endModel(); CollisionResult local_result; - CollisionRequest request (CONTACT | DISTANCE_LOWER_BOUND, 1); - MeshCollisionTraversalNodeOBB node (request); - bool success (initialize(node, (const BVHModel&)m1, tf, - (const BVHModel&)m2, Transform3f(), - local_result)); - BOOST_REQUIRE (success); + CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); + MeshCollisionTraversalNodeOBB node(request); + bool success(initialize(node, (const BVHModel&)m1, tf, + (const BVHModel&)m2, Transform3f(), + local_result)); + BOOST_REQUIRE(success); node.enable_statistics = verbose; collide(&node, request, local_result); - if(local_result.numContacts() > 0) + if (local_result.numContacts() > 0) return true; else return false; } - - diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp index 58fc6a583..e5fd4f0bc 100644 --- a/test/distance_lower_bound.cpp +++ b/test/distance_lower_bound.cpp @@ -34,7 +34,7 @@ #define BOOST_TEST_MODULE FCL_DISTANCE_LOWER_BOUND #include -# include +#include #include #include @@ -43,82 +43,77 @@ #include #include #include -# include "utility.h" -# include "fcl_resources/config.h" +#include "utility.h" +#include "fcl_resources/config.h" -using hpp::fcl::shared_ptr; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; -using hpp::fcl::Triangle; -using hpp::fcl::OBBRSS; using hpp::fcl::BVHModel; -using hpp::fcl::CollisionResult; +using hpp::fcl::CollisionGeometryPtr_t; +using hpp::fcl::CollisionObject; using hpp::fcl::CollisionRequest; +using hpp::fcl::CollisionResult; using hpp::fcl::DistanceRequest; using hpp::fcl::DistanceResult; -using hpp::fcl::CollisionObject; -using hpp::fcl::CollisionGeometryPtr_t; using hpp::fcl::FCL_REAL; +using hpp::fcl::OBBRSS; +using hpp::fcl::shared_ptr; +using hpp::fcl::Transform3f; +using hpp::fcl::Triangle; +using hpp::fcl::Vec3f; -bool testDistanceLowerBound (const Transform3f& tf, - const CollisionGeometryPtr_t& m1, - const CollisionGeometryPtr_t& m2, - FCL_REAL& distance) -{ +bool testDistanceLowerBound(const Transform3f& tf, + const CollisionGeometryPtr_t& m1, + const CollisionGeometryPtr_t& m2, + FCL_REAL& distance) { Transform3f pose1(tf), pose2; - CollisionRequest request (hpp::fcl::NO_REQUEST, 1); + CollisionRequest request(hpp::fcl::NO_REQUEST, 1); request.enable_distance_lower_bound = true; CollisionResult result; - CollisionObject co1 (m1, pose1); - CollisionObject co2 (m2, pose2); + CollisionObject co1(m1, pose1); + CollisionObject co2(m2, pose2); hpp::fcl::collide(&co1, &co2, request, result); distance = result.distance_lower_bound; - return result.isCollision (); + return result.isCollision(); } -bool testCollide (const Transform3f& tf, const CollisionGeometryPtr_t& m1, - const CollisionGeometryPtr_t& m2) -{ +bool testCollide(const Transform3f& tf, const CollisionGeometryPtr_t& m1, + const CollisionGeometryPtr_t& m2) { Transform3f pose1(tf), pose2; - CollisionRequest request (hpp::fcl::NO_REQUEST, 1); + CollisionRequest request(hpp::fcl::NO_REQUEST, 1); request.enable_distance_lower_bound = false; CollisionResult result; - CollisionObject co1 (m1, pose1); - CollisionObject co2 (m2, pose2); + CollisionObject co1(m1, pose1); + CollisionObject co2(m2, pose2); hpp::fcl::collide(&co1, &co2, request, result); - return result.isCollision (); + return result.isCollision(); } -bool testDistance (const Transform3f& tf, const CollisionGeometryPtr_t& m1, - const CollisionGeometryPtr_t& m2, FCL_REAL& distance) -{ +bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1, + const CollisionGeometryPtr_t& m2, FCL_REAL& distance) { Transform3f pose1(tf), pose2; DistanceRequest request; DistanceResult result; - CollisionObject co1 (m1, pose1); - CollisionObject co2 (m2, pose2); + CollisionObject co1(m1, pose1); + CollisionObject co2(m2, pose2); - hpp::fcl::distance (&co1, &co2, request, result); + hpp::fcl::distance(&co1, &co2, request, result); distance = result.min_distance; - if(result.min_distance <= 0) { + if (result.min_distance <= 0) { return true; - } - else { + } else { return false; } } -BOOST_AUTO_TEST_CASE(mesh_mesh) -{ +BOOST_AUTO_TEST_CASE(mesh_mesh) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -126,8 +121,8 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); - shared_ptr < BVHModel > m1 (new BVHModel ); - shared_ptr < BVHModel > m2 (new BVHModel ); + shared_ptr > m1(new BVHModel); + shared_ptr > m2(new BVHModel); m1->beginModel(); m1->addSubModel(p1, t1); @@ -144,108 +139,108 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) generateRandomTransforms(extents, transforms, n); // collision - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { FCL_REAL distanceLowerBound, distance; bool col1, col2, col3; - col1 = testDistanceLowerBound (transforms[i], m1, m2, distanceLowerBound); - col2 = testDistance (transforms[i], m1, m2, distance); - col3 = testCollide (transforms[i], m1, m2); - std::cout << "col1 = " << col1 << ", col2 = " << col2 - << ", col3 = " << col3 << std::endl; + col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound); + col2 = testDistance(transforms[i], m1, m2, distance); + col3 = testCollide(transforms[i], m1, m2); + std::cout << "col1 = " << col1 << ", col2 = " << col2 << ", col3 = " << col3 + << std::endl; std::cout << "distance lower bound = " << distanceLowerBound << std::endl; std::cout << "distance = " << distance << std::endl; - BOOST_CHECK (col1 == col3); + BOOST_CHECK(col1 == col3); if (!col1) { - BOOST_CHECK_MESSAGE (distanceLowerBound <= distance, - "distance = " << distance << ", lower bound = " - << distanceLowerBound); + BOOST_CHECK_MESSAGE(distanceLowerBound <= distance, + "distance = " << distance << ", lower bound = " + << distanceLowerBound); } } } -BOOST_AUTO_TEST_CASE(box_sphere) -{ - shared_ptr < hpp::fcl::Sphere > sphere(new hpp::fcl::Sphere(0.5)); - shared_ptr < hpp::fcl::Box > box(new hpp::fcl::Box(1.,1.,1.)); - - Transform3f M1; M1.setIdentity(); - Transform3f M2; M2.setIdentity(); - +BOOST_AUTO_TEST_CASE(box_sphere) { + shared_ptr sphere(new hpp::fcl::Sphere(0.5)); + shared_ptr box(new hpp::fcl::Box(1., 1., 1.)); + + Transform3f M1; + M1.setIdentity(); + Transform3f M2; + M2.setIdentity(); + std::vector transforms; FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; generateRandomTransforms(extents, transforms, n); - + FCL_REAL distanceLowerBound, distance; bool col1, col2; - col1 = testDistanceLowerBound(M1,sphere,box,distanceLowerBound); - col2 = testDistance(M1,sphere,box,distance); + col1 = testDistanceLowerBound(M1, sphere, box, distanceLowerBound); + col2 = testDistance(M1, sphere, box, distance); BOOST_CHECK(col1 == col2); BOOST_CHECK(distanceLowerBound <= distance); - - for(std::size_t i = 0; i < transforms.size(); ++i) - { + + for (std::size_t i = 0; i < transforms.size(); ++i) { FCL_REAL distanceLowerBound, distance; bool col1, col2; - col1 = testDistanceLowerBound (transforms[i], sphere, box, distanceLowerBound); - col2 = testDistance (transforms[i], sphere, box, distance); - BOOST_CHECK (col1 == col2); + col1 = + testDistanceLowerBound(transforms[i], sphere, box, distanceLowerBound); + col2 = testDistance(transforms[i], sphere, box, distance); + BOOST_CHECK(col1 == col2); if (!col1) { - BOOST_CHECK_MESSAGE (distanceLowerBound <= distance, - "distance = " << distance << ", lower bound = " - << distanceLowerBound); + BOOST_CHECK_MESSAGE(distanceLowerBound <= distance, + "distance = " << distance << ", lower bound = " + << distanceLowerBound); } } } -BOOST_AUTO_TEST_CASE(sphere_sphere) -{ - shared_ptr < hpp::fcl::Sphere > sphere1(new hpp::fcl::Sphere(0.5)); - shared_ptr < hpp::fcl::Sphere > sphere2(new hpp::fcl::Sphere(1.)); - - Transform3f M1; M1.setIdentity(); - Transform3f M2; M2.setIdentity(); - +BOOST_AUTO_TEST_CASE(sphere_sphere) { + shared_ptr sphere1(new hpp::fcl::Sphere(0.5)); + shared_ptr sphere2(new hpp::fcl::Sphere(1.)); + + Transform3f M1; + M1.setIdentity(); + Transform3f M2; + M2.setIdentity(); + std::vector transforms; FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; generateRandomTransforms(extents, transforms, n); - + FCL_REAL distanceLowerBound, distance; bool col1, col2; - col1 = testDistanceLowerBound(M1,sphere1,sphere2,distanceLowerBound); - col2 = testDistance(M1,sphere1,sphere2,distance); + col1 = testDistanceLowerBound(M1, sphere1, sphere2, distanceLowerBound); + col2 = testDistance(M1, sphere1, sphere2, distance); BOOST_CHECK(col1 == col2); BOOST_CHECK(distanceLowerBound <= distance); - - for(std::size_t i = 0; i < transforms.size(); ++i) - { + + for (std::size_t i = 0; i < transforms.size(); ++i) { FCL_REAL distanceLowerBound, distance; bool col1, col2; - col1 = testDistanceLowerBound (transforms[i], sphere1, sphere2, distanceLowerBound); - col2 = testDistance (transforms[i], sphere1, sphere2, distance); - BOOST_CHECK (col1 == col2); + col1 = testDistanceLowerBound(transforms[i], sphere1, sphere2, + distanceLowerBound); + col2 = testDistance(transforms[i], sphere1, sphere2, distance); + BOOST_CHECK(col1 == col2); if (!col1) { - BOOST_CHECK_MESSAGE (distanceLowerBound <= distance, - "distance = " << distance << ", lower bound = " - << distanceLowerBound); + BOOST_CHECK_MESSAGE(distanceLowerBound <= distance, + "distance = " << distance << ", lower bound = " + << distanceLowerBound); } } } -BOOST_AUTO_TEST_CASE(box_mesh) -{ +BOOST_AUTO_TEST_CASE(box_mesh) { std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); - shared_ptr < BVHModel > m1 (new BVHModel ); - shared_ptr < hpp::fcl::Box > m2 (new hpp::fcl::Box (500, 200, 150)); + shared_ptr > m1(new BVHModel); + shared_ptr m2(new hpp::fcl::Box(500, 200, 150)); m1->beginModel(); m1->addSubModel(p1, t1); @@ -258,17 +253,16 @@ BOOST_AUTO_TEST_CASE(box_mesh) generateRandomTransforms(extents, transforms, n); // collision - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { FCL_REAL distanceLowerBound, distance; bool col1, col2; - col1 = testDistanceLowerBound (transforms[i], m1, m2, distanceLowerBound); - col2 = testDistance (transforms[i], m1, m2, distance); - BOOST_CHECK (col1 == col2); + col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound); + col2 = testDistance(transforms[i], m1, m2, distance); + BOOST_CHECK(col1 == col2); if (!col1) { - BOOST_CHECK_MESSAGE (distanceLowerBound <= distance, - "distance = " << distance << ", lower bound = " - << distanceLowerBound); + BOOST_CHECK_MESSAGE(distanceLowerBound <= distance, + "distance = " << distance << ", lower bound = " + << distanceLowerBound); } } } diff --git a/test/frontlist.cpp b/test/frontlist.cpp index ab5a0124f..724144faa 100644 --- a/test/frontlist.cpp +++ b/test/frontlist.cpp @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #define BOOST_TEST_MODULE FCL_FRONT_LIST #include @@ -51,39 +50,45 @@ using namespace hpp::fcl; namespace utf = boost::unit_test::framework; -template +template bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, - SplitMethodType split_method, - bool refit_bottomup, bool verbose); - -template -bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, - SplitMethodType split_method, bool verbose); - - -template -bool collide_Test(const Transform3f& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); + const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, bool refit_bottomup, + bool verbose); + +template +bool collide_front_list_Test_Oriented(const Transform3f& tf1, + const Transform3f& tf2, + const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, + bool verbose); + +template +bool collide_Test(const Transform3f& tf, const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, bool verbose); // TODO: randomly still have some runtime error -BOOST_AUTO_TEST_CASE(front_list) -{ +BOOST_AUTO_TEST_CASE(front_list) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); - std::vector transforms; // t0 - std::vector transforms2; // t1 + std::vector transforms; // t0 + std::vector transforms2; // t1 FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; FCL_REAL delta_trans[] = {1, 1, 1}; -#ifndef NDEBUG // if debug mode +#ifndef NDEBUG // if debug mode std::size_t n = 2; #else std::size_t n = 20; @@ -91,124 +96,187 @@ BOOST_AUTO_TEST_CASE(front_list) n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n); bool verbose = false; - generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n); + generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, + transforms2, n); bool res, res2; - for(std::size_t i = 0; i < transforms.size(); ++i) - { - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + for (std::size_t i = 0; i < transforms.size(); ++i) { + res = collide_Test(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_MEDIAN, verbose); + res2 = + collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, + t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, + verbose); + res2 = + collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, + t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, + p2, t2, SPLIT_METHOD_BV_CENTER, false, + verbose); BOOST_CHECK(res == res2); } - for(std::size_t i = 0; i < transforms.size(); ++i) - { - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + for (std::size_t i = 0; i < transforms.size(); ++i) { + res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, + verbose); + res2 = + collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, + t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, + verbose); + res2 = + collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, + t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, + p2, t2, SPLIT_METHOD_BV_CENTER, false, + verbose); BOOST_CHECK(res == res2); } - for(std::size_t i = 0; i < transforms.size(); ++i) - { + for (std::size_t i = 0; i < transforms.size(); ++i) { // Disabled broken test lines. Please see #25. - // res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - // res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); - // BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + // res = collide_Test(transforms2[i], p1, t1, p2, t2, + // SPLIT_METHOD_MEDIAN, verbose); res2 = + // collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, + // t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); + res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, + verbose); + res2 = + collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, + t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, + p2, t2, SPLIT_METHOD_BV_CENTER, false, + verbose); BOOST_CHECK(res == res2); } - for(std::size_t i = 0; i < transforms.size(); ++i) - { - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + for (std::size_t i = 0; i < transforms.size(); ++i) { + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, + t1, p2, t2, SPLIT_METHOD_MEDIAN, + false, verbose); BOOST_CHECK(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, + t1, p2, t2, SPLIT_METHOD_MEAN, + false, verbose); BOOST_CHECK(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test >( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + false, verbose); BOOST_CHECK(res == res2); } - for(std::size_t i = 0; i < transforms.size(); ++i) - { - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + for (std::size_t i = 0; i < transforms.size(); ++i) { + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, + t1, p2, t2, SPLIT_METHOD_MEDIAN, + false, verbose); BOOST_CHECK(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, + t1, p2, t2, SPLIT_METHOD_MEAN, + false, verbose); BOOST_CHECK(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test >( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + false, verbose); BOOST_CHECK(res == res2); } - for(std::size_t i = 0; i < transforms.size(); ++i) - { - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + for (std::size_t i = 0; i < transforms.size(); ++i) { + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, + t1, p2, t2, SPLIT_METHOD_MEDIAN, + false, verbose); BOOST_CHECK(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, + t1, p2, t2, SPLIT_METHOD_MEAN, + false, verbose); BOOST_CHECK(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test >( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + false, verbose); BOOST_CHECK(res == res2); } - for(std::size_t i = 0; i < transforms.size(); ++i) - { - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + for (std::size_t i = 0; i < transforms.size(); ++i) { + res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, + verbose); + res2 = collide_front_list_Test_Oriented( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, + verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, + verbose); + res2 = collide_front_list_Test_Oriented( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, + verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test_Oriented( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + verbose); BOOST_CHECK(res == res2); } - for(std::size_t i = 0; i < transforms.size(); ++i) - { - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + for (std::size_t i = 0; i < transforms.size(); ++i) { + res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, + verbose); + res2 = collide_front_list_Test_Oriented( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, + verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, + verbose); + res2 = collide_front_list_Test_Oriented( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, + verbose); BOOST_CHECK(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res = collide_Test(transforms2[i], p1, t1, p2, t2, + SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test_Oriented( + transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, + verbose); BOOST_CHECK(res == res2); } - } -template +template bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, - SplitMethodType split_method, - bool refit_bottomup, bool verbose) -{ + const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, bool refit_bottomup, + bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -216,10 +284,8 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, BVHFrontList front_list; - std::vector vertices1_new(vertices1.size()); - for(std::size_t i = 0; i < vertices1_new.size(); ++i) - { + for (std::size_t i = 0; i < vertices1_new.size(); ++i) { vertices1_new[i] = tf1.transform(vertices1[i]); } @@ -234,22 +300,21 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, Transform3f pose1, pose2; CollisionResult local_result; - CollisionRequest request (NO_REQUEST, (std::numeric_limits::max)()); - MeshCollisionTraversalNode node (request); + CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); + MeshCollisionTraversalNode node(request); - bool success = initialize (node, m1, pose1, m2, pose2, local_result); - BOOST_REQUIRE (success); + bool success = initialize(node, m1, pose1, m2, pose2, local_result); + BOOST_REQUIRE(success); node.enable_statistics = verbose; collide(&node, request, local_result, &front_list); - if(verbose) std::cout << "front list size " << front_list.size() << std::endl; - + if (verbose) + std::cout << "front list size " << front_list.size() << std::endl; // update the mesh - for(std::size_t i = 0; i < vertices1.size(); ++i) - { + for (std::size_t i = 0; i < vertices1.size(); ++i) { vertices1_new[i] = tf2.transform(vertices1[i]); } @@ -264,21 +329,21 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, local_result.clear(); collide(&node, request, local_result, &front_list); - if(local_result.numContacts() > 0) + if (local_result.numContacts() > 0) return true; else return false; } - - - -template -bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, - SplitMethodType split_method, bool verbose) -{ +template +bool collide_front_list_Test_Oriented(const Transform3f& tf1, + const Transform3f& tf2, + const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, + bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -296,42 +361,42 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& Transform3f pose1(tf1), pose2; - CollisionResult local_result; - CollisionRequest request (NO_REQUEST, (std::numeric_limits::max)()); - TraversalNode node (request); + CollisionResult local_result; + CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); + TraversalNode node(request); - bool success = initialize (node, (const BVHModel&)m1, pose1, - (const BVHModel&)m2, pose2, local_result); - BOOST_REQUIRE (success); + bool success = initialize(node, (const BVHModel&)m1, pose1, + (const BVHModel&)m2, pose2, local_result); + BOOST_REQUIRE(success); node.enable_statistics = verbose; collide(&node, request, local_result, &front_list); - if(verbose) std::cout << "front list size " << front_list.size() << std::endl; - + if (verbose) + std::cout << "front list size " << front_list.size() << std::endl; // update the mesh pose1 = tf2; - success = initialize (node, (const BVHModel&)m1, pose1, - (const BVHModel&)m2, pose2, local_result); - BOOST_REQUIRE (success); + success = initialize(node, (const BVHModel&)m1, pose1, + (const BVHModel&)m2, pose2, local_result); + BOOST_REQUIRE(success); local_result.clear(); collide(&node, request, local_result, &front_list); - if(local_result.numContacts() > 0) + if (local_result.numContacts() > 0) return true; else return false; } - -template -bool collide_Test(const Transform3f& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) -{ +template +bool collide_Test(const Transform3f& tf, const std::vector& vertices1, + const std::vector& triangles1, + const std::vector& vertices2, + const std::vector& triangles2, + SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -348,19 +413,18 @@ bool collide_Test(const Transform3f& tf, Transform3f pose1(tf), pose2; CollisionResult local_result; - CollisionRequest request (NO_REQUEST, (std::numeric_limits::max)()); - MeshCollisionTraversalNode node (request); + CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); + MeshCollisionTraversalNode node(request); - bool success = initialize (node, m1, pose1, m2, pose2, local_result); - BOOST_REQUIRE (success); + bool success = initialize(node, m1, pose1, m2, pose2, local_result); + BOOST_REQUIRE(success); node.enable_statistics = verbose; collide(&node, request, local_result); - if(local_result.numContacts() > 0) + if (local_result.numContacts() > 0) return true; else return false; } - diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp index 8ecf79c3c..d909e6e21 100644 --- a/test/geometric_shapes.cpp +++ b/test/geometric_shapes.cpp @@ -35,7 +35,6 @@ /** \author Jia Pan */ - #define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES #include @@ -49,7 +48,7 @@ using namespace hpp::fcl; -FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10}; +FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; FCL_REAL tol_gjk = 0.01; GJKSolver solver1; @@ -57,66 +56,65 @@ GJKSolver solver2; int line; #define SET_LINE line = __LINE__ -#define FCL_CHECK(cond) BOOST_CHECK_MESSAGE(cond, "from line " << line << ": " #cond) -#define FCL_CHECK_EQUAL(a,b) \ - BOOST_CHECK_MESSAGE((a) == (b), \ - "from line " << line << ": " #a "[" << (a) << "] != " #b "[" << (b) << "].") +#define FCL_CHECK(cond) \ + BOOST_CHECK_MESSAGE(cond, "from line " << line << ": " #cond) +#define FCL_CHECK_EQUAL(a, b) \ + BOOST_CHECK_MESSAGE((a) == (b), "from line " << line << ": " #a "[" << (a) \ + << "] != " #b "[" << (b) \ + << "].") #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) namespace hpp { namespace fcl { -std::ostream& operator<< (std::ostream& os, const ShapeBase&) -{ +std::ostream& operator<<(std::ostream& os, const ShapeBase&) { return os << "a_shape"; } -std::ostream& operator<< (std::ostream& os, const Box& b) -{ - return os << "Box(" << 2*b.halfSide.transpose() << ')'; -} -} +std::ostream& operator<<(std::ostream& os, const Box& b) { + return os << "Box(" << 2 * b.halfSide.transpose() << ')'; } +} // namespace fcl +} // namespace hpp template -void printComparisonError(const std::string& comparison_type, - const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, +void printComparisonError(const std::string& comparison_type, const S1& s1, + const Transform3f& tf1, const S2& s2, + const Transform3f& tf2, const Vec3f& contact_or_normal, const Vec3f& expected_contact_or_normal, - bool check_opposite_normal, - FCL_REAL tol) -{ - std::cout << "Disagreement between " << comparison_type - << " and expected_" << comparison_type << " for " - << getNodeTypeName(s1.getNodeType()) << " and " - << getNodeTypeName(s2.getNodeType()) << ".\n" + bool check_opposite_normal, FCL_REAL tol) { + std::cout << "Disagreement between " << comparison_type << " and expected_" + << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) + << " and " << getNodeTypeName(s2.getNodeType()) << ".\n" << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl - << "tf1.translation: " << tf1.getTranslation().transpose() << std::endl + << "tf1.translation: " << tf1.getTranslation().transpose() + << std::endl << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl - << "tf2.translation: " << tf2.getTranslation().transpose() << std::endl - << comparison_type << ": " << contact_or_normal.transpose() << std::endl - << "expected_" << comparison_type << ": " << expected_contact_or_normal.transpose(); + << "tf2.translation: " << tf2.getTranslation().transpose() + << std::endl + << comparison_type << ": " << contact_or_normal.transpose() + << std::endl + << "expected_" << comparison_type << ": " + << expected_contact_or_normal.transpose(); if (check_opposite_normal) std::cout << " or " << -expected_contact_or_normal.transpose(); std::cout << std::endl - << "difference: " << (contact_or_normal - expected_contact_or_normal).norm() << std::endl + << "difference: " + << (contact_or_normal - expected_contact_or_normal).norm() + << std::endl << "tolerance: " << tol << std::endl; } template -void printComparisonError(const std::string& comparison_type, - const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - 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()) << ".\n" +void printComparisonError(const std::string& comparison_type, const S1& s1, + const Transform3f& tf1, const S2& s2, + const Transform3f& tf2, 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()) << ".\n" << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl << "tf1.translation: " << tf1.getTranslation() << std::endl << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl @@ -128,31 +126,29 @@ void printComparisonError(const std::string& comparison_type, } template -void compareContact(const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - 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) - { +void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2, + const Transform3f& tf2, 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 = isEqual(contact, *expected_point, tol); FCL_CHECK(contact_equal); if (!contact_equal) - printComparisonError("contact", s1, tf1, s2, tf2, contact, *expected_point, false, tol); + printComparisonError("contact", s1, tf1, s2, tf2, contact, + *expected_point, false, tol); } - if (expected_depth) - { + if (expected_depth) { bool depth_equal = std::fabs(depth - *expected_depth) < tol; FCL_CHECK(depth_equal); if (!depth_equal) - printComparisonError("depth", s1, tf1, s2, tf2, depth, *expected_depth, tol); + printComparisonError("depth", s1, tf1, s2, tf2, depth, *expected_depth, + tol); } - if (expected_normal) - { + if (expected_normal) { bool normal_equal = isEqual(normal, *expected_normal, tol); if (!normal_equal && check_opposite_normal) @@ -160,20 +156,19 @@ void compareContact(const S1& s1, const Transform3f& tf1, FCL_CHECK(normal_equal); if (!normal_equal) - printComparisonError("normal", s1, tf1, s2, tf2, normal, *expected_normal, check_opposite_normal, tol); + printComparisonError("normal", s1, tf1, s2, tf2, normal, *expected_normal, + check_opposite_normal, tol); } } template -void testShapeIntersection(const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - bool expect_collision, +void testShapeIntersection(const S1& s1, const Transform3f& tf1, const S2& s2, + const Transform3f& tf2, bool expect_collision, Vec3f* expected_point = NULL, FCL_REAL* expected_depth = NULL, Vec3f* expected_normal = NULL, bool check_opposite_normal = false, - FCL_REAL tol = 1e-9) -{ + FCL_REAL tol = 1e-9) { CollisionRequest request; CollisionResult result; @@ -195,115 +190,107 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1, check_failed = check_failed || (collision != expect_collision); if (check_failed) { - BOOST_TEST_MESSAGE("Failure occured between " << s1 << " and " << s2 << " at transformations\n" - << tf1 << '\n' << tf2); + BOOST_TEST_MESSAGE("Failure occured between " << s1 << " and " << s2 + << " at transformations\n" + << tf1 << '\n' + << tf2); } - if (expect_collision) - { + if (expect_collision) { FCL_CHECK_EQUAL(result.numContacts(), 1); - if (result.numContacts() == 1) - { + if (result.numContacts() == 1) { Contact contact = result.getContact(0); - compareContact(s1, tf1, s2, tf2, contact.pos, expected_point, contact.penetration_depth, expected_depth, contact.normal, expected_normal, check_opposite_normal, tol); + compareContact(s1, tf1, s2, tf2, contact.pos, expected_point, + contact.penetration_depth, expected_depth, contact.normal, + expected_normal, check_opposite_normal, tol); } } } -BOOST_AUTO_TEST_CASE (box_to_bvh) -{ - Box shape (1,1,1); +BOOST_AUTO_TEST_CASE(box_to_bvh) { + Box shape(1, 1, 1); BVHModel bvh; - generateBVHModel (bvh, shape, Transform3f()); + generateBVHModel(bvh, shape, Transform3f()); } -BOOST_AUTO_TEST_CASE (sphere_to_bvh) -{ - Sphere shape (1); +BOOST_AUTO_TEST_CASE(sphere_to_bvh) { + Sphere shape(1); BVHModel bvh; - generateBVHModel (bvh, shape, Transform3f(), 10, 10); - generateBVHModel (bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3f(), 10, 10); + generateBVHModel(bvh, shape, Transform3f(), 50); } -BOOST_AUTO_TEST_CASE (cylinder_to_bvh) -{ - Cylinder shape (1,1); +BOOST_AUTO_TEST_CASE(cylinder_to_bvh) { + Cylinder shape(1, 1); BVHModel bvh; - generateBVHModel (bvh, shape, Transform3f(), 10, 10); - generateBVHModel (bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3f(), 10, 10); + generateBVHModel(bvh, shape, Transform3f(), 50); } -BOOST_AUTO_TEST_CASE (cone_to_bvh) -{ - Cone shape (1,1); +BOOST_AUTO_TEST_CASE(cone_to_bvh) { + Cone shape(1, 1); BVHModel bvh; - generateBVHModel (bvh, shape, Transform3f(), 10, 10); - generateBVHModel (bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3f(), 10, 10); + generateBVHModel(bvh, shape, Transform3f(), 50); } -BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox) -{ - Cylinder s1 (0.029, 0.1); - Box s2 (1.6, 0.6, 0.025); +BOOST_AUTO_TEST_CASE(shapeIntersection_cylinderbox) { + Cylinder s1(0.029, 0.1); + Box s2(1.6, 0.6, 0.025); - Transform3f tf1 - (Quaternion3f (0.5279170511703305, -0.50981118132505521, + Transform3f tf1( + Quaternion3f(0.5279170511703305, -0.50981118132505521, -0.67596178682051911, 0.0668715876735793), - Vec3f (0.041218354748013122, 1.2022554710435607, 0.77338855025700015)); + Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015)); - Transform3f tf2 - (Quaternion3f (0.70738826916719977, 0, 0, 0.70682518110536596), - Vec3f (-0.29936284351096382, 0.80023864435868775, 0.71750000000000003)); + Transform3f tf2( + Quaternion3f(0.70738826916719977, 0, 0, 0.70682518110536596), + Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003)); GJKSolver solver; FCL_REAL distance; Vec3f p1, p2, normal; - bool res = solver.shapeDistance (s1, tf1, s2, tf2, distance, p1, p2, normal); - BOOST_CHECK ((res && distance > 0) || (!res && distance <= 0)); + bool res = solver.shapeDistance(s1, tf1, s2, tf2, distance, p1, p2, normal); + BOOST_CHECK((res && distance > 0) || (!res && distance <= 0)); // If objects are not colliding, p2 should be outside the cylinder and // p1 should be outside the box - Vec3f p2Loc (tf1.inverse().transform (p2)); - bool p2_in_cylinder ((fabs (p2Loc [2]) <= s1.halfLength) && - (p2Loc [0] * p2Loc [0] + p2Loc [1] * p2Loc [1] - <= s1.radius)); - Vec3f p1Loc (tf2.inverse().transform (p1)); + Vec3f p2Loc(tf1.inverse().transform(p2)); + bool p2_in_cylinder((fabs(p2Loc[2]) <= s1.halfLength) && + (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius)); + Vec3f p1Loc(tf2.inverse().transform(p1)); bool p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all(); - std::cout << "p2 in cylinder = (" << p2Loc.transpose () << ")" << std::endl; - std::cout << "p1 in box = (" << p1Loc.transpose () << ")" << std::endl; + std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl; + std::cout << "p1 in box = (" << p1Loc.transpose() << ")" << std::endl; - BOOST_CHECK ((res && !p2_in_cylinder && !p1_in_box) || - (!res && p2_in_cylinder && p1_in_box)); + BOOST_CHECK((res && !p2_in_cylinder && !p1_in_box) || + (!res && p2_in_cylinder && p1_in_box)); - res = solver.shapeDistance (s2, tf2, s1, tf1, distance, p2, p1, normal); - BOOST_CHECK ((res && distance > 0) || (!res && distance <= 0)); + res = solver.shapeDistance(s2, tf2, s1, tf1, distance, p2, p1, normal); + BOOST_CHECK((res && distance > 0) || (!res && distance <= 0)); // If objects are not colliding, p2 should be outside the cylinder and // p1 should be outside the box - p2Loc = tf1.inverse().transform (p2); - p2_in_cylinder = (fabs (p2Loc [2]) <= s1.halfLength) && - (p2Loc [0] * p2Loc [0] + p2Loc [1] * p2Loc [1] - <= s1.radius); - p1Loc = tf2.inverse().transform (p1); + p2Loc = tf1.inverse().transform(p2); + p2_in_cylinder = (fabs(p2Loc[2]) <= s1.halfLength) && + (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius); + p1Loc = tf2.inverse().transform(p1); p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all(); - std::cout << "p2 in cylinder = (" << p2Loc.transpose () << ")" << std::endl; - std::cout << "p1 in box = (" << p1.transpose () << ")" << std::endl; + std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl; + std::cout << "p1 in box = (" << p1.transpose() << ")" << std::endl; - BOOST_CHECK ((res && !p2_in_cylinder && !p1_in_box) || - (!res && p2_in_cylinder && p1_in_box)); + BOOST_CHECK((res && !p2_in_cylinder && !p1_in_box) || + (!res && p2_in_cylinder && p1_in_box)); - s1 = Cylinder (0.06, 0.1); - tf1.setTranslation (Vec3f (-0.66734052046473924, 0.22219183277457269, - 0.76825248755616293)); - tf1.setQuatRotation (Quaternion3f (0.52613359459338371, - 0.32189408354839893, - 0.70415587451837913, - -0.35175580165512249)); - res = solver.shapeDistance (s1, tf1, s2, tf2, distance, p1, p2, normal); + s1 = Cylinder(0.06, 0.1); + tf1.setTranslation( + Vec3f(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293)); + tf1.setQuatRotation(Quaternion3f(0.52613359459338371, 0.32189408354839893, + 0.70415587451837913, -0.35175580165512249)); + res = solver.shapeDistance(s1, tf1, s2, tf2, distance, p1, p2, normal); } -BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere) { Sphere s1(20); Sphere s2(10); @@ -319,94 +306,107 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(40, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.9, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, 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) - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + normal.setZero(); // If the centers of two sphere are at the same position, + // the normal is (0, 0, 0) + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, 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) - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + normal.setZero(); // If the centers of two sphere are at the same position, + // the normal is (0, 0, 0) + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.9, 0, 0)); normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.0, 0, 0)); normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.01, 0, 0)); normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); } -bool compareContactPoints(const Vec3f& c1,const Vec3f& c2) -{ +bool compareContactPoints(const Vec3f& c1, const Vec3f& c2) { return c1[2] < c2[2]; -} // Ascending order +} // Ascending order -void testBoxBoxContactPoints(const Matrix3f& R) -{ +void testBoxBoxContactPoints(const Matrix3f& R) { Box s1(100, 100, 100); Box s2(10, 20, 30); // Vertices of s2 std::vector vertices(8); - vertices[0] << 1, 1, 1; - vertices[1] << 1, 1, -1; - vertices[2] << 1, -1, 1; - vertices[3] << 1, -1, -1; - vertices[4] << -1, 1, 1; - vertices[5] << -1, 1, -1; - vertices[6] << -1, -1, 1; + vertices[0] << 1, 1, 1; + vertices[1] << 1, 1, -1; + vertices[2] << 1, -1, 1; + vertices[3] << 1, -1, -1; + vertices[4] << -1, 1, 1; + vertices[5] << -1, 1, -1; + vertices[6] << -1, -1, 1; vertices[7] << -1, -1, -1; - for (std::size_t i = 0; i < 8; ++i) - { + for (std::size_t i = 0; i < 8; ++i) { vertices[i].array() *= s2.halfSide.array(); } @@ -414,30 +414,29 @@ void testBoxBoxContactPoints(const Matrix3f& R) Transform3f tf2 = Transform3f(R); Vec3f normal; - Vec3f point(0.,0.,0.); + Vec3f point(0., 0., 0.); double distance; // Make sure the two boxes are colliding solver1.gjk_tolerance = 1e-5; solver1.epa_tolerance = 1e-5; - bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, distance, true, &point, &normal); + bool res = + solver1.shapeIntersect(s1, tf1, s2, tf2, distance, true, &point, &normal); FCL_CHECK(res); // Compute global vertices - for (std::size_t i = 0; i < 8; ++i) - vertices[i] = tf2.transform(vertices[i]); + for (std::size_t i = 0; i < 8; ++i) vertices[i] = tf2.transform(vertices[i]); // Sort the vertices so that the lowest vertex along z-axis comes first std::sort(vertices.begin(), vertices.end(), compareContactPoints); // The lowest vertex along z-axis should be the contact point - FCL_CHECK(normal.isApprox(Vec3f(0,0,1), 1e-6)); + FCL_CHECK(normal.isApprox(Vec3f(0, 0, 1), 1e-6)); FCL_CHECK(vertices[0].head<2>().isApprox(point.head<2>(), 1e-6)); FCL_CHECK(vertices[0][2] <= point[2] && point[2] < 0); } -BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); @@ -456,46 +455,54 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) 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). + // TODO: Need convention for normal when the centers of two objects are at + // same position. The current result is (1, 0, 0). normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). + // TODO: Need convention for normal when the centers of two objects are at + // same position. The current result is (1, 0, 0). normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-8); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + 1e-8); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(q); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = transform; tf2 = transform * Transform3f(q); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); int numTests = 100; - for (int i = 0; i < numTests; ++i) - { + for (int i = 0; i < numTests; ++i) { Transform3f tf; generateRandomTransform(extents, tf); - SET_LINE; testBoxBoxContactPoints(tf.getRotation()); + SET_LINE; + testBoxBoxContactPoints(tf.getRotation()); } } -BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) { Sphere s1(20); Box s2(5, 5, 5); @@ -511,61 +518,68 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) 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). + // TODO: Need convention for normal when the centers of two objects are at + // same position. The current result is (-1, 0, 0). normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.50001, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.501, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.4, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); } -BOOST_AUTO_TEST_CASE(shapeDistance_spherebox) -{ +BOOST_AUTO_TEST_CASE(shapeDistance_spherebox) { hpp::fcl::Matrix3f rotSphere; - rotSphere<< 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; + rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; hpp::fcl::Vec3f trSphere(0.0, 0.0, 0.0); hpp::fcl::Matrix3f rotBox; - rotBox<< 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; + rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; hpp::fcl::Vec3f trBox(0.0, 5.0, 3.0); hpp::fcl::Sphere sphere(1); hpp::fcl::Box box(10, 2, 10); hpp::fcl::DistanceResult result; - distance(&sphere, Transform3f(rotSphere, trSphere), - &box, Transform3f(rotBox, trBox), - DistanceRequest(true), result); + distance(&sphere, Transform3f(rotSphere, trSphere), &box, + Transform3f(rotBox, trBox), DistanceRequest(true), result); FCL_REAL eps = Eigen::NumTraits::epsilon(); BOOST_CHECK_CLOSE(result.min_distance, 3., eps); - EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3f(0,1,0), eps); - EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3f(0,4,0), eps); - EIGEN_VECTOR_IS_APPROX(result.normal, Vec3f(0,1,0), eps); + EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3f(0, 1, 0), eps); + EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3f(0, 4, 0), eps); + EIGEN_VECTOR_IS_APPROX(result.normal, Vec3f(0, 1, 0), eps); } -BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) { Sphere s1(20); Capsule s2(5, 10); @@ -581,47 +595,56 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(24.9, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.999999, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25.1, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) { Cylinder s1(5, 15); Cylinder s2(5, 15); @@ -637,36 +660,48 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 9.9, 0)); normal << 0, 1, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); } /* @@ -687,23 +722,27 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at +same position. SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, +NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at +same position. SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, +NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, +false, tol_gjk); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, +false, tol_gjk); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.001, 0, 0)); @@ -743,23 +782,27 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at +same position. SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, +NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at +same position. SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, +NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 0.061); + SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, +false, 0.061); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 0.46); + SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, +false, 0.46); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); @@ -789,8 +832,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) } */ -BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle) { Sphere s(10); Vec3f t[3]; t[0] << 20, 0, 0; @@ -804,42 +846,40 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle) FCL_REAL distance; bool res; - res = solver1.shapeTriangleInteraction - (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver1.shapeTriangleInteraction(s, transform, t[0], t[1], t[2], - transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(s, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - t[0] << 30, 0, 0; t[1] << 9.9, -20, 0; t[2] << 9.9, 20, 0; - res = solver1.shapeTriangleInteraction - (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver1.shapeTriangleInteraction - (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(s, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - res = solver1.shapeTriangleInteraction - (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9)); - res = solver1.shapeTriangleInteraction - (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(s, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } -BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle) { Halfspace hs(Vec3f(1, 0, 0), 0); Vec3f t[3]; t[0] << 20, 0, 0; @@ -854,49 +894,48 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle) Vec3f normal; bool res; - res = solver1.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver1.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - t[0] << 20, 0, 0; t[1] << 0, -20, 0; t[2] << 0, 20, 0; - res = solver1.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - // These tests fail because of numerical precision errors. The points t[1] and t[2] - // lies on the border of the half-space. - // The normals should be good, when computed (i.e. res == true) - res = solver1.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + // These tests fail because of numerical precision errors. The points t[1] and + // t[2] lies on the border of the half-space. The normals should be good, when + // computed (i.e. res == true) + res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); // BOOST_CHECK(res); if (res) - BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); + BOOST_CHECK( + isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); - res = solver1.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); // BOOST_CHECK(res); - if (res) - BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9)); + if (res) BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9)); - res = solver1.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); // BOOST_CHECK(res); if (res) - BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); + BOOST_CHECK( + isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } -BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle) { Plane hs(Vec3f(1, 0, 0), 0); Vec3f t[3]; t[0] << 20, 0, 0; @@ -911,38 +950,36 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle) Vec3f normal; bool res; - res = solver1.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver1.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - t[0] << 20, 0, 0; t[1] << -0.1, -20, 0; t[2] << -0.1, 20, 0; - res = solver1.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - res = solver1.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9)); - res = solver1.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } -BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) { Sphere s(10); Halfspace hs(Vec3f(1, 0, 0), 0); @@ -961,68 +998,77 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) contact << -5, 0, 0; depth = 10; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(-5, 0, 0)); depth = 10; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5, 0, 0)); contact << -2.5, 0, 0; depth = 15; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5, 0, 0)); contact << -7.5, 0, 0; depth = 5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-10.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); contact << 0.05, 0, 0; depth = 20.1; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); } -BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) { Sphere s(10); Plane hs(Vec3f(1, 0, 0), 0); @@ -1041,62 +1087,71 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) contact.setZero(); depth = 10; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5, 0, 0)); contact << 5, 0, 0; depth = 5; normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5, 0, 0)); contact = transform.transform(Vec3f(5, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5, 0, 0)); contact << -5, 0, 0; depth = 5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); contact = transform.transform(Vec3f(-5, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-10.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) { Box s(5, 10, 20); Halfspace hs(Vec3f(1, 0, 0), 0); @@ -1115,72 +1170,82 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) contact << -1.25, 0, 0; depth = 2.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(-1.25, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); contact << -0.625, 0, 0; depth = 3.75; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); contact = transform.transform(Vec3f(-0.625, 0, 0)); depth = 3.75; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); contact << -1.875, 0, 0; depth = 1.25; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); contact = transform.transform(Vec3f(-1.875, 0, 0)); depth = 1.25; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.51, 0, 0)); contact << 0.005, 0, 0; depth = 5.01; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.51, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(transform.getRotation()); tf2 = Transform3f(); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true); } -BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) { Box s(5, 10, 20); Plane hs(Vec3f(1, 0, 0), 0); @@ -1199,66 +1264,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) contact << 0, 0, 0; depth = 2.5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); contact << 1.25, 0, 0; depth = 1.25; normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); contact = transform.transform(Vec3f(1.25, 0, 0)); depth = 1.25; normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); contact << -1.25, 0, 0; depth = 1.25; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); contact = transform.transform(Vec3f(-1.25, 0, 0)); depth = 1.25; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.51, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.51, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(transform.getRotation()); tf2 = Transform3f(); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true); } -BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) { Capsule s(5, 10); Halfspace hs(Vec3f(1, 0, 0), 0); @@ -1277,67 +1352,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) contact << -2.5, 0, 0; depth = 5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact << -1.25, 0, 0; depth = 7.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(-1.25, 0, 0)); depth = 7.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = 2.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-3.75, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contact << 0.05, 0, 0; depth = 10.1; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Halfspace(Vec3f(0, 1, 0), 0); @@ -1346,67 +1428,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) contact << 0, -2.5, 0; depth = 5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact << 0, -1.25, 0; depth = 7.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, -1.25, 0)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact << 0, -3.75, 0; depth = 2.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -3.75, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contact << 0, 0.05, 0; depth = 10.1; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Halfspace(Vec3f(0, 0, 1), 0); @@ -1415,68 +1504,77 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) contact << 0, 0, -5; depth = 10; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, -5)); depth = 10; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact << 0, 0, -3.75; depth = 12.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, -3.75)); depth = 12.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact << 0, 0, -6.25; depth = 7.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -6.25)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); contact << 0, 0, 0.05; depth = 20.1; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) { Capsule s(5, 10); Plane hs(Vec3f(1, 0, 0), 0); @@ -1495,61 +1593,68 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) contact << 0, 0, 0; depth = 5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, 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) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact << 2.5, 0, 0; depth = 2.5; normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact << -2.5, 0, 0; depth = 2.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Plane(Vec3f(0, 1, 0), 0); @@ -1558,63 +1663,68 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) contact << 0, 0, 0; depth = 5; normal << 0, 1, 0; // (0, 1, 0) or (0, -1, 0) - SET_LINE; testShapeIntersection - (s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, 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) - SET_LINE; testShapeIntersection - (s, tf1, hs, tf2, true, 0x0, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact << 0, 2.5, 0; depth = 2.5; normal << 0, 1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact << 0, -2.5, 0; depth = 2.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Plane(Vec3f(0, 0, 1), 0); @@ -1623,64 +1733,71 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) contact << 0, 0, 0; depth = 10; normal << 0, 0, 1; // (0, 0, 1) or (0, 0, -1) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, 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) - SET_LINE; testShapeIntersection - (s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact << 0, 0, 2.5; depth = 7.5; normal << 0, 0, 1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact << 0, 0, -2.5; depth = 7.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); 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); - SET_LINE; testShapeIntersection - (s, tf1, hs, tf2, true, 0x0, &depth, 0x0); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) { Cylinder s(5, 10); Halfspace hs(Vec3f(1, 0, 0), 0); @@ -1699,67 +1816,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) contact << -2.5, 0, 0; depth = 5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact << -1.25, 0, 0; depth = 7.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(-1.25, 0, 0)); depth = 7.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = 2.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-3.75, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contact << 0.05, 0, 0; depth = 10.1; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Halfspace(Vec3f(0, 1, 0), 0); @@ -1768,67 +1892,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) contact << 0, -2.5, 0; depth = 5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact << 0, -1.25, 0; depth = 7.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, -1.25, 0)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact << 0, -3.75, 0; depth = 2.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -3.75, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contact << 0, 0.05, 0; depth = 10.1; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Halfspace(Vec3f(0, 0, 1), 0); @@ -1837,68 +1968,77 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) contact << 0, 0, -2.5; depth = 5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact << 0, 0, -1.25; depth = 7.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, -1.25)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact << 0, 0, -3.75; depth = 2.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -3.75)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 5.1)); contact << 0, 0, 0.05; depth = 10.1; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -5.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) { Cylinder s(5, 10); Plane hs(Vec3f(1, 0, 0), 0); @@ -1917,61 +2057,68 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) contact << 0, 0, 0; depth = 5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact << 2.5, 0, 0; depth = 2.5; normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(2.5, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact << -2.5, 0, 0; depth = 2.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-2.5, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Plane(Vec3f(0, 1, 0), 0); @@ -1980,61 +2127,68 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) contact << 0, 0, 0; depth = 5; normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact << 0, 2.5, 0; depth = 2.5; normal << 0, 1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, 2.5, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact << 0, -2.5, 0; depth = 2.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -2.5, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Plane(Vec3f(0, 0, 1), 0); @@ -2043,63 +2197,71 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) contact << 0, 0, 0; depth = 5; normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact << 0, 0, 2.5; depth = 2.5; normal << 0, 0, 1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, 2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, 1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact << 0, 0, -2.5; depth = 2.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); } - -BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) { Cone s(5, 10); Halfspace hs(Vec3f(1, 0, 0), 0); @@ -2118,67 +2280,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) contact << -2.5, 0, -5; depth = 5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact << -1.25, 0, -5; depth = 7.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(-1.25, 0, -5)); depth = 7.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact << -3.75, 0, -5; depth = 2.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-3.75, 0, -5)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contact << 0.05, 0, -5; depth = 10.1; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Halfspace(Vec3f(0, 1, 0), 0); @@ -2187,67 +2356,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) contact << 0, -2.5, -5; depth = 5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact << 0, -1.25, -5; depth = 7.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, -1.25, -5)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact << 0, -3.75, -5; depth = 2.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -3.75, -5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contact << 0, 0.05, -5; depth = 10.1; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Halfspace(Vec3f(0, 0, 1), 0); @@ -2256,68 +2432,77 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) contact << 0, 0, -2.5; depth = 5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact << 0, 0, -1.25; depth = 7.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, -1.25)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact << 0, 0, -3.75; depth = 2.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -3.75)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 5.1)); contact << 0, 0, 0.05; depth = 10.1; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -5.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) -{ +BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) { Cone s(5, 10); Plane hs(Vec3f(1, 0, 0), 0); @@ -2336,61 +2521,69 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) contact << 0, 0, 0; depth = 5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + normal = + transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact << 2.5, 0, -2.5; depth = 2.5; normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(2.5, 0, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact << -2.5, 0, -2.5; depth = 2.5; normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-2.5, 0, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Plane(Vec3f(0, 1, 0), 0); @@ -2399,61 +2592,68 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) contact << 0, 0, 0; depth = 5; normal << 0, 1, 0; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact << 0, 2.5, -2.5; depth = 2.5; normal << 0, 1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, 2.5, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact << 0, -2.5, -2.5; depth = 2.5; normal << 0, -1, 0; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -2.5, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); - - - + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); hs = Plane(Vec3f(0, 0, 1), 0); @@ -2462,156 +2662,160 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) contact << 0, 0, 0; depth = 5; normal << 0, 0, 1; // (1, 0, 0) or (-1, 0, 0) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, 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) - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact << 0, 0, 2.5; depth = 2.5; normal << 0, 0, 1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, 2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, 1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact << 0, 0, -2.5; depth = 2.5; normal << 0, 0, -1; - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); - SET_LINE; testShapeIntersection(s, tf1, hs, tf2, false); + SET_LINE; + testShapeIntersection(s, tf1, hs, tf2, false); } - - -BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere) -{ +BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere) { Sphere s1(20); Sphere s2(10); Transform3f transform; - //generateRandomTransform(extents, transform); + // generateRandomTransform(extents, transform); bool res; FCL_REAL dist = -1; Vec3f closest_p1, closest_p2, normal; - res = solver1.shapeDistance (s1, Transform3f(), s2, - Transform3f(Vec3f(0, 40, 0)), - dist, closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 40, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, - Transform3f(Vec3f(30.1, 0, 0)), dist, - closest_p1, closest_p2, normal); + Transform3f(Vec3f(30.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, - Transform3f(Vec3f(29.9, 0, 0)), dist, - closest_p1, closest_p2, normal); + Transform3f(Vec3f(29.9, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), - s2, Transform3f(), dist, - closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), - s2, Transform3f(), dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, + Transform3f(), dist, closest_p1, closest_p2, + normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), - s2, Transform3f(), dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, + Transform3f(), dist, closest_p1, closest_p2, + normal); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); - - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(40, 0, 0)), dist, + closest_p1, closest_p2, normal); // this is one problem: the precise is low sometimes BOOST_CHECK(fabs(dist - 10) < 0.1); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(30.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(30.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.06); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, - transform * Transform3f(Vec3f(29.9, 0, 0)), - dist, closest_p1, closest_p2, normal); + transform * Transform3f(Vec3f(29.9, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), - s2, transform, dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, + transform, dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.1); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), - s2, transform, dist, - closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, + transform, dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.1); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), - s2, transform, dist, - closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, + transform, dist, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(shapeDistance_boxbox) -{ +BOOST_AUTO_TEST_CASE(shapeDistance_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); Vec3f closest_p1, closest_p2, normal; Transform3f transform; - //generateRandomTransform(extents, transform); + // generateRandomTransform(extents, transform); bool res; FCL_REAL dist; @@ -2621,81 +2825,79 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxbox) BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s2, Transform3f(), - s2, Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s2, Transform3f(), s2, + Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s2, Transform3f(), - s2, Transform3f(Vec3f(20.1, 0, 0)), dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s2, Transform3f(), s2, + Transform3f(Vec3f(20.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s2, Transform3f(), - s2, Transform3f(Vec3f(0, 20.2, 0)), dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s2, Transform3f(), s2, + Transform3f(Vec3f(0, 20.2, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 10.2) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s2, Transform3f(), - s2, Transform3f(Vec3f(10.1, 10.1, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s2, Transform3f(), s2, + Transform3f(Vec3f(10.1, 10.1, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s2, Transform3f(), - s2, Transform3f(Vec3f(10.1, 0, 0)), dist, - closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s2, Transform3f(), s2, + Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s2, Transform3f(), - s2, Transform3f(Vec3f(20.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s2, Transform3f(), s2, + Transform3f(Vec3f(20.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s2, Transform3f(), - s2, Transform3f(Vec3f(0, 20.1, 0)),dist, - closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s2, Transform3f(), s2, + Transform3f(Vec3f(0, 20.1, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s2, Transform3f(), - s2, Transform3f(Vec3f(10.1, 10.1, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s2, Transform3f(), s2, + Transform3f(Vec3f(10.1, 10.1, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); BOOST_CHECK(res); - - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(15.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(15.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(20, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(20, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(20, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere) -{ +BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere) { Sphere s1(20); Box s2(5, 5, 5); Vec3f closest_p1, closest_p2, normal; @@ -2707,62 +2909,59 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere) FCL_REAL dist; int N = 10; - for (int i = 0; i < N+1; ++i) { - FCL_REAL dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3*N); - res = solver1.shapeDistance( - s1, Transform3f(Vec3f(dbox,0.,0.)), - s2, Transform3f(), - dist, closest_p1, closest_p2, normal); + for (int i = 0; i < N + 1; ++i) { + FCL_REAL dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N); + res = solver1.shapeDistance(s1, Transform3f(Vec3f(dbox, 0., 0.)), s2, + Transform3f(), dist, closest_p1, closest_p2, + normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6); - EIGEN_VECTOR_IS_APPROX(normal, -Vec3f(1,0,0), 1e-6); + EIGEN_VECTOR_IS_APPROX(normal, -Vec3f(1, 0, 0), 1e-6); - res = solver1.shapeDistance(s1, transform, s2, transform, dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); res = solver1.shapeDistance( - s1, transform * Transform3f(Vec3f(dbox,0.,0.)), - s2, transform, - dist, closest_p1, closest_p2, normal); + s1, transform * Transform3f(Vec3f(dbox, 0., 0.)), s2, transform, dist, + closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6); EIGEN_VECTOR_IS_APPROX(normal, -transform.getRotation().col(0), 1e-6); } - res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(22.6, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(22.6, 0, 0)), dist, closest_p1, + closest_p2, normal); 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, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(22.6, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.05); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(40, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder) -{ +BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); Vec3f closest_p1, closest_p2, normal; @@ -2773,45 +2972,42 @@ BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder) bool res; FCL_REAL dist; - res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(10.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(40, 0, 0)), dist, - closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(40, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); } - - -BOOST_AUTO_TEST_CASE(shapeDistance_conecone) -{ +BOOST_AUTO_TEST_CASE(shapeDistance_conecone) { Cone s1(5, 10); Cone s2(5, 10); Vec3f closest_p1, closest_p2, normal; @@ -2827,38 +3023,37 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecone) BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(10.1, 0, 0)), dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(10.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(0, 0, 40)), dist, - closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(0, 0, 40)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(0, 0, 40)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder) -{ +BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); Vec3f closest_p1, closest_p2, normal; @@ -2874,40 +3069,37 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder) BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, dist, - closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(10.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.02); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = + solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.01); BOOST_CHECK(res); - res = solver1.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver1.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(40, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.1); BOOST_CHECK(res); } - - -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) { Sphere s1(20); Sphere s2(10); @@ -2917,79 +3109,94 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) Transform3f transform; generateRandomTransform(extents, transform); -// Vec3f contact; -// FCL_REAL depth; + // Vec3f contact; + // FCL_REAL depth; Vec3f normal; tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(40, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.9, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, 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) - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + normal.setZero(); // If the centers of two sphere are at the same position, + // the normal is (0, 0, 0) + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, 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) - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + normal.setZero(); // If the centers of two sphere are at the same position, + // the normal is (0, 0, 0) + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.9, 0, 0)); normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); normal = transform.getRotation() * Vec3f(-1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.0, 0, 0)); normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.01, 0, 0)); normal << -1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); @@ -3008,38 +3215,46 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) 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). + // TODO: Need convention for normal when the centers of two objects are at + // same position. The current result is (1, 0, 0). normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). + // TODO: Need convention for normal when the centers of two objects are at + // same position. The current result is (1, 0, 0). normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-8); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + 1e-8); tf1 = transform; tf2 = transform * Transform3f(Vec3f(15.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(q); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = transform; tf2 = transform * Transform3f(q); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) { Sphere s1(20); Box s2(5, 5, 5); @@ -3055,38 +3270,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.5, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-7); // built-in GJK solver requires larger tolerance than libccd + SET_LINE; + testShapeIntersection( + s1, tf1, s2, tf2, 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)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.4, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd + SET_LINE; + testShapeIntersection( + s1, tf1, s2, tf2, 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); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); // built-in GJK solver returns incorrect normal. // testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) { Sphere s1(20); Capsule s2(5, 10); @@ -3102,37 +3328,44 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(24.9, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); @@ -3148,37 +3381,48 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10, 0, 0)); normal << 1, 0, 0; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) { Cone s1(5, 10); Cone s2(5, 10); @@ -3194,46 +3438,60 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); // z=0 is a singular points. Two normals could be returned. tf2 = Transform3f(Vec3f(9.9, 0, 0.00001)); - normal = Vec3f(2*(s1.halfLength + s2.halfLength), 0, s1.radius+s2.radius).normalized(); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) + .normalized(); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = transform * tf1; tf2 = transform * tf2; normal = transform.getRotation() * normal; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); normal << 0, 0, 1; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); normal = transform.getRotation() * Vec3f(0, 0, 1); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); @@ -3249,53 +3507,66 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) tf1 = Transform3f(); tf2 = Transform3f(); - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + // TODO: Need convention for normal when the centers of two objects are at + // same position. + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10, 0, 0)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); normal << 0, 0, 1; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); normal = transform.getRotation() * Vec3f(0, 0, 1); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10)); normal << 0, 0, 1; - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, + tol_gjk); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - SET_LINE; testShapeIntersection(s1, tf1, s2, tf2, false); + SET_LINE; + testShapeIntersection(s1, tf1, s2, tf2, false); } - -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle) { Sphere s(10); Vec3f t[3]; t[0] << 20, 0, 0; @@ -3310,41 +3581,40 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle) Vec3f normal; bool res; - res = solver2.shapeTriangleInteraction - (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver2.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver2.shapeTriangleInteraction - (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver2.shapeTriangleInteraction(s, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); t[0] << 30, 0, 0; t[1] << 9.9, -20, 0; t[2] << 9.9, 20, 0; - res = solver2.shapeTriangleInteraction - (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver2.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver2.shapeTriangleInteraction - (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver2.shapeTriangleInteraction(s, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - res = solver2.shapeTriangleInteraction - (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver2.shapeTriangleInteraction(s, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9)); - res = solver2.shapeTriangleInteraction - (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver2.shapeTriangleInteraction(s, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle) { Halfspace hs(Vec3f(1, 0, 0), 0); Vec3f t[3]; t[0] << 20, 0, 0; @@ -3359,42 +3629,40 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle) Vec3f normal; bool res; - res = solver2.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver2.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - t[0] << 20, 0, 0; t[1] << 0, -20, 0; t[2] << 0, 20, 0; - res = solver2.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver2.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - res = solver2.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9)); - res = solver2.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle) -{ +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle) { Plane hs(Vec3f(1, 0, 0), 0); Vec3f t[3]; t[0] << 20, 0, 0; @@ -3409,45 +3677,40 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle) Vec3f normal; bool res; - res = solver1.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver1.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver1.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver1.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - t[0] << 20, 0, 0; t[1] << -0.1, -20, 0; t[2] << -0.1, 20, 0; - res = solver2.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); - res = solver2.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); - res = solver2.shapeTriangleInteraction - (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, - normal); + res = + solver2.shapeTriangleInteraction(hs, Transform3f(), t[0], t[1], t[2], + Transform3f(), distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9)); - res = solver2.shapeTriangleInteraction - (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal); + res = solver2.shapeTriangleInteraction(hs, transform, t[0], t[1], t[2], + transform, distance, c1, c2, normal); BOOST_CHECK(res); BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } - - - -BOOST_AUTO_TEST_CASE(spheresphere) -{ +BOOST_AUTO_TEST_CASE(spheresphere) { Sphere s1(20); Sphere s2(10); Vec3f closest_p1, closest_p2, normal; @@ -3458,82 +3721,79 @@ BOOST_AUTO_TEST_CASE(spheresphere) bool res; FCL_REAL dist = -1; - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = + solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(30.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(30.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(29.9, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(29.9, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), - s2, Transform3f(), dist, closest_p1, - closest_p2, normal); + res = + solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), - s2, Transform3f(), dist, closest_p1, - closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, + Transform3f(), dist, closest_p1, closest_p2, + normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), - s2, Transform3f(), dist, closest_p1, closest_p2, + res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, + Transform3f(), dist, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(40, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(30.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(30.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(29.9, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(29.9, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), - s2, transform, dist, closest_p1, closest_p2, - normal); + res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, + transform, dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), - s2, transform, dist, closest_p1, closest_p2, - normal); + res = + solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, + transform, dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), - s2, transform, dist, closest_p1, closest_p2, - normal); + res = + solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, + transform, dist, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(boxbox) -{ +BOOST_AUTO_TEST_CASE(boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); Vec3f closest_p1, closest_p2, normal; @@ -3544,43 +3804,42 @@ BOOST_AUTO_TEST_CASE(boxbox) bool res; FCL_REAL dist; - res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform, - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(15.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(15.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(15.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(15.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(20, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = + solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(20, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(20, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(boxsphere) -{ +BOOST_AUTO_TEST_CASE(boxsphere) { Sphere s1(20); Box s2(5, 5, 5); Vec3f closest_p1, closest_p2, normal; @@ -3591,43 +3850,42 @@ BOOST_AUTO_TEST_CASE(boxsphere) bool res; FCL_REAL dist; - res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform, - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(22.6, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(22.6, 0, 0)), dist, closest_p1, + closest_p2, normal); 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, closest_p1, closest_p2, normal); + transform * Transform3f(Vec3f(22.6, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = + solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(40, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(cylindercylinder) -{ +BOOST_AUTO_TEST_CASE(cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); Vec3f closest_p1, closest_p2, normal; @@ -3638,45 +3896,42 @@ BOOST_AUTO_TEST_CASE(cylindercylinder) bool res; FCL_REAL dist; - res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform, - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(10.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = + solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(40, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(40, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); } - - -BOOST_AUTO_TEST_CASE(conecone) -{ +BOOST_AUTO_TEST_CASE(conecone) { Cone s1(5, 10); Cone s2(5, 10); Vec3f closest_p1, closest_p2, normal; @@ -3687,47 +3942,44 @@ BOOST_AUTO_TEST_CASE(conecone) bool res; FCL_REAL dist; - res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform, - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1, + closest_p2, normal); BOOST_CHECK(dist <= 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, Transform3f(), s2, + Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1, + closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(10.1, 0, 0)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(10.1, 0, 0)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, Transform3f(), - s2, Transform3f(Vec3f(0, 0, 40)), - dist, closest_p1, closest_p2, normal); + res = + solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), + dist, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); - res = solver2.shapeDistance(s1, transform, - s2, transform * Transform3f(Vec3f(0, 0, 40)), - dist, closest_p1, closest_p2, normal); + res = solver2.shapeDistance(s1, transform, s2, + transform * Transform3f(Vec3f(0, 0, 40)), dist, + closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); } - - - -template -void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance) -{ +template +void testReversibleShapeDistance(const S1& s1, const S2& s2, + FCL_REAL distance) { Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0)); Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0)); @@ -3749,8 +4001,9 @@ void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance) BOOST_CHECK(resA); BOOST_CHECK(resB); - BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same - BOOST_CHECK(isEqual(p1A, p2B, tol)); // closest points should in reverse order + BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same + BOOST_CHECK( + isEqual(p1A, p2B, tol)); // closest points should in reverse order BOOST_CHECK(isEqual(p2A, p1B, tol)); resA = solver2.shapeDistance(s1, tf1, s2, tf2, distA, p1A, p2A, normalA); @@ -3763,53 +4016,54 @@ void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance) BOOST_CHECK(isEqual(p2A, p1B, tol)); } -BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) -{ +BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) { // This test check whether a shape distance algorithm is called for the // reverse case as well. For example, if FCL has sphere-capsule distance // algorithm, then this algorithm should be called for capsule-sphere case. - // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace + // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, + // cylinder, plane, halfspace Box box(10, 10, 10); Sphere sphere(5); Capsule capsule(5, 10); Cone cone(5, 10); Cylinder cylinder(5, 10); - Plane plane(Vec3f(0,0,0), 0.0); - Halfspace halfspace(Vec3f(0,0,0), 0.0); + Plane plane(Vec3f(0, 0, 0), 0.0); + Halfspace halfspace(Vec3f(0, 0, 0), 0.0); - // Use sufficiently long distance so that all the primitive shapes CANNOT intersect + // Use sufficiently long distance so that all the primitive shapes CANNOT + // intersect FCL_REAL distance = 15.0; // If new shape distance algorithm is added for two distinct primitive // shapes, uncomment associated lines. For example, box-sphere intersection // algorithm is added, then uncomment box-sphere. -// testReversibleShapeDistance(box, sphere, distance); -// testReversibleShapeDistance(box, capsule, distance); -// testReversibleShapeDistance(box, cone, distance); -// testReversibleShapeDistance(box, cylinder, distance); -// testReversibleShapeDistance(box, plane, distance); -// testReversibleShapeDistance(box, halfspace, distance); - - SET_LINE; testReversibleShapeDistance(sphere, capsule, distance); -// testReversibleShapeDistance(sphere, cone, distance); -// testReversibleShapeDistance(sphere, cylinder, distance); -// testReversibleShapeDistance(sphere, plane, distance); -// testReversibleShapeDistance(sphere, halfspace, distance); - -// testReversibleShapeDistance(capsule, cone, distance); -// testReversibleShapeDistance(capsule, cylinder, distance); -// testReversibleShapeDistance(capsule, plane, distance); -// testReversibleShapeDistance(capsule, halfspace, distance); - -// testReversibleShapeDistance(cone, cylinder, distance); -// testReversibleShapeDistance(cone, plane, distance); -// testReversibleShapeDistance(cone, halfspace, distance); - -// testReversibleShapeDistance(cylinder, plane, distance); -// testReversibleShapeDistance(cylinder, halfspace, distance); - -// testReversibleShapeDistance(plane, halfspace, distance); + // testReversibleShapeDistance(box, sphere, distance); + // testReversibleShapeDistance(box, capsule, distance); + // testReversibleShapeDistance(box, cone, distance); + // testReversibleShapeDistance(box, cylinder, distance); + // testReversibleShapeDistance(box, plane, distance); + // testReversibleShapeDistance(box, halfspace, distance); + + SET_LINE; + testReversibleShapeDistance(sphere, capsule, distance); + // testReversibleShapeDistance(sphere, cone, distance); + // testReversibleShapeDistance(sphere, cylinder, distance); + // testReversibleShapeDistance(sphere, plane, distance); + // testReversibleShapeDistance(sphere, halfspace, distance); + + // testReversibleShapeDistance(capsule, cone, distance); + // testReversibleShapeDistance(capsule, cylinder, distance); + // testReversibleShapeDistance(capsule, plane, distance); + // testReversibleShapeDistance(capsule, halfspace, distance); + + // testReversibleShapeDistance(cone, cylinder, distance); + // testReversibleShapeDistance(cone, plane, distance); + // testReversibleShapeDistance(cone, halfspace, distance); + + // testReversibleShapeDistance(cylinder, plane, distance); + // testReversibleShapeDistance(cylinder, halfspace, distance); + + // testReversibleShapeDistance(plane, halfspace, distance); } - diff --git a/test/gjk.cpp b/test/gjk.cpp index 9ab988179..7f70c5bc4 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -45,150 +45,152 @@ #include "utility.h" +using hpp::fcl::FCL_REAL; using hpp::fcl::GJKSolver; -using hpp::fcl::TriangleP; -using hpp::fcl::Vec3f; +using hpp::fcl::Matrix3f; using hpp::fcl::Quaternion3f; using hpp::fcl::Transform3f; -using hpp::fcl::Matrix3f; -using hpp::fcl::FCL_REAL; +using hpp::fcl::TriangleP; +using hpp::fcl::Vec3f; typedef Eigen::Matrix vector_t; typedef Eigen::Matrix vector6_t; typedef Eigen::Matrix vector4_t; typedef Eigen::Matrix matrix_t; -struct Result -{ +struct Result { bool collision; clock_t timeGjk; clock_t timeGte; -}; // struct benchmark +}; // struct benchmark -typedef std::vector Results_t; +typedef std::vector Results_t; -BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) -{ - Eigen::IOFormat numpy (Eigen::FullPrecision, Eigen::DontAlignCols, ", ", - ", ", "np.array ((", "))", "", ""); - Eigen::IOFormat tuple (Eigen::FullPrecision, Eigen::DontAlignCols, "", - ", ", "", "", "(", ")"); +BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) { + Eigen::IOFormat numpy(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ", + "np.array ((", "))", "", ""); + Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", + "", "", "(", ")"); std::size_t N = 10000; GJKSolver solver; Transform3f tf1, tf2; Vec3f p1, p2, a1, a2; Matrix3f M; - FCL_REAL distance (sqrt (-1)); + FCL_REAL distance(sqrt(-1)); clock_t start, end; std::size_t nCol = 0, nDiff = 0; FCL_REAL eps = 1e-7; - Results_t results (N); - for (std::size_t i=0; i 0)); + start = clock(); + res = solver.shapeDistance(tri1, tf1, tri2, tf2, distance, p1, p2, normal); + end = clock(); + results[i].timeGjk = end - start; + results[i].collision = !res; + assert(res == (distance > 0)); if (!res) { Vec3f c1, c2, normal2; ++nCol; // check that moving triangle 2 by the penetration depth in the // direction of the normal makes the triangles collision free. - FCL_REAL penetration_depth (-distance); - assert (penetration_depth >= 0); - tf2.setTranslation ((penetration_depth + 10-4) * normal); - res = solver.shapeDistance (tri1, tf1, tri2, tf2, distance, c1, c2, - normal2); + FCL_REAL penetration_depth(-distance); + assert(penetration_depth >= 0); + tf2.setTranslation((penetration_depth + 10 - 4) * normal); + res = + solver.shapeDistance(tri1, tf1, tri2, tf2, distance, c1, c2, normal2); if (!res) { - std::cerr << "P1 = " << P1_loc.format (tuple) << std::endl; - std::cerr << "P2 = " << P2_loc.format (tuple) << std::endl; - std::cerr << "P3 = " << P3_loc.format (tuple) << std::endl; - std::cerr << "Q1 = " << Q1_loc.format (tuple) << std::endl; - std::cerr << "Q2 = " << Q2_loc.format (tuple) << std::endl; - std::cerr << "Q3 = " << Q3_loc.format (tuple) << std::endl; - std::cerr << "p1 = " << c1.format (tuple) << std::endl; - std::cerr << "p2 = " << c2.format (tuple) << std::endl; - std::cerr << "tf1 = " << tf1.getTranslation ().format (tuple) - << " + " << tf1.getQuatRotation ().coeffs ().format (tuple) - << std::endl; - std::cerr << "tf2 = " << tf2.getTranslation ().format (tuple) - << " + " << tf2.getQuatRotation ().coeffs ().format (tuple) - << std::endl; - std::cerr << "normal = " << normal.format (tuple) << std::endl; - abort (); + std::cerr << "P1 = " << P1_loc.format(tuple) << std::endl; + std::cerr << "P2 = " << P2_loc.format(tuple) << std::endl; + std::cerr << "P3 = " << P3_loc.format(tuple) << std::endl; + std::cerr << "Q1 = " << Q1_loc.format(tuple) << std::endl; + std::cerr << "Q2 = " << Q2_loc.format(tuple) << std::endl; + std::cerr << "Q3 = " << Q3_loc.format(tuple) << std::endl; + std::cerr << "p1 = " << c1.format(tuple) << std::endl; + std::cerr << "p2 = " << c2.format(tuple) << std::endl; + std::cerr << "tf1 = " << tf1.getTranslation().format(tuple) << " + " + << tf1.getQuatRotation().coeffs().format(tuple) << std::endl; + std::cerr << "tf2 = " << tf2.getTranslation().format(tuple) << " + " + << tf2.getQuatRotation().coeffs().format(tuple) << std::endl; + std::cerr << "normal = " << normal.format(tuple) << std::endl; + abort(); } distance = 0; - tf2.setIdentity (); + tf2.setIdentity(); } // Compute vectors between vertices - Vec3f P1 (tf1.transform (P1_loc)), P2 (tf1.transform (P2_loc)), - P3 (tf1.transform (P3_loc)), Q1 (tf2.transform (Q1_loc)), - Q2 (tf2.transform (Q2_loc)), Q3 (tf2.transform (Q3_loc)); - Vec3f u1 (P2 - P1); - Vec3f v1 (P3 - P1); - Vec3f w1 (u1.cross (v1)); - Vec3f u2 (Q2 - Q1); - Vec3f v2 (Q3 - Q1); - Vec3f w2 (u2.cross (v2)); - BOOST_CHECK (w1.squaredNorm () > eps*eps); - M.col (0) = u1; M.col (1) = v1; M.col (2) = w1; + Vec3f P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)), + P3(tf1.transform(P3_loc)), Q1(tf2.transform(Q1_loc)), + Q2(tf2.transform(Q2_loc)), Q3(tf2.transform(Q3_loc)); + Vec3f u1(P2 - P1); + Vec3f v1(P3 - P1); + Vec3f w1(u1.cross(v1)); + Vec3f u2(Q2 - Q1); + Vec3f v2(Q3 - Q1); + Vec3f w2(u2.cross(v2)); + BOOST_CHECK(w1.squaredNorm() > eps * eps); + M.col(0) = u1; + M.col(1) = v1; + M.col(2) = w1; // Compute a1 such that p1 = P1 + a11 u1 + a12 v1 + a13 u1 x v1 a1 = M.inverse() * (p1 - P1); EIGEN_VECTOR_IS_APPROX(p1, P1 + a1[0] * u1 + a1[1] * v1, eps); - BOOST_CHECK (w2.squaredNorm () > eps*eps); + BOOST_CHECK(w2.squaredNorm() > eps * eps); // Compute a2 such that p2 = Q1 + a21 u2 + a22 v2 + a23 u2 x v2 - M.col (0) = u2; M.col (1) = v2; M.col (2) = w2; + M.col(0) = u2; + M.col(1) = v2; + M.col(2) = w2; a2 = M.inverse() * (p2 - Q1); EIGEN_VECTOR_IS_APPROX(p2, Q1 + a2[0] * u2 + a2[1] * v2, eps); @@ -213,29 +215,32 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) // Compute gradient of f vector4_t grad_f; - grad_f [0] = -(p2 - p1).dot (u1); - grad_f [1] = -(p2 - p1).dot (v1); - grad_f [2] = (p2 - p1).dot (u2); - grad_f [3] = (p2 - p1).dot (v2); + grad_f[0] = -(p2 - p1).dot(u1); + grad_f[1] = -(p2 - p1).dot(v1); + grad_f[2] = (p2 - p1).dot(u2); + grad_f[3] = (p2 - p1).dot(v2); vector6_t g; - g [0] = -a1 [0]; - g [1] = -a1 [1]; - g [2] = a1 [0] + a1 [1] - 1; - g [3] = -a2 [0]; - g [4] = -a2 [1]; - g [5] = a2 [0] + a2 [1] - 1; - matrix_t grad_g (4, 6); grad_g.setZero (); - grad_g (0,0) = -1.; - grad_g (1,1) = -1; - grad_g (0,2) = 1; grad_g (1,2) = 1; - grad_g (2,3) = -1; - grad_g (3,4) = -1; - grad_g (2,5) = 1; grad_g (3,5) = 1; + g[0] = -a1[0]; + g[1] = -a1[1]; + g[2] = a1[0] + a1[1] - 1; + g[3] = -a2[0]; + g[4] = -a2[1]; + g[5] = a2[0] + a2[1] - 1; + matrix_t grad_g(4, 6); + grad_g.setZero(); + grad_g(0, 0) = -1.; + grad_g(1, 1) = -1; + grad_g(0, 2) = 1; + grad_g(1, 2) = 1; + grad_g(2, 3) = -1; + grad_g(3, 4) = -1; + grad_g(2, 5) = 1; + grad_g(3, 5) = 1; // Check that closest points are on triangles planes // Projection of [P1p1] on line normal to triangle 1 plane is equal to 0 - BOOST_CHECK_SMALL (a1 [2], eps); + BOOST_CHECK_SMALL(a1[2], eps); // Projection of [Q1p2] on line normal to triangle 2 plane is equal to 0 - BOOST_CHECK_SMALL (a2 [2], eps); + BOOST_CHECK_SMALL(a2[2], eps); /* Check Karush–Kuhn–Tucker conditions 6 @@ -250,25 +255,27 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) i i */ - matrix_t Mkkt (4, 6); matrix_t::Index col = 0; + matrix_t Mkkt(4, 6); + matrix_t::Index col = 0; // Check that constraints are satisfied - for (vector6_t::Index j=0; j<6; ++j) { - BOOST_CHECK (g [j] <= eps); + for (vector6_t::Index j = 0; j < 6; ++j) { + BOOST_CHECK(g[j] <= eps); // if constraint is saturated, add gradient in matrix - if (fabs (g [j]) <= eps) { - Mkkt.col (col) = grad_g.col (j); ++col; + if (fabs(g[j]) <= eps) { + Mkkt.col(col) = grad_g.col(j); + ++col; } } if (col > 0) { - Mkkt.conservativeResize (4, col); + Mkkt.conservativeResize(4, col); // Compute KKT coefficients ci by inverting // Mkkt.c = -grad_f - Eigen::JacobiSVD svd - (Mkkt, Eigen::ComputeThinU | Eigen::ComputeThinV); - vector_t c (svd.solve (-grad_f)); - for (vector_t::Index j=0; j < c.size (); ++j) { - BOOST_CHECK_MESSAGE (c [j] >= -eps, - "c[" << j << "]{" << c[j] << "} is below " << -eps); + Eigen::JacobiSVD svd(Mkkt, + Eigen::ComputeThinU | Eigen::ComputeThinV); + vector_t c(svd.solve(-grad_f)); + for (vector_t::Index j = 0; j < c.size(); ++j) { + BOOST_CHECK_MESSAGE(c[j] >= -eps, + "c[" << j << "]{" << c[j] << "} is below " << -eps); } } } @@ -277,30 +284,37 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) // statistics clock_t totalTimeGjkColl = 0; clock_t totalTimeGjkNoColl = 0; - for (std::size_t i=0; i Vec4f; - Transform3f tf0 (Quaternion3f(Vec4f::Random().normalized()), Vec3f::Zero()), - tf1 (Quaternion3f(Vec4f::Random().normalized()), center_distance * ray); + Transform3f tf0(Quaternion3f(Vec4f::Random().normalized()), Vec3f::Zero()), + tf1(Quaternion3f(Vec4f::Random().normalized()), center_distance * ray); details::MinkowskiDiff shape; shape.set(&sphere, &sphere, tf0, tf1); @@ -308,7 +322,7 @@ void test_gjk_unit_sphere (FCL_REAL center_distance, Vec3f ray, BOOST_CHECK_EQUAL(shape.inflation[0], sphere.radius); BOOST_CHECK_EQUAL(shape.inflation[1], sphere.radius); - details::GJK gjk (2, 1e-6); + details::GJK gjk(2, 1e-6); details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); if (expect_collision) @@ -317,37 +331,32 @@ void test_gjk_unit_sphere (FCL_REAL center_distance, Vec3f ray, BOOST_CHECK_EQUAL(status, details::GJK::Valid); Vec3f w0, w1; - gjk.getClosestPoints (shape, w0, w1); + gjk.getClosestPoints(shape, w0, w1); - Vec3f w0_expected (tf0.inverse().transform(tf0.getTranslation() + ray)); - Vec3f w1_expected (tf0.inverse().transform(tf1.getTranslation() - ray)); + Vec3f w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray)); + Vec3f w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray)); EIGEN_VECTOR_IS_APPROX(w0, w0_expected, 1e-10); EIGEN_VECTOR_IS_APPROX(w1, w1_expected, 1e-10); } -BOOST_AUTO_TEST_CASE(sphere_sphere) -{ - test_gjk_unit_sphere(3. , Vec3f(1, 0, 0), false); +BOOST_AUTO_TEST_CASE(sphere_sphere) { + test_gjk_unit_sphere(3., Vec3f(1, 0, 0), false); test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), false); - test_gjk_unit_sphere(2. , Vec3f(1, 0, 0), true); - test_gjk_unit_sphere(1. , Vec3f(1, 0, 0), true); + test_gjk_unit_sphere(2., Vec3f(1, 0, 0), true); + test_gjk_unit_sphere(1., Vec3f(1, 0, 0), true); - test_gjk_unit_sphere(3. , Vec3f::Random().normalized(), false); + test_gjk_unit_sphere(3., Vec3f::Random().normalized(), false); test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), false); - test_gjk_unit_sphere(2. , Vec3f::Random().normalized(), true); - test_gjk_unit_sphere(1. , Vec3f::Random().normalized(), true); + test_gjk_unit_sphere(2., Vec3f::Random().normalized(), true); + test_gjk_unit_sphere(1., Vec3f::Random().normalized(), true); } -void test_gjk_triangle_capsule (Vec3f T, bool expect_collision, - Vec3f w0_expected, Vec3f w1_expected) -{ +void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, + Vec3f w0_expected, Vec3f w1_expected) { using namespace hpp::fcl; - Capsule capsule(1., 2.); // Radius 1 and length 2 - TriangleP triangle ( - Vec3f(0., 0., 0.), - Vec3f(1., 0., 0.), - Vec3f(1., 1., 0.)); + Capsule capsule(1., 2.); // Radius 1 and length 2 + TriangleP triangle(Vec3f(0., 0., 0.), Vec3f(1., 0., 0.), Vec3f(1., 1., 0.)); Transform3f tf0, tf1; tf1.setTranslation(T); @@ -358,7 +367,7 @@ void test_gjk_triangle_capsule (Vec3f T, bool expect_collision, BOOST_CHECK_EQUAL(shape.inflation[0], capsule.radius); BOOST_CHECK_EQUAL(shape.inflation[1], 0.); - details::GJK gjk (10, 1e-6); + details::GJK gjk(10, 1e-6); details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); if (expect_collision) @@ -368,14 +377,14 @@ void test_gjk_triangle_capsule (Vec3f T, bool expect_collision, // Check that guess works as expected Vec3f guess = gjk.getGuessFromSimplex(); - details::GJK gjk2 (3, 1e-6); + details::GJK gjk2(3, 1e-6); details::GJK::Status status2 = gjk2.evaluate(shape, guess); BOOST_CHECK_EQUAL(status2, details::GJK::Valid); } Vec3f w0, w1; if (status == details::GJK::Valid || gjk.hasPenetrationInformation(shape)) { - gjk.getClosestPoints (shape, w0, w1); + gjk.getClosestPoints(shape, w0, w1); } else { details::EPA epa(128, 64, 255, 1e-6); details::EPA::Status epa_status = epa.evaluate(gjk, Vec3f(1, 0, 0)); @@ -384,23 +393,19 @@ void test_gjk_triangle_capsule (Vec3f T, bool expect_collision, } EIGEN_VECTOR_IS_APPROX(w0, w0_expected, 1e-10); - EIGEN_VECTOR_IS_APPROX(w1-T, w1_expected, 1e-10); + EIGEN_VECTOR_IS_APPROX(w1 - T, w1_expected, 1e-10); } -BOOST_AUTO_TEST_CASE(triangle_capsule) -{ +BOOST_AUTO_TEST_CASE(triangle_capsule) { // GJK -> no collision - test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, - Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, Vec3f(1., 0, 0), + Vec3f(0., 0, 0)); // GJK -> collision - test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, - Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, Vec3f(1., 0, 0), + Vec3f(0., 0, 0)); // GJK + EPA -> collision - test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, - Vec3f(0, 1, 0), - Vec3f(0.5, 0, 0)); + test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, Vec3f(0, 1, 0), + Vec3f(0.5, 0, 0)); } diff --git a/test/hfields.cpp b/test/hfields.cpp index ab97c3a54..b16873570 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -34,7 +34,6 @@ /** \author Justin Carpentier */ - #define BOOST_TEST_MODULE FCL_HEIGHT_FIELDS #include #include @@ -55,255 +54,247 @@ using namespace hpp::fcl; -template -void test_constant_hfields(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny, - const FCL_REAL min_altitude, const FCL_REAL max_altitude) -{ +template +void test_constant_hfields(const Eigen::DenseIndex nx, + const Eigen::DenseIndex ny, + const FCL_REAL min_altitude, + const FCL_REAL max_altitude) { const FCL_REAL x_dim = 1., y_dim = 2.; const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); - - HeightField hfield(x_dim,y_dim,heights,min_altitude); + + HeightField hfield(x_dim, y_dim, heights, min_altitude); BOOST_CHECK(hfield.getXDim() == x_dim); BOOST_CHECK(hfield.getYDim() == y_dim); - const VecXf & x_grid = hfield.getXGrid(); - BOOST_CHECK(x_grid[0] == -x_dim/2.); - BOOST_CHECK(x_grid[nx-1] == x_dim/2.); + const VecXf& x_grid = hfield.getXGrid(); + BOOST_CHECK(x_grid[0] == -x_dim / 2.); + BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.); + + const VecXf& y_grid = hfield.getYGrid(); + BOOST_CHECK(y_grid[0] == y_dim / 2.); + BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.); - const VecXf & y_grid = hfield.getYGrid(); - BOOST_CHECK(y_grid[0] == y_dim/2.); - BOOST_CHECK(y_grid[ny-1] == -y_dim/2.); - // Test local AABB hfield.computeLocalAABB(); - - for(Eigen::DenseIndex i = 0; i < nx; ++i) - { - for(Eigen::DenseIndex j = 0; j < ny; ++j) - { - Vec3f point(x_grid[i],y_grid[j],heights(j,i)); + + for (Eigen::DenseIndex i = 0; i < nx; ++i) { + for (Eigen::DenseIndex j = 0; j < ny; ++j) { + Vec3f point(x_grid[i], y_grid[j], heights(j, i)); BOOST_CHECK(hfield.aabb_local.contain(point)); } } - + // Test clone { - HeightField * hfield_clone = hfield.clone(); + HeightField* hfield_clone = hfield.clone(); hfield_clone->computeLocalAABB(); BOOST_CHECK(*hfield_clone == hfield); - + delete hfield_clone; } - + // Build equivalent object - const Box equivalent_box(x_dim,y_dim,max_altitude - min_altitude); - const Transform3f box_placement(Matrix3f::Identity(), - Vec3f(0.,0.,(max_altitude + min_altitude)/2.)); - + const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); + const Transform3f box_placement( + Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); + // Test collision const Sphere sphere(1.); static const Transform3f IdTransform; - + const Box box(Vec3f::Ones()); - + Transform3f M_sphere, M_box; - + // No collision case { - const FCL_REAL eps_no_collision = +0.1*(max_altitude - min_altitude); - M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_no_collision)); - M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_no_collision)); + const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; - + CollisionResult result; - collide(&hfield,IdTransform, - &sphere,M_sphere, - request,result); - + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + BOOST_CHECK(!result.isCollision()); - + CollisionResult result_check_sphere; - collide(&equivalent_box,IdTransform*box_placement, - &sphere,M_sphere, - request,result_check_sphere); - + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + BOOST_CHECK(!result_check_sphere.isCollision()); - + CollisionResult result_check_box; - collide(&equivalent_box,IdTransform*box_placement, - &box,M_box, - request,result_check_box); - + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + BOOST_CHECK(!result_check_box.isCollision()); } - + // Collision case { - const FCL_REAL eps_collision = -0.1*(max_altitude - min_altitude); - M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_collision)); - M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_collision)); - CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); - + const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + CollisionRequest + request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); + CollisionResult result; - collide(&hfield,IdTransform, - &sphere,M_sphere, - request,result); + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(result.isCollision()); - + CollisionResult result_check; - collide(&equivalent_box,IdTransform*box_placement, - &sphere,M_sphere, - request,result_check); - + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check); + BOOST_CHECK(result_check.isCollision()); - + CollisionResult result_check_box; - collide(&equivalent_box,IdTransform*box_placement, - &box,M_box, - request,result_check_box); - + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + BOOST_CHECK(result_check_box.isCollision()); } - + // Update height - hfield.updateHeights(MatrixXf::Constant(ny, nx, max_altitude/2.)); // We change nothing - + hfield.updateHeights( + MatrixXf::Constant(ny, nx, max_altitude / 2.)); // We change nothing + // No collision case { - const FCL_REAL eps_no_collision = +0.1*(max_altitude - min_altitude); - M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_no_collision)); - M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_no_collision)); + const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; - + CollisionResult result; - collide(&hfield,IdTransform, - &sphere,M_sphere, - request,result); - + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + BOOST_CHECK(!result.isCollision()); - + CollisionResult result_check_sphere; - collide(&equivalent_box,IdTransform*box_placement, - &sphere,M_sphere, - request,result_check_sphere); - + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + BOOST_CHECK(!result_check_sphere.isCollision()); - + CollisionResult result_check_box; - collide(&equivalent_box,IdTransform*box_placement, - &box,M_box, - request,result_check_box); - + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + BOOST_CHECK(!result_check_box.isCollision()); } - + // Collision case { - const FCL_REAL eps_collision = -0.1*(max_altitude - min_altitude); - M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_collision)); - M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_collision)); - CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); - + const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + CollisionRequest + request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); + CollisionResult result; - collide(&hfield,IdTransform, - &sphere,M_sphere, - request,result); + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(!result.isCollision()); - + CollisionResult result_check; - collide(&equivalent_box,IdTransform*box_placement, - &sphere,M_sphere, - request,result_check); - + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check); + BOOST_CHECK(result_check.isCollision()); - + CollisionResult result_check_box; - collide(&equivalent_box,IdTransform*box_placement, - &box,M_box, - request,result_check_box); - + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + BOOST_CHECK(result_check_box.isCollision()); } - + // Restore height - hfield.updateHeights(MatrixXf::Constant(ny, nx, max_altitude)); // We change nothing - + hfield.updateHeights( + MatrixXf::Constant(ny, nx, max_altitude)); // We change nothing + // Collision case { - const FCL_REAL eps_collision = -0.1*(max_altitude - min_altitude); - M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_collision)); - M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_collision)); - CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); - + const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + CollisionRequest + request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); + CollisionResult result; - collide(&hfield,IdTransform, - &sphere,M_sphere, - request,result); + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); BOOST_CHECK(result.isCollision()); - + CollisionResult result_check; - collide(&equivalent_box,IdTransform*box_placement, - &sphere,M_sphere, - request,result_check); - + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check); + BOOST_CHECK(result_check.isCollision()); - + CollisionResult result_check_box; - collide(&equivalent_box,IdTransform*box_placement, - &box,M_box, - request,result_check_box); - + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + BOOST_CHECK(result_check_box.isCollision()); } - } -BOOST_AUTO_TEST_CASE(building_constant_hfields) -{ +BOOST_AUTO_TEST_CASE(building_constant_hfields) { const FCL_REAL max_altitude = 1., min_altitude = 0.; - - test_constant_hfields(2,2,min_altitude,max_altitude); // Simple case - test_constant_hfields(20,2,min_altitude,max_altitude); - test_constant_hfields(100,100,min_altitude,max_altitude); -// test_constant_hfields(1000,1000,min_altitude,max_altitude); - - test_constant_hfields(2,2,min_altitude,max_altitude); // Simple case - test_constant_hfields(20,2,min_altitude,max_altitude); - test_constant_hfields(100,100,min_altitude,max_altitude); + + test_constant_hfields(2, 2, min_altitude, + max_altitude); // Simple case + test_constant_hfields(20, 2, min_altitude, max_altitude); + test_constant_hfields(100, 100, min_altitude, max_altitude); + // test_constant_hfields(1000,1000,min_altitude,max_altitude); + + test_constant_hfields(2, 2, min_altitude, max_altitude); // Simple case + test_constant_hfields(20, 2, min_altitude, max_altitude); + test_constant_hfields(100, 100, min_altitude, max_altitude); } -BOOST_AUTO_TEST_CASE(hfield_with_square_hole) -{ +BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { const Eigen::DenseIndex nx = 100, ny = 100; - + typedef AABB BV; - const MatrixXf X = Eigen::RowVectorXd::LinSpaced(nx,-1.,1.).replicate(ny,1); - const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny,1.,-1.).replicate(1,nx); - + const MatrixXf X = + Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); + const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); + const FCL_REAL dim_square = 0.5; - - const Eigen::Array hole = (X.array().abs() < dim_square) && (Y.array().abs() < dim_square); - - const MatrixXf heights = MatrixXf::Ones(ny,nx) - hole.cast().matrix(); - - const HeightField hfield(2.,2.,heights,-10.); - + + const Eigen::Array hole = + (X.array().abs() < dim_square) && (Y.array().abs() < dim_square); + + const MatrixXf heights = + MatrixXf::Ones(ny, nx) - hole.cast().matrix(); + + const HeightField hfield(2., 2., heights, -10.); + Sphere sphere(0.48); - const Transform3f sphere_pos(Vec3f(0.,0.,0.5)); + const Transform3f sphere_pos(Vec3f(0., 0., 0.5)); const Transform3f hfield_pos; - + const CollisionRequest request; - - + CollisionResult result; { - collide(&hfield,hfield_pos, - &sphere,sphere_pos, - request,result); + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); BOOST_CHECK(!result.isCollision()); } @@ -313,9 +304,7 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { CollisionResult result; const Sphere sphere2(0.51); - collide(&hfield,hfield_pos, - &sphere2,sphere_pos, - request,result); + collide(&hfield, hfield_pos, &sphere2, sphere_pos, request, result); BOOST_CHECK(result.isCollision()); } diff --git a/test/math.cpp b/test/math.cpp index 0a2f320fd..f26ebb341 100644 --- a/test/math.cpp +++ b/test/math.cpp @@ -47,10 +47,7 @@ using namespace hpp::fcl; - - -BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) -{ +BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) { Vec3f v1(1.0, 2.0, 3.0); BOOST_CHECK(v1[0] == 1.0); BOOST_CHECK(v1[1] == 2.0); @@ -102,43 +99,42 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5); BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm())); - v1 = Vec3f(1.0, 2.0, 3.0); v2 = Vec3f(3.0, 4.0, 5.0); BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-2.0, 4.0, -2.0))); BOOST_CHECK(v1.dot(v2) == 26); } -Vec3f rotate (Vec3f input, FCL_REAL w, Vec3f vec) { - return 2*vec.dot(input)*vec + (w*w - vec.dot(vec))*input + 2*w*vec.cross(input); +Vec3f rotate(Vec3f input, FCL_REAL w, Vec3f vec) { + return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input + + 2 * w * vec.cross(input); } -BOOST_AUTO_TEST_CASE(quaternion) -{ - Quaternion3f q1 (Quaternion3f::Identity()), q2, q3; - q2 = fromAxisAngle(Vec3f(0,0,1), M_PI/2); +BOOST_AUTO_TEST_CASE(quaternion) { + Quaternion3f q1(Quaternion3f::Identity()), q2, q3; + q2 = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2); q3 = q2.inverse(); - Vec3f v(1,-1,0); + Vec3f v(1, -1, 0); BOOST_CHECK(isEqual(v, q1 * v)); - BOOST_CHECK(isEqual(Vec3f(1,1,0), q2 * v)); - BOOST_CHECK(isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v)); + BOOST_CHECK(isEqual(Vec3f(1, 1, 0), q2 * v)); + BOOST_CHECK( + isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v)); } -BOOST_AUTO_TEST_CASE(transform) -{ - Quaternion3f q = fromAxisAngle(Vec3f(0,0,1), M_PI/2); - Vec3f T (0,1,2); - Transform3f tf (q, T); +BOOST_AUTO_TEST_CASE(transform) { + Quaternion3f q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2); + Vec3f T(0, 1, 2); + Transform3f tf(q, T); - Vec3f v(1,-1,0); + Vec3f v(1, -1, 0); BOOST_CHECK(isEqual(q * v + T, q * v + T)); - Vec3f rv (q * v); + Vec3f rv(q * v); // typename Transform3f::transform_return_type::type output = - // tf * v; + // tf * v; // std::cout << rv << std::endl; // std::cout << output.lhs() << std::endl; BOOST_CHECK(isEqual(rv + T, tf.transform(v))); diff --git a/test/obb.cpp b/test/obb.cpp index b4d2fefe4..f9e6c3484 100644 --- a/test/obb.cpp +++ b/test/obb.cpp @@ -49,8 +49,7 @@ using namespace hpp::fcl; -int getNumCPUs() -{ +int getNumCPUs() { int NumCPUs = 0; int MaxID = -1; std::ifstream f("/proc/cpuinfo"); @@ -68,7 +67,7 @@ int getNumCPUs() if (ln.size() >= Key.size() && ln.compare(0, Key.size(), Key) == 0) { NumCPUs++; if (!value.empty()) { - int CurID = (int) strtol(value.c_str(), NULL, 10); + int CurID = (int)strtol(value.c_str(), NULL, 10); MaxID = std::max(CurID, MaxID); } } @@ -91,9 +90,8 @@ int getNumCPUs() return NumCPUs; } -bool checkCpuScalingEnabled() -{ - int num_cpus = getNumCPUs (); +bool checkCpuScalingEnabled() { + int num_cpus = getNumCPUs(); // We don't have a valid CPU count, so don't even bother. if (num_cpus <= 0) return false; @@ -114,31 +112,27 @@ bool checkCpuScalingEnabled() return false; } -void randomOBBs ( - Vec3f& a, Vec3f& b, FCL_REAL extentNorm) -{ +void randomOBBs(Vec3f& a, Vec3f& b, FCL_REAL extentNorm) { // Extent norm is between 0 and extentNorm on each axis - //a = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); - //b = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); + // a = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); + // b = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); a = extentNorm * Vec3f::Random().cwiseAbs().normalized(); b = extentNorm * Vec3f::Random().cwiseAbs().normalized(); } -void randomTransform ( - Matrix3f& B, Vec3f& T, - const Vec3f& a, const Vec3f& b, const FCL_REAL extentNorm) -{ +void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b, + const FCL_REAL extentNorm) { // TODO Should we scale T to a and b norm ? - (void) a; - (void) b; - (void) extentNorm; + (void)a; + (void)b; + (void)extentNorm; FCL_REAL N = a.norm() + b.norm(); // A translation of norm N ensures there is no collision. // Set translation to be between 0 and 2 * N; - T = ( Vec3f::Random() / sqrt(3) ) * 1.5 * N; - //T.setZero(); + T = (Vec3f::Random() / sqrt(3)) * 1.5 * N; + // T.setZero(); Quaternion3f q; q.coeffs().setRandom(); @@ -150,1032 +144,1056 @@ void randomTransform ( #define MANUAL_PRODUCT 1 #if MANUAL_PRODUCT -# define PRODUCT(M33,v3) ( M33.col(0) * v3[0] + M33.col(1) * v3[1] + M33.col(2) * v3[2] ) +#define PRODUCT(M33, v3) \ + (M33.col(0) * v3[0] + M33.col(1) * v3[1] + M33.col(2) * v3[2]) #else -# define PRODUCT(M33,v3) (M33*v3) +#define PRODUCT(M33, v3) (M33 * v3) #endif typedef boost::chrono::high_resolution_clock clock_type; typedef clock_type::duration duration_type; -const char* sep = ",\t"; +const char* sep = ",\t"; const FCL_REAL eps = 1.5e-7; -const Eigen::IOFormat py_fmt(Eigen::FullPrecision, - 0, - ", ", // Coeff separator - ",\n", // Row separator - "(", // row prefix - ",)", // row suffix - "( ", // mat prefix - ", )" // mat suffix - ); - -namespace obbDisjoint_impls -{ - /// @return true if OBB are disjoint. - bool distance (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, FCL_REAL& distance) - { - GJKSolver gjk; - Box ba (2*a), bb (2*b); - Transform3f tfa, tfb (B, T); - - Vec3f p1, p2, normal; - return gjk.shapeDistance (ba, tfa, bb, tfb, distance, p1, p2, normal); - } +const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, + ", ", // Coeff separator + ",\n", // Row separator + "(", // row prefix + ",)", // row suffix + "( ", // mat prefix + ", )" // mat suffix +); + +namespace obbDisjoint_impls { +/// @return true if OBB are disjoint. +bool distance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, + FCL_REAL& distance) { + GJKSolver gjk; + Box ba(2 * a), bb(2 * b); + Transform3f tfa, tfb(B, T); + + Vec3f p1, p2, normal; + return gjk.shapeDistance(ba, tfa, bb, tfb, distance, p1, p2, normal); +} - inline FCL_REAL _computeDistanceForCase1 ( - const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf) - { - Vec3f AABB_corner; - /* This seems to be slower - AABB_corner.noalias() = T.cwiseAbs () - a; - AABB_corner.noalias() -= PRODUCT(Bf,b); - /*/ +inline FCL_REAL _computeDistanceForCase1(const Vec3f& T, const Vec3f& a, + const Vec3f& b, const Matrix3f& Bf) { + Vec3f AABB_corner; + /* This seems to be slower + AABB_corner.noalias() = T.cwiseAbs () - a; + AABB_corner.noalias() -= PRODUCT(Bf,b); + /*/ #if MANUAL_PRODUCT - AABB_corner.noalias() = T.cwiseAbs () - a; - AABB_corner.noalias() -= Bf.col(0) * b[0]; - AABB_corner.noalias() -= Bf.col(1) * b[1]; - AABB_corner.noalias() -= Bf.col(2) * b[2]; + AABB_corner.noalias() = T.cwiseAbs() - a; + AABB_corner.noalias() -= Bf.col(0) * b[0]; + AABB_corner.noalias() -= Bf.col(1) * b[1]; + AABB_corner.noalias() -= Bf.col(2) * b[2]; #else - AABB_corner.noalias() = T.cwiseAbs () - Bf * b - a; + AABB_corner.noalias() = T.cwiseAbs() - Bf * b - a; #endif - // */ - return AABB_corner.array().max(0).matrix().squaredNorm (); - } + // */ + return AABB_corner.array().max(0).matrix().squaredNorm(); +} - inline FCL_REAL _computeDistanceForCase2 ( - const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf) - { - /* - Vec3f AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); - AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a); - return AABB_corner.array().max(0).matrix().squaredNorm (); - /*/ +inline FCL_REAL _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const Matrix3f& Bf) { + /* + Vec3f AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); + AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a); + return AABB_corner.array().max(0).matrix().squaredNorm (); + /*/ #if MANUAL_PRODUCT - FCL_REAL s, t = 0; - s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; - if (s > 0) t += s*s; - s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; - if (s > 0) t += s*s; - s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2]; - if (s > 0) t += s*s; - return t; + FCL_REAL s, t = 0; + s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; + if (s > 0) t += s * s; + s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; + if (s > 0) t += s * s; + s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2]; + if (s > 0) t += s * s; + return t; #else - Vec3f AABB_corner((B.transpose () * T).cwiseAbs () - Bf.transpose () * a - b); - return AABB_corner.array().max(0).matrix().squaredNorm (); + Vec3f AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b); + return AABB_corner.array().max(0).matrix().squaredNorm(); #endif - // */ - } + // */ +} - int separatingAxisId (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - int id = 0; - - Matrix3f Bf (B.cwiseAbs()); - - // Corner of b axis aligned bounding box the closest to the origin - squaredLowerBoundDistance = _computeDistanceForCase1 (T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return id; - ++id; - - // | B^T T| - b - Bf^T a - squaredLowerBoundDistance = _computeDistanceForCase2 (B, T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return id; - ++id; - - int ja = 1, ka = 2, jb = 1, kb = 2; - for (int ia = 0; ia < 3; ++ia) { - for (int ib = 0; ib < 3; ++ib) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); - // We need to divide by the norm || Aia x Bib || - // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 - if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return id; - } - } - /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - squaredLowerBoundDistance = diff * diff; - if (squaredLowerBoundDistance > breakDistance2 * sinus2) { - squaredLowerBoundDistance /= sinus2; - return true; - } - // */ - } - ++id; +int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, + const Vec3f& b, const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + int id = 0; - jb = kb; kb = ib; - } - ja = ka; ka = ia; - } + Matrix3f Bf(B.cwiseAbs()); - return id; - } + // Corner of b axis aligned bounding box the closest to the origin + squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return id; + ++id; - // ------------------------ 0 -------------------------------------- - bool withRuntimeLoop (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - Matrix3f Bf (B.cwiseAbs()); + // | B^T T| - b - Bf^T a + squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return id; + ++id; - // Corner of b axis aligned bounding box the closest to the origin - squaredLowerBoundDistance = _computeDistanceForCase1 (T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return true; - - // | B^T T| - b - Bf^T a - squaredLowerBoundDistance = _computeDistanceForCase2 (B, T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return true; + int ja = 1, ka = 2, jb = 1, kb = 2; + for (int ia = 0; ia < 3; ++ia) { + for (int ib = 0; ib < 3; ++ib) { + const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - int ja = 1, ka = 2, jb = 1, kb = 2; - for (int ia = 0; ia < 3; ++ia) { - for (int ib = 0; ib < 3; ++ib) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); - // We need to divide by the norm || Aia x Bib || - // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 - if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + // We need to divide by the norm || Aia x Bib || + // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 + if (diff > 0) { + FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return id; } - /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - squaredLowerBoundDistance = diff * diff; - if (squaredLowerBoundDistance > breakDistance2 * sinus2) { - squaredLowerBoundDistance /= sinus2; - return true; - } - // */ } - - jb = kb; kb = ib; + /* // or + FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + squaredLowerBoundDistance = diff * diff; + if (squaredLowerBoundDistance > breakDistance2 * sinus2) { + squaredLowerBoundDistance /= sinus2; + return true; + } + // */ } - ja = ka; ka = ia; + ++id; + + jb = kb; + kb = ib; } + ja = ka; + ka = ia; + } - return false; + return id; +} - } +// ------------------------ 0 -------------------------------------- +bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, + const Vec3f& b, const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + Matrix3f Bf(B.cwiseAbs()); - // ------------------------ 1 -------------------------------------- - bool withManualLoopUnrolling_1 (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - FCL_REAL t, s; - FCL_REAL diff; - - // Matrix3f Bf = abs(B); - // Bf += reps; - Matrix3f Bf (B.cwiseAbs()); - - // Corner of b axis aligned bounding box the closest to the origin - Vec3f AABB_corner (T.cwiseAbs () - Bf * b); - Vec3f diff3 (AABB_corner - a); - diff3 = diff3.cwiseMax (Vec3f::Zero()); - - //for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); - squaredLowerBoundDistance = diff3.squaredNorm (); - if (squaredLowerBoundDistance > breakDistance2) - return true; + // Corner of b axis aligned bounding box the closest to the origin + squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; - AABB_corner = (B.transpose () * T).cwiseAbs () - Bf.transpose () * a; - // diff3 = | B^T T| - b - Bf^T a - diff3 = AABB_corner - b; - diff3 = diff3.cwiseMax (Vec3f::Zero()); - squaredLowerBoundDistance = diff3.squaredNorm (); + // | B^T T| - b - Bf^T a + squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; - if (squaredLowerBoundDistance > breakDistance2) - return true; + int ja = 1, ka = 2, jb = 1, kb = 2; + for (int ia = 0; ia < 3; ++ia) { + for (int ib = 0; ib < 3; ++ib) { + const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - // A0 x B0 - s = T[2] * B(1, 0) - T[1] * B(2, 0); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - FCL_REAL sinus2; - diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + - b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); - // We need to divide by the norm || A0 x B0 || - // As ||A0|| = ||B0|| = 1, - // 2 2 - // || A0 x B0 || + (A0 | B0) = 1 - if (diff > 0) { - sinus2 = 1 - Bf (0,0) * Bf (0,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; + const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + // We need to divide by the norm || Aia x Bib || + // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 + if (diff > 0) { + FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; + } } + /* // or + FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + squaredLowerBoundDistance = diff * diff; + if (squaredLowerBoundDistance > breakDistance2 * sinus2) { + squaredLowerBoundDistance /= sinus2; + return true; + } + // */ } + + jb = kb; + kb = ib; } + ja = ka; + ka = ia; + } - // A0 x B1 - s = T[2] * B(1, 1) - T[1] * B(2, 1); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); + return false; +} - diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + - b[0] * Bf(0, 2) + b[2] * Bf(0, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (0,1) * Bf (0,1); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } +// ------------------------ 1 -------------------------------------- +bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + FCL_REAL t, s; + FCL_REAL diff; + + // Matrix3f Bf = abs(B); + // Bf += reps; + Matrix3f Bf(B.cwiseAbs()); + + // Corner of b axis aligned bounding box the closest to the origin + Vec3f AABB_corner(T.cwiseAbs() - Bf * b); + Vec3f diff3(AABB_corner - a); + diff3 = diff3.cwiseMax(Vec3f::Zero()); + + // for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); + squaredLowerBoundDistance = diff3.squaredNorm(); + if (squaredLowerBoundDistance > breakDistance2) return true; + + AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a; + // diff3 = | B^T T| - b - Bf^T a + diff3 = AABB_corner - b; + diff3 = diff3.cwiseMax(Vec3f::Zero()); + squaredLowerBoundDistance = diff3.squaredNorm(); + + if (squaredLowerBoundDistance > breakDistance2) return true; + + // A0 x B0 + s = T[2] * B(1, 0) - T[1] * B(2, 0); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + FCL_REAL sinus2; + diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + + b[2] * Bf(0, 1)); + // We need to divide by the norm || A0 x B0 || + // As ||A0|| = ||B0|| = 1, + // 2 2 + // || A0 x B0 || + (A0 | B0) = 1 + if (diff > 0) { + sinus2 = 1 - Bf(0, 0) * Bf(0, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A0 x B2 - s = T[2] * B(1, 2) - T[1] * B(2, 2); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + - b[0] * Bf(0, 1) + b[1] * Bf(0, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (0,2) * Bf (0,2); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A0 x B1 + s = T[2] * B(1, 1) - T[1] * B(2, 1); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + + b[2] * Bf(0, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(0, 1) * Bf(0, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B0 - s = T[0] * B(2, 0) - T[2] * B(0, 0); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + - b[1] * Bf(1, 2) + b[2] * Bf(1, 1)); - if (diff > 0) { - sinus2 = 1 - Bf (1,0) * Bf (1,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A0 x B2 + s = T[2] * B(1, 2) - T[1] * B(2, 2); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + + b[1] * Bf(0, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(0, 2) * Bf(0, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B1 - s = T[0] * B(2, 1) - T[2] * B(0, 1); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + - b[0] * Bf(1, 2) + b[2] * Bf(1, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (1,1) * Bf (1,1); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A1 x B0 + s = T[0] * B(2, 0) - T[2] * B(0, 0); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + + b[2] * Bf(1, 1)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 0) * Bf(1, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B2 - s = T[0] * B(2, 2) - T[2] * B(0, 2); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + - b[0] * Bf(1, 1) + b[1] * Bf(1, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (1,2) * Bf (1,2); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A1 x B1 + s = T[0] * B(2, 1) - T[2] * B(0, 1); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + + b[2] * Bf(1, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 1) * Bf(1, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A2 x B0 - s = T[1] * B(0, 0) - T[0] * B(1, 0); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + - b[1] * Bf(2, 2) + b[2] * Bf(2, 1)); - if (diff > 0) { - sinus2 = 1 - Bf (2,0) * Bf (2,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A1 x B2 + s = T[0] * B(2, 2) - T[2] * B(0, 2); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + + b[1] * Bf(1, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 2) * Bf(1, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A2 x B1 - s = T[1] * B(0, 1) - T[0] * B(1, 1); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + - b[0] * Bf(2, 2) + b[2] * Bf(2, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (2,1) * Bf (2,1); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A2 x B0 + s = T[1] * B(0, 0) - T[0] * B(1, 0); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + + b[2] * Bf(2, 1)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 0) * Bf(2, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A2 x B2 - s = T[1] * B(0, 2) - T[0] * B(1, 2); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + - b[0] * Bf(2, 1) + b[1] * Bf(2, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (2,2) * Bf (2,2); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A2 x B1 + s = T[1] * B(0, 1) - T[0] * B(1, 1); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + + b[2] * Bf(2, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 1) * Bf(2, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } - - return false; - } - // ------------------------ 2 -------------------------------------- - bool withManualLoopUnrolling_2 (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - Matrix3f Bf (B.cwiseAbs()); - - // Corner of b axis aligned bounding box the closest to the origin - squaredLowerBoundDistance = _computeDistanceForCase1 (T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return true; - - squaredLowerBoundDistance = _computeDistanceForCase2 (B, T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return true; - - // A0 x B0 - FCL_REAL t, s; - s = T[2] * B(1, 0) - T[1] * B(2, 0); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - FCL_REAL sinus2; - FCL_REAL diff; - diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + - b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); - // We need to divide by the norm || A0 x B0 || - // As ||A0|| = ||B0|| = 1, - // 2 2 - // || A0 x B0 || + (A0 | B0) = 1 - if (diff > 0) { - sinus2 = 1 - Bf (0,0) * Bf (0,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A2 x B2 + s = T[1] * B(0, 2) - T[0] * B(1, 2); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + + b[1] * Bf(2, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 2) * Bf(2, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A0 x B1 - s = T[2] * B(1, 1) - T[1] * B(2, 1); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); + return false; +} - diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + - b[0] * Bf(0, 2) + b[2] * Bf(0, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (0,1) * Bf (0,1); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } +// ------------------------ 2 -------------------------------------- +bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + Matrix3f Bf(B.cwiseAbs()); + + // Corner of b axis aligned bounding box the closest to the origin + squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; + + squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; + + // A0 x B0 + FCL_REAL t, s; + s = T[2] * B(1, 0) - T[1] * B(2, 0); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + FCL_REAL sinus2; + FCL_REAL diff; + diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + + b[2] * Bf(0, 1)); + // We need to divide by the norm || A0 x B0 || + // As ||A0|| = ||B0|| = 1, + // 2 2 + // || A0 x B0 || + (A0 | B0) = 1 + if (diff > 0) { + sinus2 = 1 - Bf(0, 0) * Bf(0, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A0 x B2 - s = T[2] * B(1, 2) - T[1] * B(2, 2); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + - b[0] * Bf(0, 1) + b[1] * Bf(0, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (0,2) * Bf (0,2); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A0 x B1 + s = T[2] * B(1, 1) - T[1] * B(2, 1); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + + b[2] * Bf(0, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(0, 1) * Bf(0, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B0 - s = T[0] * B(2, 0) - T[2] * B(0, 0); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); - - diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + - b[1] * Bf(1, 2) + b[2] * Bf(1, 1)); - if (diff > 0) { - sinus2 = 1 - Bf (1,0) * Bf (1,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A0 x B2 + s = T[2] * B(1, 2) - T[1] * B(2, 2); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + + b[1] * Bf(0, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(0, 2) * Bf(0, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B1 - s = T[0] * B(2, 1) - T[2] * B(0, 1); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); + // A1 x B0 + s = T[0] * B(2, 0) - T[2] * B(0, 0); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + + b[2] * Bf(1, 1)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 0) * Bf(1, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; + } + } + } - diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + - b[0] * Bf(1, 2) + b[2] * Bf(1, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (1,1) * Bf (1,1); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A1 x B1 + s = T[0] * B(2, 1) - T[2] * B(0, 1); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + + b[2] * Bf(1, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 1) * Bf(1, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B2 - s = T[0] * B(2, 2) - T[2] * B(0, 2); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); + // A1 x B2 + s = T[0] * B(2, 2) - T[2] * B(0, 2); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + + b[1] * Bf(1, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 2) * Bf(1, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; + } + } + } - diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + - b[0] * Bf(1, 1) + b[1] * Bf(1, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (1,2) * Bf (1,2); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A2 x B0 + s = T[1] * B(0, 0) - T[0] * B(1, 0); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + + b[2] * Bf(2, 1)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 0) * Bf(2, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A2 x B0 - s = T[1] * B(0, 0) - T[0] * B(1, 0); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); + // A2 x B1 + s = T[1] * B(0, 1) - T[0] * B(1, 1); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + + b[2] * Bf(2, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 1) * Bf(2, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; + } + } + } - diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + - b[1] * Bf(2, 2) + b[2] * Bf(2, 1)); - if (diff > 0) { - sinus2 = 1 - Bf (2,0) * Bf (2,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A2 x B2 + s = T[1] * B(0, 2) - T[0] * B(1, 2); + t = ((s < 0.0) ? -s : s); + assert(t == fabs(s)); + + diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + + b[1] * Bf(2, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 2) * Bf(2, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A2 x B1 - s = T[1] * B(0, 1) - T[0] * B(1, 1); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); + return false; +} - diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + - b[0] * Bf(2, 2) + b[2] * Bf(2, 0)); +// ------------------------ 3 -------------------------------------- +template +struct loop_case_1 { + static inline bool run(const Matrix3f& B, const Vec3f& T, const Vec3f& a, + const Vec3f& b, const Matrix3f& Bf, + const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + + const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + // We need to divide by the norm || Aia x Bib || + // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - sinus2 = 1 - Bf (2,1) * Bf (2,1); + FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } + /* // or + FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + squaredLowerBoundDistance = diff * diff; + if (squaredLowerBoundDistance > breakDistance2 * sinus2) { + squaredLowerBoundDistance /= sinus2; + return true; + } + // */ } + return false; + } +}; + +bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + Matrix3f Bf(B.cwiseAbs()); + + // Corner of b axis aligned bounding box the closest to the origin + squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; + + squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; + + // Ai x Bj + if (loop_case_1<0, 0>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (loop_case_1<0, 1>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (loop_case_1<0, 2>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (loop_case_1<1, 0>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (loop_case_1<1, 1>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (loop_case_1<1, 2>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (loop_case_1<2, 0>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (loop_case_1<2, 1>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + if (loop_case_1<2, 2>::run(B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + + return false; +} - // A2 x B2 - s = T[1] * B(0, 2) - T[0] * B(1, 2); - t = ((s < 0.0) ? -s : s); assert (t == fabs (s)); +// ------------------------ 4 -------------------------------------- - diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + - b[0] * Bf(2, 1) + b[1] * Bf(2, 0)); +template +struct loop_case_2 { + static inline bool run(int ia, int ja, int ka, const Matrix3f& B, + const Vec3f& T, const Vec3f& a, const Vec3f& b, + const Matrix3f& Bf, const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + + const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + // We need to divide by the norm || Aia x Bib || + // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - sinus2 = 1 - Bf (2,2) * Bf (2,2); + FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { return true; } } + /* // or + FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + squaredLowerBoundDistance = diff * diff; + if (squaredLowerBoundDistance > breakDistance2 * sinus2) { + squaredLowerBoundDistance /= sinus2; + return true; + } + // */ } - return false; - } +}; - // ------------------------ 3 -------------------------------------- - template - struct loop_case_1 { - static inline bool run ( - const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf, - const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); +bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + Matrix3f Bf(B.cwiseAbs()); - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); - // We need to divide by the norm || Aia x Bib || - // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 - if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } - } - /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - squaredLowerBoundDistance = diff * diff; - if (squaredLowerBoundDistance > breakDistance2 * sinus2) { - squaredLowerBoundDistance /= sinus2; - return true; - } - // */ - } - return false; - } - }; + // Corner of b axis aligned bounding box the closest to the origin + squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; - bool withTemplateLoopUnrolling_1 (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - Matrix3f Bf (B.cwiseAbs()); + squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); + if (squaredLowerBoundDistance > breakDistance2) return true; - // Corner of b axis aligned bounding box the closest to the origin - squaredLowerBoundDistance = _computeDistanceForCase1 (T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) + // Ai x Bj + int ja = 1, ka = 2; + for (int ia = 0; ia < 3; ++ia) { + if (loop_case_2<0>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) return true; - - squaredLowerBoundDistance = _computeDistanceForCase2 (B, T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) + if (loop_case_2<1>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) return true; - - // Ai x Bj - if (loop_case_1<0,0>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_1<0,1>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_1<0,2>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_1<1,0>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_1<1,1>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_1<1,2>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_1<2,0>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_1<2,1>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_1<2,2>::run (B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - - return false; + if (loop_case_2<2>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, + squaredLowerBoundDistance)) + return true; + ja = ka; + ka = ia; } - // ------------------------ 4 -------------------------------------- - - template - struct loop_case_2 - { - static inline bool run (int ia, int ja, int ka, - const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf, - const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); - // We need to divide by the norm || Aia x Bib || - // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 - if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } - } - /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); - squaredLowerBoundDistance = diff * diff; - if (squaredLowerBoundDistance > breakDistance2 * sinus2) { - squaredLowerBoundDistance /= sinus2; - return true; - } - // */ - } - return false; - } - }; + return false; +} - bool withPartialTemplateLoopUnrolling_1 (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - Matrix3f Bf (B.cwiseAbs()); +// ------------------------ 5 -------------------------------------- +bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, + const Vec3f& b, const FCL_REAL& breakDistance2, + FCL_REAL& squaredLowerBoundDistance) { + FCL_REAL t, s; + FCL_REAL diff; - // Corner of b axis aligned bounding box the closest to the origin - squaredLowerBoundDistance = _computeDistanceForCase1 (T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return true; + Matrix3f Bf(B.cwiseAbs()); + squaredLowerBoundDistance = 0; - squaredLowerBoundDistance = _computeDistanceForCase2 (B, T, a, b, Bf); - if (squaredLowerBoundDistance > breakDistance2) - return true; + // if any of these tests are one-sided, then the polyhedra are disjoint - // Ai x Bj - int ja = 1, ka = 2; - for (int ia = 0; ia < 3; ++ia) { - if (loop_case_2<0>::run (ia, ja, ka, - B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_2<1>::run (ia, ja, ka, - B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - if (loop_case_2<2>::run (ia, ja, ka, - B, T, a, b, Bf, breakDistance2, squaredLowerBoundDistance)) return true; - ja = ka; ka = ia; - } + // A1 x A2 = A0 + t = ((T[0] < 0.0) ? -T[0] : T[0]); - return false; + diff = t - (a[0] + Bf.row(0).dot(b)); + if (diff > 0) { + squaredLowerBoundDistance = diff * diff; } - // ------------------------ 5 -------------------------------------- - bool originalWithLowerBound (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance) - { - FCL_REAL t, s; - FCL_REAL diff; - - Matrix3f Bf (B.cwiseAbs()); - squaredLowerBoundDistance = 0; - - // if any of these tests are one-sided, then the polyhedra are disjoint - - // A1 x A2 = A0 - t = ((T[0] < 0.0) ? -T[0] : T[0]); - - diff = t - (a[0] + Bf.row(0).dot(b)); - if (diff > 0) { - squaredLowerBoundDistance = diff*diff; - } - - // A2 x A0 = A1 - t = ((T[1] < 0.0) ? -T[1] : T[1]); + // A2 x A0 = A1 + t = ((T[1] < 0.0) ? -T[1] : T[1]); - diff = t - (a[1] + Bf.row(1).dot(b)); - if (diff > 0) { - squaredLowerBoundDistance += diff*diff; - } + diff = t - (a[1] + Bf.row(1).dot(b)); + if (diff > 0) { + squaredLowerBoundDistance += diff * diff; + } - // A0 x A1 = A2 - t =((T[2] < 0.0) ? -T[2] : T[2]); + // A0 x A1 = A2 + t = ((T[2] < 0.0) ? -T[2] : T[2]); - diff = t - (a[2] + Bf.row(2).dot(b)); - if (diff > 0) { - squaredLowerBoundDistance += diff*diff; - } + diff = t - (a[2] + Bf.row(2).dot(b)); + if (diff > 0) { + squaredLowerBoundDistance += diff * diff; + } - if (squaredLowerBoundDistance > breakDistance2) - return true; + if (squaredLowerBoundDistance > breakDistance2) return true; - squaredLowerBoundDistance = 0; + squaredLowerBoundDistance = 0; - // B1 x B2 = B0 - s = B.col(0).dot(T); - t = ((s < 0.0) ? -s : s); + // B1 x B2 = B0 + s = B.col(0).dot(T); + t = ((s < 0.0) ? -s : s); - diff = t - (b[0] + Bf.col(0).dot(a)); - if (diff > 0) { - squaredLowerBoundDistance += diff*diff; - } + diff = t - (b[0] + Bf.col(0).dot(a)); + if (diff > 0) { + squaredLowerBoundDistance += diff * diff; + } - // B2 x B0 = B1 - s = B.col(1).dot(T); - t = ((s < 0.0) ? -s : s); + // B2 x B0 = B1 + s = B.col(1).dot(T); + t = ((s < 0.0) ? -s : s); - diff = t - (b[1] + Bf.col(1).dot(a)); - if (diff > 0) { - squaredLowerBoundDistance += diff*diff; - } + diff = t - (b[1] + Bf.col(1).dot(a)); + if (diff > 0) { + squaredLowerBoundDistance += diff * diff; + } - // B0 x B1 = B2 - s = B.col(2).dot(T); - t = ((s < 0.0) ? -s : s); + // B0 x B1 = B2 + s = B.col(2).dot(T); + t = ((s < 0.0) ? -s : s); - diff = t - (b[2] + Bf.col(2).dot(a)); - if (diff > 0) { - squaredLowerBoundDistance += diff*diff; - } - - if (squaredLowerBoundDistance > breakDistance2) - return true; + diff = t - (b[2] + Bf.col(2).dot(a)); + if (diff > 0) { + squaredLowerBoundDistance += diff * diff; + } - // A0 x B0 - s = T[2] * B(1, 0) - T[1] * B(2, 0); - t = ((s < 0.0) ? -s : s); - - FCL_REAL sinus2; - diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + - b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); - // We need to divide by the norm || A0 x B0 || - // As ||A0|| = ||B0|| = 1, - // 2 2 - // || A0 x B0 || + (A0 | B0) = 1 - if (diff > 0) { - sinus2 = 1 - Bf (0,0) * Bf (0,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + if (squaredLowerBoundDistance > breakDistance2) return true; + + // A0 x B0 + s = T[2] * B(1, 0) - T[1] * B(2, 0); + t = ((s < 0.0) ? -s : s); + + FCL_REAL sinus2; + diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + + b[2] * Bf(0, 1)); + // We need to divide by the norm || A0 x B0 || + // As ||A0|| = ||B0|| = 1, + // 2 2 + // || A0 x B0 || + (A0 | B0) = 1 + if (diff > 0) { + sinus2 = 1 - Bf(0, 0) * Bf(0, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A0 x B1 - s = T[2] * B(1, 1) - T[1] * B(2, 1); - t = ((s < 0.0) ? -s : s); - - diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + - b[0] * Bf(0, 2) + b[2] * Bf(0, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (0,1) * Bf (0,1); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A0 x B1 + s = T[2] * B(1, 1) - T[1] * B(2, 1); + t = ((s < 0.0) ? -s : s); + + diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + + b[2] * Bf(0, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(0, 1) * Bf(0, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A0 x B2 - s = T[2] * B(1, 2) - T[1] * B(2, 2); - t = ((s < 0.0) ? -s : s); - - diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + - b[0] * Bf(0, 1) + b[1] * Bf(0, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (0,2) * Bf (0,2); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A0 x B2 + s = T[2] * B(1, 2) - T[1] * B(2, 2); + t = ((s < 0.0) ? -s : s); + + diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + + b[1] * Bf(0, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(0, 2) * Bf(0, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B0 - s = T[0] * B(2, 0) - T[2] * B(0, 0); - t = ((s < 0.0) ? -s : s); - - diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + - b[1] * Bf(1, 2) + b[2] * Bf(1, 1)); - if (diff > 0) { - sinus2 = 1 - Bf (1,0) * Bf (1,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A1 x B0 + s = T[0] * B(2, 0) - T[2] * B(0, 0); + t = ((s < 0.0) ? -s : s); + + diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + + b[2] * Bf(1, 1)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 0) * Bf(1, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B1 - s = T[0] * B(2, 1) - T[2] * B(0, 1); - t = ((s < 0.0) ? -s : s); - - diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + - b[0] * Bf(1, 2) + b[2] * Bf(1, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (1,1) * Bf (1,1); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A1 x B1 + s = T[0] * B(2, 1) - T[2] * B(0, 1); + t = ((s < 0.0) ? -s : s); + + diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + + b[2] * Bf(1, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 1) * Bf(1, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A1 x B2 - s = T[0] * B(2, 2) - T[2] * B(0, 2); - t = ((s < 0.0) ? -s : s); - - diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + - b[0] * Bf(1, 1) + b[1] * Bf(1, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (1,2) * Bf (1,2); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A1 x B2 + s = T[0] * B(2, 2) - T[2] * B(0, 2); + t = ((s < 0.0) ? -s : s); + + diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + + b[1] * Bf(1, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(1, 2) * Bf(1, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A2 x B0 - s = T[1] * B(0, 0) - T[0] * B(1, 0); - t = ((s < 0.0) ? -s : s); - - diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + - b[1] * Bf(2, 2) + b[2] * Bf(2, 1)); - if (diff > 0) { - sinus2 = 1 - Bf (2,0) * Bf (2,0); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A2 x B0 + s = T[1] * B(0, 0) - T[0] * B(1, 0); + t = ((s < 0.0) ? -s : s); + + diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + + b[2] * Bf(2, 1)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 0) * Bf(2, 0); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A2 x B1 - s = T[1] * B(0, 1) - T[0] * B(1, 1); - t = ((s < 0.0) ? -s : s); - - diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + - b[0] * Bf(2, 2) + b[2] * Bf(2, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (2,1) * Bf (2,1); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A2 x B1 + s = T[1] * B(0, 1) - T[0] * B(1, 1); + t = ((s < 0.0) ? -s : s); + + diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + + b[2] * Bf(2, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 1) * Bf(2, 1); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } + } - // A2 x B2 - s = T[1] * B(0, 2) - T[0] * B(1, 2); - t = ((s < 0.0) ? -s : s); - - diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + - b[0] * Bf(2, 1) + b[1] * Bf(2, 0)); - if (diff > 0) { - sinus2 = 1 - Bf (2,2) * Bf (2,2); - if (sinus2 > 1e-6) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } + // A2 x B2 + s = T[1] * B(0, 2) - T[0] * B(1, 2); + t = ((s < 0.0) ? -s : s); + + diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + + b[1] * Bf(2, 0)); + if (diff > 0) { + sinus2 = 1 - Bf(2, 2) * Bf(2, 2); + if (sinus2 > 1e-6) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } } - - return false; - } - // ------------------------ 6 -------------------------------------- - bool originalWithNoLowerBound (const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const FCL_REAL& , FCL_REAL& squaredLowerBoundDistance) - { - FCL_REAL t, s; - const FCL_REAL reps = 1e-6; + return false; +} - squaredLowerBoundDistance = 0; +// ------------------------ 6 -------------------------------------- +bool originalWithNoLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, + const Vec3f& b, const FCL_REAL&, + FCL_REAL& squaredLowerBoundDistance) { + FCL_REAL t, s; + const FCL_REAL reps = 1e-6; - Matrix3f Bf (B.array().abs() + reps); - // Bf += reps; + squaredLowerBoundDistance = 0; - // if any of these tests are one-sided, then the polyhedra are disjoint + Matrix3f Bf(B.array().abs() + reps); + // Bf += reps; - // A1 x A2 = A0 - t = ((T[0] < 0.0) ? -T[0] : T[0]); + // if any of these tests are one-sided, then the polyhedra are disjoint - // if(t > (a[0] + Bf.dotX(b))) - if(t > (a[0] + Bf.row(0).dot(b))) - return true; + // A1 x A2 = A0 + t = ((T[0] < 0.0) ? -T[0] : T[0]); - // B1 x B2 = B0 - // s = B.transposeDotX(T); - s = B.col(0).dot(T); - t = ((s < 0.0) ? -s : s); + // if(t > (a[0] + Bf.dotX(b))) + if (t > (a[0] + Bf.row(0).dot(b))) return true; - // if(t > (b[0] + Bf.transposeDotX(a))) - if(t > (b[0] + Bf.col(0).dot(a))) - return true; + // B1 x B2 = B0 + // s = B.transposeDotX(T); + s = B.col(0).dot(T); + t = ((s < 0.0) ? -s : s); - // A2 x A0 = A1 - t = ((T[1] < 0.0) ? -T[1] : T[1]); + // if(t > (b[0] + Bf.transposeDotX(a))) + if (t > (b[0] + Bf.col(0).dot(a))) return true; - // if(t > (a[1] + Bf.dotY(b))) - if(t > (a[1] + Bf.row(1).dot(b))) - return true; + // A2 x A0 = A1 + t = ((T[1] < 0.0) ? -T[1] : T[1]); - // A0 x A1 = A2 - t =((T[2] < 0.0) ? -T[2] : T[2]); + // if(t > (a[1] + Bf.dotY(b))) + if (t > (a[1] + Bf.row(1).dot(b))) return true; - // if(t > (a[2] + Bf.dotZ(b))) - if(t > (a[2] + Bf.row(2).dot(b))) - return true; + // A0 x A1 = A2 + t = ((T[2] < 0.0) ? -T[2] : T[2]); - // B2 x B0 = B1 - // s = B.transposeDotY(T); - s = B.col(1).dot(T); - t = ((s < 0.0) ? -s : s); + // if(t > (a[2] + Bf.dotZ(b))) + if (t > (a[2] + Bf.row(2).dot(b))) return true; - // if(t > (b[1] + Bf.transposeDotY(a))) - if(t > (b[1] + Bf.col(1).dot(a))) - return true; + // B2 x B0 = B1 + // s = B.transposeDotY(T); + s = B.col(1).dot(T); + t = ((s < 0.0) ? -s : s); - // B0 x B1 = B2 - // s = B.transposeDotZ(T); - s = B.col(2).dot(T); - t = ((s < 0.0) ? -s : s); + // if(t > (b[1] + Bf.transposeDotY(a))) + if (t > (b[1] + Bf.col(1).dot(a))) return true; - // if(t > (b[2] + Bf.transposeDotZ(a))) - if(t > (b[2] + Bf.col(2).dot(a))) - return true; + // B0 x B1 = B2 + // s = B.transposeDotZ(T); + s = B.col(2).dot(T); + t = ((s < 0.0) ? -s : s); - // A0 x B0 - s = T[2] * B(1, 0) - T[1] * B(2, 0); - t = ((s < 0.0) ? -s : s); + // if(t > (b[2] + Bf.transposeDotZ(a))) + if (t > (b[2] + Bf.col(2).dot(a))) return true; - if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + - b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) - return true; + // A0 x B0 + s = T[2] * B(1, 0) - T[1] * B(2, 0); + t = ((s < 0.0) ? -s : s); - // A0 x B1 - s = T[2] * B(1, 1) - T[1] * B(2, 1); - t = ((s < 0.0) ? -s : s); + if (t > + (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) + return true; - if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + - b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) - return true; + // A0 x B1 + s = T[2] * B(1, 1) - T[1] * B(2, 1); + t = ((s < 0.0) ? -s : s); - // A0 x B2 - s = T[2] * B(1, 2) - T[1] * B(2, 2); - t = ((s < 0.0) ? -s : s); + if (t > + (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) + return true; - if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + - b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) - return true; + // A0 x B2 + s = T[2] * B(1, 2) - T[1] * B(2, 2); + t = ((s < 0.0) ? -s : s); - // A1 x B0 - s = T[0] * B(2, 0) - T[2] * B(0, 0); - t = ((s < 0.0) ? -s : s); + if (t > + (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) + return true; - if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + - b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) - return true; + // A1 x B0 + s = T[0] * B(2, 0) - T[2] * B(0, 0); + t = ((s < 0.0) ? -s : s); - // A1 x B1 - s = T[0] * B(2, 1) - T[2] * B(0, 1); - t = ((s < 0.0) ? -s : s); + if (t > + (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) + return true; - if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + - b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) - return true; + // A1 x B1 + s = T[0] * B(2, 1) - T[2] * B(0, 1); + t = ((s < 0.0) ? -s : s); - // A1 x B2 - s = T[0] * B(2, 2) - T[2] * B(0, 2); - t = ((s < 0.0) ? -s : s); + if (t > + (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) + return true; - if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + - b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) - return true; + // A1 x B2 + s = T[0] * B(2, 2) - T[2] * B(0, 2); + t = ((s < 0.0) ? -s : s); - // A2 x B0 - s = T[1] * B(0, 0) - T[0] * B(1, 0); - t = ((s < 0.0) ? -s : s); + if (t > + (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) + return true; - if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + - b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) - return true; + // A2 x B0 + s = T[1] * B(0, 0) - T[0] * B(1, 0); + t = ((s < 0.0) ? -s : s); - // A2 x B1 - s = T[1] * B(0, 1) - T[0] * B(1, 1); - t = ((s < 0.0) ? -s : s); + if (t > + (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) + return true; - if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + - b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) - return true; + // A2 x B1 + s = T[1] * B(0, 1) - T[0] * B(1, 1); + t = ((s < 0.0) ? -s : s); - // A2 x B2 - s = T[1] * B(0, 2) - T[0] * B(1, 2); - t = ((s < 0.0) ? -s : s); + if (t > + (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) + return true; - if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + - b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) - return true; + // A2 x B2 + s = T[1] * B(0, 2) - T[0] * B(1, 2); + t = ((s < 0.0) ? -s : s); - return false; + if (t > + (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) + return true; - } + return false; } +} // namespace obbDisjoint_impls -struct BenchmarkResult -{ +struct BenchmarkResult { /// The test ID: /// - 0-10 identifies a separating axes. /// - 11 means no separatins axes could be found. (distance should be <= 0) @@ -1185,84 +1203,76 @@ struct BenchmarkResult duration_type duration[NB_METHODS]; bool failure; - static std::ostream& headers (std::ostream& os) - { - duration_type dummy (1); - std::string unit = " (" + boost::chrono::duration_units_default<>().get_unit (boost::chrono::duration_style::symbol, dummy) + ")"; - os << boost::chrono::symbol_format - << "separating axis" - << sep << "distance lower bound" - << sep << "distance" - << sep << "failure" - << sep << "Runtime Loop" << unit - << sep << "Manual Loop Unrolling 1" << unit - << sep << "Manual Loop Unrolling 2" << unit - << sep << "Template Unrolling" << unit - << sep << "Partial Template Unrolling" << unit - << sep << "Original (LowerBound)" << unit - << sep << "Original (NoLowerBound)" << unit - ; + static std::ostream& headers(std::ostream& os) { + duration_type dummy(1); + std::string unit = " (" + + boost::chrono::duration_units_default<>().get_unit( + boost::chrono::duration_style::symbol, dummy) + + ")"; + os << boost::chrono::symbol_format << "separating axis" << sep + << "distance lower bound" << sep << "distance" << sep << "failure" << sep + << "Runtime Loop" << unit << sep << "Manual Loop Unrolling 1" << unit + << sep << "Manual Loop Unrolling 2" << unit << sep + << "Template Unrolling" << unit << sep << "Partial Template Unrolling" + << unit << sep << "Original (LowerBound)" << unit << sep + << "Original (NoLowerBound)" << unit; return os; } - std::ostream& print (std::ostream& os) const - { - os << ifId - << sep << std::sqrt(squaredLowerBoundDistance) - << sep << distance - << sep << failure; + std::ostream& print(std::ostream& os) const { + os << ifId << sep << std::sqrt(squaredLowerBoundDistance) << sep << distance + << sep << failure; for (int i = 0; i < NB_METHODS; ++i) os << sep << duration[i].count(); return os; } }; -std::ostream& operator<< (std::ostream& os, const BenchmarkResult& br) -{ - return br.print (os); +std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) { + return br.print(os); } -BenchmarkResult benchmark_obb_case ( - const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const CollisionRequest& request, - std::size_t N) -{ - const FCL_REAL breakDistance (request.break_distance + request.security_margin); +BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const CollisionRequest& request, + std::size_t N) { + const FCL_REAL breakDistance(request.break_distance + + request.security_margin); const FCL_REAL breakDistance2 = breakDistance * breakDistance; BenchmarkResult result; // First determine which axis provide the answer - result.ifId = obbDisjoint_impls::separatingAxisId (B, T, a, b, breakDistance2, - result.squaredLowerBoundDistance); - bool disjoint = obbDisjoint_impls::distance (B, T, a, b, result.distance); - assert (0 <= result.ifId && result.ifId <= 11); + result.ifId = obbDisjoint_impls::separatingAxisId( + B, T, a, b, breakDistance2, result.squaredLowerBoundDistance); + bool disjoint = obbDisjoint_impls::distance(B, T, a, b, result.distance); + assert(0 <= result.ifId && result.ifId <= 11); // Sanity check result.failure = true; - bool overlap = (result.ifId == 11); + bool overlap = (result.ifId == 11); FCL_REAL dist_thr = request.break_distance + request.security_margin; - if ( !overlap && result.distance <= 0) { + if (!overlap && result.distance <= 0) { std::cerr << "Failure: negative distance for disjoint OBBs."; } else if (!overlap && result.squaredLowerBoundDistance < 0) { std::cerr << "Failure: negative distance lower bound."; - } else if (!overlap && eps < std::sqrt (result.squaredLowerBoundDistance) - result.distance) { - std::cerr << "Failure: distance is inferior to its lower bound (diff = " << - std::sqrt (result.squaredLowerBoundDistance) - result.distance << ")."; + } else if (!overlap && eps < std::sqrt(result.squaredLowerBoundDistance) - + result.distance) { + std::cerr << "Failure: distance is inferior to its lower bound (diff = " + << std::sqrt(result.squaredLowerBoundDistance) - result.distance + << ")."; } else if (overlap != !disjoint && result.distance >= dist_thr - eps) { std::cerr << "Failure: overlapping test and distance query mismatch."; - } else if (overlap && result.distance >= dist_thr - eps) { + } else if (overlap && result.distance >= dist_thr - eps) { std::cerr << "Failure: positive distance for overlapping OBBs."; } else { result.failure = false; } if (result.failure) { - std::cerr - << "\nR = " << Quaternion3f(B).coeffs().transpose().format (py_fmt) - << "\nT = " << T.transpose().format (py_fmt) - << "\na = " << a.transpose().format (py_fmt) - << "\nb = " << b.transpose().format (py_fmt) - << "\nresult = " << result - << '\n' << std::endl; + std::cerr << "\nR = " << Quaternion3f(B).coeffs().transpose().format(py_fmt) + << "\nT = " << T.transpose().format(py_fmt) + << "\na = " << a.transpose().format(py_fmt) + << "\nb = " << b.transpose().format(py_fmt) + << "\nresult = " << result << '\n' + << std::endl; } // Compute time @@ -1272,42 +1282,46 @@ BenchmarkResult benchmark_obb_case ( // ------------------------ 0 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) - obbDisjoint_impls::withRuntimeLoop (B, T, a, b, breakDistance2, tmp); + obbDisjoint_impls::withRuntimeLoop(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[0] = (end - start) / N; // ------------------------ 1 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) - obbDisjoint_impls::withManualLoopUnrolling_1 (B, T, a, b, breakDistance2, tmp); + obbDisjoint_impls::withManualLoopUnrolling_1(B, T, a, b, breakDistance2, + tmp); end = clock_type::now(); result.duration[1] = (end - start) / N; // ------------------------ 2 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) - obbDisjoint_impls::withManualLoopUnrolling_2 (B, T, a, b, breakDistance2, tmp); + obbDisjoint_impls::withManualLoopUnrolling_2(B, T, a, b, breakDistance2, + tmp); end = clock_type::now(); result.duration[2] = (end - start) / N; // ------------------------ 3 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) - obbDisjoint_impls::withTemplateLoopUnrolling_1 (B, T, a, b, breakDistance2, tmp); + obbDisjoint_impls::withTemplateLoopUnrolling_1(B, T, a, b, breakDistance2, + tmp); end = clock_type::now(); result.duration[3] = (end - start) / N; // ------------------------ 4 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) - obbDisjoint_impls::withPartialTemplateLoopUnrolling_1 (B, T, a, b, breakDistance2, tmp); + obbDisjoint_impls::withPartialTemplateLoopUnrolling_1(B, T, a, b, + breakDistance2, tmp); end = clock_type::now(); result.duration[4] = (end - start) / N; // ------------------------ 5 -------------------------------------- start = clock_type::now(); for (std::size_t i = 0; i < N; ++i) - obbDisjoint_impls::originalWithLowerBound (B, T, a, b, breakDistance2, tmp); + obbDisjoint_impls::originalWithLowerBound(B, T, a, b, breakDistance2, tmp); end = clock_type::now(); result.duration[5] = (end - start) / N; @@ -1315,15 +1329,15 @@ BenchmarkResult benchmark_obb_case ( start = clock_type::now(); // The 2 last arguments are unused. for (std::size_t i = 0; i < N; ++i) - obbDisjoint_impls::originalWithNoLowerBound (B, T, a, b, breakDistance2, tmp); + obbDisjoint_impls::originalWithNoLowerBound(B, T, a, b, breakDistance2, + tmp); end = clock_type::now(); result.duration[6] = (end - start) / N; return result; } -std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) -{ +std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) { std::size_t nbFailure = 0; // Create two OBBs axis @@ -1332,25 +1346,25 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) Vec3f T; CollisionRequest request; -#ifndef NDEBUG // if debug mode - static const size_t nbRandomOBB = 10; +#ifndef NDEBUG // if debug mode + static const size_t nbRandomOBB = 10; static const size_t nbTransformPerOBB = 10; - static const size_t nbRunForTimeMeas = 10; + static const size_t nbRunForTimeMeas = 10; #else - static const size_t nbRandomOBB = 100; + static const size_t nbRandomOBB = 100; static const size_t nbTransformPerOBB = 100; - static const size_t nbRunForTimeMeas = 1000; + static const size_t nbRunForTimeMeas = 1000; #endif static const FCL_REAL extentNorm = 1.; if (output != NULL) *output << BenchmarkResult::headers << '\n'; - + BenchmarkResult result; for (std::size_t iobb = 0; iobb < nbRandomOBB; ++iobb) { - randomOBBs (a, b, extentNorm); + randomOBBs(a, b, extentNorm); for (std::size_t itf = 0; itf < nbTransformPerOBB; ++itf) { - randomTransform (B, T, a, b, extentNorm); - result = benchmark_obb_case (B, T, a, b, request, nbRunForTimeMeas); + randomTransform(B, T, a, b, extentNorm); + result = benchmark_obb_case(B, T, a, b, request, nbRunForTimeMeas); if (output != NULL) *output << result << '\n'; if (result.failure) nbFailure++; } @@ -1358,24 +1372,21 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) return nbFailure; } -int main (int argc, char** argv) -{ +int main(int argc, char** argv) { std::ostream* output = NULL; - if (argc > 1 && strcmp(argv[1], "--generate-output") == 0) - { + if (argc > 1 && strcmp(argv[1], "--generate-output") == 0) { output = &std::cout; } bool cpuScalingEnabled = checkCpuScalingEnabled(); if (cpuScalingEnabled) - std::cerr << - "CPU scaling is enabled." - "\n\tThe benchmark real time measurements may be noisy and will incur extra overhead." - "\n\tUse the following commands to turn on and off." - "\n\t\tsudo cpufreq-set --governor performance" - "\n\t\tsudo cpufreq-set --governor powersave" - "\n" - ; + std::cerr << "CPU scaling is enabled." + "\n\tThe benchmark real time measurements may be noisy and " + "will incur extra overhead." + "\n\tUse the following commands to turn on and off." + "\n\t\tsudo cpufreq-set --governor performance" + "\n\t\tsudo cpufreq-set --governor powersave" + "\n"; std::size_t nbFailure = obb_overlap_and_lower_bound_distance(output); if (nbFailure > INT_MAX) return INT_MAX; diff --git a/test/octree.cpp b/test/octree.cpp index 4e7d476d6..222cf5f34 100644 --- a/test/octree.cpp +++ b/test/octree.cpp @@ -50,21 +50,20 @@ namespace utf = boost::unit_test::framework; -using hpp::fcl::Vec3f; -using hpp::fcl::Triangle; -using hpp::fcl::OBBRSS; using hpp::fcl::BVHModel; using hpp::fcl::BVSplitter; -using hpp::fcl::OcTree; -using hpp::fcl::FCL_REAL; -using hpp::fcl::Transform3f; using hpp::fcl::CollisionRequest; using hpp::fcl::CollisionResult; +using hpp::fcl::FCL_REAL; +using hpp::fcl::OBBRSS; +using hpp::fcl::OcTree; +using hpp::fcl::Transform3f; +using hpp::fcl::Triangle; +using hpp::fcl::Vec3f; -void makeMesh (const std::vector& vertices, - const std::vector& triangles, BVHModel& model) -{ - hpp::fcl::SplitMethodType split_method (hpp::fcl::SPLIT_METHOD_MEAN); +void makeMesh(const std::vector& vertices, + const std::vector& triangles, BVHModel& model) { + hpp::fcl::SplitMethodType split_method(hpp::fcl::SPLIT_METHOD_MEAN); model.bv_splitter.reset(new BVSplitter(split_method)); model.bv_splitter.reset(new BVSplitter(split_method)); @@ -73,31 +72,32 @@ void makeMesh (const std::vector& vertices, model.endModel(); } -hpp::fcl::OcTree makeOctree (const BVHModel & mesh, - const FCL_REAL& resolution) -{ - Vec3f m (mesh.aabb_local.min_); - Vec3f M (mesh.aabb_local.max_); - hpp::fcl::Box box (resolution, resolution, resolution); - CollisionRequest request (hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, 1); +hpp::fcl::OcTree makeOctree(const BVHModel& mesh, + const FCL_REAL& resolution) { + Vec3f m(mesh.aabb_local.min_); + Vec3f M(mesh.aabb_local.max_); + hpp::fcl::Box box(resolution, resolution, resolution); + CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, + 1); CollisionResult result; Transform3f tfBox; - octomap::OcTreePtr_t octree (new octomap::OcTree (resolution)); + octomap::OcTreePtr_t octree(new octomap::OcTree(resolution)); - for (FCL_REAL x = resolution * floor (m [0]/resolution); x <= M [0]; + for (FCL_REAL x = resolution * floor(m[0] / resolution); x <= M[0]; x += resolution) { - for (FCL_REAL y = resolution * floor (m [1]/resolution); y <= M [1]; + for (FCL_REAL y = resolution * floor(m[1] / resolution); y <= M[1]; y += resolution) { - for (FCL_REAL z = resolution * floor (m [2]/resolution); z <= M [2]; + for (FCL_REAL z = resolution * floor(m[2] / resolution); z <= M[2]; z += resolution) { - Vec3f center (x + .5*resolution, y + .5*resolution, z + .5*resolution); - tfBox.setTranslation (center); - hpp::fcl::collide (&box, tfBox, &mesh, Transform3f (), request, result); - if (result.isCollision ()) { - octomap::point3d p - ((float) center [0], (float) center [1], (float) center [2]); - octree->updateNode (p, true); - result.clear (); + Vec3f center(x + .5 * resolution, y + .5 * resolution, + z + .5 * resolution); + tfBox.setTranslation(center); + hpp::fcl::collide(&box, tfBox, &mesh, Transform3f(), request, result); + if (result.isCollision()) { + octomap::point3d p((float)center[0], (float)center[1], + (float)center[2]); + octree->updateNode(p, true); + result.clear(); } } } @@ -105,63 +105,64 @@ hpp::fcl::OcTree makeOctree (const BVHModel & mesh, octree->updateInnerOccupancy(); octree->writeBinary("./env.octree"); - return OcTree (octree); + return OcTree(octree); } -BOOST_AUTO_TEST_CASE (OCTREE) -{ - Eigen::IOFormat tuple (Eigen::FullPrecision, Eigen::DontAlignCols, "", - ", ", "", "", "(", ")"); - FCL_REAL resolution (10.); +BOOST_AUTO_TEST_CASE(OCTREE) { + Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", + "", "", "(", ")"); + FCL_REAL resolution(10.); std::vector pRob, pEnv; std::vector tRob, tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "rob.obj").string().c_str(), pRob, tRob); loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv); - BVHModel robMesh, envMesh; + BVHModel robMesh, envMesh; // Build meshes with robot and environment - makeMesh (pRob, tRob, robMesh); - makeMesh (pEnv, tEnv, envMesh); + makeMesh(pRob, tRob, robMesh); + makeMesh(pEnv, tEnv, envMesh); // Build octomap with environment - envMesh.computeLocalAABB (); + envMesh.computeLocalAABB(); // Load octree built from envMesh by makeOctree(envMesh, resolution) - OcTree envOctree - (hpp::fcl::loadOctreeFile ((path / "env.octree").string(), resolution)); + OcTree envOctree( + hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution)); std::cout << "Finished loading octree." << std::endl; std::vector transforms; FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; -#ifndef NDEBUG // if debug mode +#ifndef NDEBUG // if debug mode std::size_t N = 100; #else std::size_t N = 10000; #endif - N = hpp::fcl::getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, N); + N = hpp::fcl::getNbRun(utf::master_test_suite().argc, + utf::master_test_suite().argv, N); - generateRandomTransforms(extents, transforms, 2*N); + generateRandomTransforms(extents, transforms, 2 * N); - CollisionRequest request (hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, 1); - for (std::size_t i=0; i. - #include #include @@ -31,40 +30,38 @@ using namespace hpp::fcl; CollisionFunctionMatrix lookupTable; -bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) -{ +bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) { OBJECT_TYPE object_type1 = o1->getObjectType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); - if(object_type1 == OT_GEOM && object_type2 == OT_BVH) + if (object_type1 == OT_GEOM && object_type2 == OT_BVH) return (lookupTable.collision_matrix[node_type2][node_type1] != NULL); else return (lookupTable.collision_matrix[node_type1][node_type2] != NULL); } -template -CollisionGeometryPtr_t objToGeom (const std::string& filename) -{ +template +CollisionGeometryPtr_t objToGeom(const std::string& filename) { std::vector pt; std::vector tri; loadOBJFile(filename.c_str(), pt, tri); - BVHModel* model (new BVHModel()); + BVHModel* model(new BVHModel()); // model->bv_splitter.reset(new BVSplitter(split_method)); model->beginModel(); model->addSubModel(pt, tri); model->endModel(); - return CollisionGeometryPtr_t (model); + return CollisionGeometryPtr_t(model); } -template -CollisionGeometryPtr_t meshToGeom (const std::string& filename, const Vec3f& scale = Vec3f(1, 1, 1)) -{ - shared_ptr< BVHModel > model (new BVHModel()); +template +CollisionGeometryPtr_t meshToGeom(const std::string& filename, + const Vec3f& scale = Vec3f(1, 1, 1)) { + shared_ptr > model(new BVHModel()); loadPolyhedronFromResource(filename, scale, model); return model; } @@ -73,33 +70,27 @@ struct Geometry { std::string type; CollisionGeometryPtr_t o; - Geometry (const std::string& t, CollisionGeometry* ob) : - type(t), o(ob) - {} - Geometry (const std::string& t, const CollisionGeometryPtr_t& ob) : - type(t), o(ob) - {} + Geometry(const std::string& t, CollisionGeometry* ob) : type(t), o(ob) {} + Geometry(const std::string& t, const CollisionGeometryPtr_t& ob) + : type(t), o(ob) {} }; struct Results { std::vector rs; - Eigen::VectorXd times; // micro-seconds + Eigen::VectorXd times; // micro-seconds - Results (size_t i) : rs(i), times((Eigen::DenseIndex)i) {} + Results(size_t i) : rs(i), times((Eigen::DenseIndex)i) {} }; -void collide(const std::vector& tf, - const CollisionGeometry* o1, - const CollisionGeometry* o2, - const CollisionRequest& request, - Results& results) -{ +void collide(const std::vector& tf, const CollisionGeometry* o1, + const CollisionGeometry* o2, const CollisionRequest& request, + Results& results) { Transform3f Id; BenchTimer timer; for (std::size_t i = 0; i < tf.size(); ++i) { timer.start(); /* int num_contact = */ - collide (o1, tf[i], o2, Id, request, results.rs[i]); + collide(o1, tf[i], o2, Id, request, results.rs[i]); timer.stop(); results.times[(Eigen::DenseIndex)i] = timer.getElapsedTimeInMicroSec(); } @@ -107,19 +98,21 @@ void collide(const std::vector& tf, const char* sep = ", "; -void printResultHeaders () -{ - std::cout << "Type 1" << sep << "Type 2" << sep << "mean time" << sep << "time std dev" << sep << "min time" << sep << "max time" << std::endl; +void printResultHeaders() { + std::cout << "Type 1" << sep << "Type 2" << sep << "mean time" << sep + << "time std dev" << sep << "min time" << sep << "max time" + << std::endl; } -void printResults (const Geometry& g1, const Geometry& g2, const Results& rs) -{ +void printResults(const Geometry& g1, const Geometry& g2, const Results& rs) { double mean = rs.times.mean(); - double var = rs.times.cwiseAbs2().mean() - mean*mean; - std::cout << g1.type << sep << g2.type << sep << mean << sep << std::sqrt(var) << sep << rs.times.minCoeff() << sep << rs.times.maxCoeff() << std::endl; + double var = rs.times.cwiseAbs2().mean() - mean * mean; + std::cout << g1.type << sep << g2.type << sep << mean << sep << std::sqrt(var) + << sep << rs.times.minCoeff() << sep << rs.times.maxCoeff() + << std::endl; } -#ifndef NDEBUG // if debug mode +#ifndef NDEBUG // if debug mode size_t Ntransform = 1; #else size_t Ntransform = 100; @@ -127,25 +120,27 @@ size_t Ntransform = 100; FCL_REAL limit = 20; bool verbose = false; -#define OUT(x) if (verbose) std::cout << x << std::endl +#define OUT(x) \ + if (verbose) std::cout << x << std::endl #define CHECK_PARAM_NB(NB, NAME) \ - if (iarg + NB >= argc) throw std::invalid_argument(#NAME " requires " #NB " numbers") -void handleParam (int& iarg, const int& argc, char** argv, CollisionRequest& request) -{ + if (iarg + NB >= argc) \ + throw std::invalid_argument(#NAME " requires " #NB " numbers") +void handleParam(int& iarg, const int& argc, char** argv, + CollisionRequest& request) { while (iarg < argc) { std::string a(argv[iarg]); if (a == "-nb_transform") { CHECK_PARAM_NB(1, nb_transform); - Ntransform = (size_t)atoi(argv[iarg+1]); + Ntransform = (size_t)atoi(argv[iarg + 1]); OUT("nb_transform = " << Ntransform); iarg += 2; } else if (a == "-enable_distance_lower_bound") { CHECK_PARAM_NB(1, enable_distance_lower_bound); - request.enable_distance_lower_bound = bool(atoi(argv[iarg+1])); + request.enable_distance_lower_bound = bool(atoi(argv[iarg + 1])); iarg += 2; } else if (a == "-limit") { CHECK_PARAM_NB(1, limit); - limit = atof(argv[iarg+1]); + limit = atof(argv[iarg + 1]); iarg += 2; } else if (a == "-verbose") { verbose = true; @@ -155,60 +150,59 @@ void handleParam (int& iarg, const int& argc, char** argv, CollisionRequest& req } } } -#define CREATE_SHAPE_2(var, Name) \ - CHECK_PARAM_NB(2, Name); \ - var.reset(new Name (atof(argv[iarg+1]), atof(argv[iarg+2]))); \ - iarg += 3; -Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) -{ +#define CREATE_SHAPE_2(var, Name) \ + CHECK_PARAM_NB(2, Name); \ + var.reset(new Name(atof(argv[iarg + 1]), atof(argv[iarg + 2]))); \ + iarg += 3; +Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) { if (iarg >= argc) throw std::invalid_argument("An argument is required."); std::string a(argv[iarg]); std::string type; CollisionGeometryPtr_t o; if (a == "-box") { CHECK_PARAM_NB(3, Box); - o.reset(new Box (atof(argv[iarg+1]), - atof(argv[iarg+2]), - atof(argv[iarg+3]))); + o.reset(new Box(atof(argv[iarg + 1]), atof(argv[iarg + 2]), + atof(argv[iarg + 3]))); iarg += 4; type = "box"; } else if (a == "-sphere") { CHECK_PARAM_NB(1, Sphere); - o.reset(new Sphere (atof(argv[iarg+1]))); + o.reset(new Sphere(atof(argv[iarg + 1]))); iarg += 2; type = "sphere"; } else if (a == "-mesh") { CHECK_PARAM_NB(2, Mesh); - OUT("Loading " << argv[iarg+2] << " as BVHModel<" << argv[iarg+1] << ">..."); - if (strcmp(argv[iarg+1], "obb") == 0) { - o = meshToGeom(argv[iarg+2]); - OUT("Mesh has " << dynamic_pointer_cast >(o)->num_tris << " triangles"); + OUT("Loading " << argv[iarg + 2] << " as BVHModel<" << argv[iarg + 1] + << ">..."); + if (strcmp(argv[iarg + 1], "obb") == 0) { + o = meshToGeom(argv[iarg + 2]); + OUT("Mesh has " << dynamic_pointer_cast >(o)->num_tris + << " triangles"); type = "mesh_obb"; - } - else if (strcmp(argv[iarg+1], "obbrss") == 0) { - o = meshToGeom(argv[iarg+2]); - OUT("Mesh has " << dynamic_pointer_cast >(o)->num_tris << " triangles"); + } else if (strcmp(argv[iarg + 1], "obbrss") == 0) { + o = meshToGeom(argv[iarg + 2]); + OUT("Mesh has " << dynamic_pointer_cast >(o)->num_tris + << " triangles"); type = "mesh_obbrss"; - } - else - throw std::invalid_argument ("BV type must be obb or obbrss"); + } else + throw std::invalid_argument("BV type must be obb or obbrss"); OUT("done."); iarg += 3; if (iarg < argc && strcmp(argv[iarg], "crop") == 0) { CHECK_PARAM_NB(6, Crop); - hpp::fcl::AABB aabb( - Vec3f(atof(argv[iarg+1]), - atof(argv[iarg+2]), - atof(argv[iarg+3])), - Vec3f(atof(argv[iarg+4]), - atof(argv[iarg+5]), - atof(argv[iarg+6]))); - OUT("Cropping " << aabb.min_.transpose() << " ---- " << aabb.max_.transpose() << " ..."); + hpp::fcl::AABB aabb(Vec3f(atof(argv[iarg + 1]), atof(argv[iarg + 2]), + atof(argv[iarg + 3])), + Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]), + atof(argv[iarg + 6]))); + OUT("Cropping " << aabb.min_.transpose() << " ---- " + << aabb.max_.transpose() << " ..."); o->computeLocalAABB(); - OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- " << o->aabb_local.max_.transpose() << " ..."); + OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- " + << o->aabb_local.max_.transpose() << " ..."); o.reset(extract(o.get(), Transform3f(), aabb)); - if (!o) throw std::invalid_argument ("Failed to crop."); - OUT("Crop has " << dynamic_pointer_cast >(o)->num_tris << " triangles"); + if (!o) throw std::invalid_argument("Failed to crop."); + OUT("Crop has " << dynamic_pointer_cast >(o)->num_tris + << " triangles"); iarg += 7; } } else if (a == "-capsule") { @@ -221,14 +215,12 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) CREATE_SHAPE_2(o, Cylinder); type = "cylinder"; } else { - throw std::invalid_argument (std::string("Unknown argument: ") + a); + throw std::invalid_argument(std::string("Unknown argument: ") + a); } return Geometry(type, o); } -int main(int argc, char** argv) -{ - +int main(int argc, char** argv) { std::vector transforms; CollisionRequest request; @@ -236,13 +228,13 @@ int main(int argc, char** argv) if (argc > 1) { int iarg = 1; handleParam(iarg, argc, argv, request); - Geometry first = makeGeomFromParam(iarg, argc, argv); + Geometry first = makeGeomFromParam(iarg, argc, argv); Geometry second = makeGeomFromParam(iarg, argc, argv); FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit}; generateRandomTransforms(extents, transforms, Ntransform); printResultHeaders(); - Results results (Ntransform); + Results results(Ntransform); collide(transforms, first.o.get(), second.o.get(), request, results); printResults(first, second, results); } else { @@ -251,33 +243,43 @@ int main(int argc, char** argv) boost::filesystem::path path(TEST_RESOURCES_DIR); std::vector geoms; - geoms.push_back(Geometry ("Box" , new Box (1, 2, 3) )); - geoms.push_back(Geometry ("Sphere" , new Sphere (2) )); - geoms.push_back(Geometry ("Capsule" , new Capsule (2, 1) )); - geoms.push_back(Geometry ("Cone" , new Cone (2, 1) )); - geoms.push_back(Geometry ("Cylinder" , new Cylinder (2, 1) )); - //geoms.push_back(Geometry ("Plane" , new Plane (Vec3f(1,1,1).normalized(), 0) )); - //geoms.push_back(Geometry ("Halfspace" , new Halfspace (Vec3f(1,1,1).normalized(), 0) )); - // not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP (Vec3f(0,1,0), Vec3f(0,0,1), Vec3f(-1,0,0)) )); - - geoms.push_back(Geometry ("rob BVHModel" , objToGeom ((path / "rob.obj").string()))); - // geoms.push_back(Geometry ("rob BVHModel" , objToGeom ((path / "rob.obj").string()))); - // geoms.push_back(Geometry ("rob BVHModel" , objToGeom ((path / "rob.obj").string()))); - geoms.push_back(Geometry ("rob BVHModel", objToGeom((path / "rob.obj").string()))); - - geoms.push_back(Geometry ("env BVHModel" , objToGeom ((path / "env.obj").string()))); - // geoms.push_back(Geometry ("env BVHModel" , objToGeom ((path / "env.obj").string()))); - // geoms.push_back(Geometry ("env BVHModel" , objToGeom ((path / "env.obj").string()))); - geoms.push_back(Geometry ("env BVHModel", objToGeom((path / "env.obj").string()))); + geoms.push_back(Geometry("Box", new Box(1, 2, 3))); + geoms.push_back(Geometry("Sphere", new Sphere(2))); + geoms.push_back(Geometry("Capsule", new Capsule(2, 1))); + geoms.push_back(Geometry("Cone", new Cone(2, 1))); + geoms.push_back(Geometry("Cylinder", new Cylinder(2, 1))); + // geoms.push_back(Geometry ("Plane" , new Plane + // (Vec3f(1,1,1).normalized(), 0) )); + // geoms.push_back(Geometry ("Halfspace" , new Halfspace + // (Vec3f(1,1,1).normalized(), 0) )); + // not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP + // (Vec3f(0,1,0), Vec3f(0,0,1), Vec3f(-1,0,0)) )); + + geoms.push_back(Geometry("rob BVHModel", + objToGeom((path / "rob.obj").string()))); + // geoms.push_back(Geometry ("rob BVHModel" , objToGeom ((path + // / "rob.obj").string()))); geoms.push_back(Geometry ("rob BVHModel" + // , objToGeom ((path / "rob.obj").string()))); + geoms.push_back(Geometry("rob BVHModel", + objToGeom((path / "rob.obj").string()))); + + geoms.push_back(Geometry("env BVHModel", + objToGeom((path / "env.obj").string()))); + // geoms.push_back(Geometry ("env BVHModel" , objToGeom ((path + // / "env.obj").string()))); geoms.push_back(Geometry ("env BVHModel" + // , objToGeom ((path / "env.obj").string()))); + geoms.push_back(Geometry("env BVHModel", + objToGeom((path / "env.obj").string()))); printResultHeaders(); // collision - for(std::size_t i = 0; i < geoms.size(); ++i) { + for (std::size_t i = 0; i < geoms.size(); ++i) { for (std::size_t j = i; j < geoms.size(); ++j) { if (!supportedPair(geoms[i].o.get(), geoms[j].o.get())) continue; - Results results (Ntransform); - collide(transforms, geoms[i].o.get(), geoms[j].o.get(), request, results); + Results results(Ntransform); + collide(transforms, geoms[i].o.get(), geoms[j].o.get(), request, + results); printResults(geoms[i], geoms[j], results); } } diff --git a/test/python_unit/api.py b/test/python_unit/api.py index a4ac61b9b..10c92c9f9 100644 --- a/test/python_unit/api.py +++ b/test/python_unit/api.py @@ -1,30 +1,32 @@ import unittest from test_case import TestCase import hppfcl + hppfcl.switchToNumpyArray() import numpy as np -class TestMainAPI(TestCase): +class TestMainAPI(TestCase): def test_collision(self): - capsule = hppfcl.Capsule(1.,2.) + capsule = hppfcl.Capsule(1.0, 2.0) M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3),np.array([3, 0, 0])) + M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0])) - req=hppfcl.CollisionRequest() - res=hppfcl.CollisionResult() + req = hppfcl.CollisionRequest() + res = hppfcl.CollisionResult() self.assertTrue(not hppfcl.collide(capsule, M1, capsule, M2, req, res)) def test_distance(self): - capsule = hppfcl.Capsule(1.,2.) + capsule = hppfcl.Capsule(1.0, 2.0) M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3),np.array([3, 0, 0])) + M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0])) - req=hppfcl.DistanceRequest() - res=hppfcl.DistanceResult() + req = hppfcl.DistanceRequest() + res = hppfcl.DistanceResult() self.assertTrue(hppfcl.distance(capsule, M1, capsule, M2, req, res) > 0) -if __name__ == '__main__': + +if __name__ == "__main__": unittest.main() diff --git a/test/python_unit/collision.py b/test/python_unit/collision.py index ee7849a0a..fd69df3c9 100644 --- a/test/python_unit/collision.py +++ b/test/python_unit/collision.py @@ -1,46 +1,49 @@ import unittest from test_case import TestCase import hppfcl + hppfcl.switchToNumpyArray() import numpy as np + def tetahedron(): pts = hppfcl.StdVec_Vec3f() - pts.append( np.array((0, 0, 0)) ) - pts.append( np.array((0, 1, 0)) ) - pts.append( np.array((1, 0, 0)) ) - pts.append( np.array((0, 0, 1)) ) + pts.append(np.array((0, 0, 0))) + pts.append(np.array((0, 1, 0))) + pts.append(np.array((1, 0, 0))) + pts.append(np.array((0, 0, 1))) tri = hppfcl.StdVec_Triangle() - tri.append(hppfcl.Triangle(0,1,2)) - tri.append(hppfcl.Triangle(0,1,3)) - tri.append(hppfcl.Triangle(0,2,3)) - tri.append(hppfcl.Triangle(1,2,3)) + tri.append(hppfcl.Triangle(0, 1, 2)) + tri.append(hppfcl.Triangle(0, 1, 3)) + tri.append(hppfcl.Triangle(0, 2, 3)) + tri.append(hppfcl.Triangle(1, 2, 3)) return hppfcl.Convex(pts, tri) -class TestMainAPI(TestCase): +class TestMainAPI(TestCase): def test_convex_halfspace(self): convex = tetahedron() - halfspace = hppfcl.Halfspace(np.array((0,0,1)), 0) + halfspace = hppfcl.Halfspace(np.array((0, 0, 1)), 0) - req=hppfcl.CollisionRequest() - res=hppfcl.CollisionResult() + req = hppfcl.CollisionRequest() + res = hppfcl.CollisionResult() M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, -0.1])) + M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, -0.1])) res.clear() hppfcl.collide(convex, M1, halfspace, M2, req, res) self.assertFalse(hppfcl.collide(convex, M1, halfspace, M2, req, res)) M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, 0.1])) + M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 0.1])) res.clear() self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res)) M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, 2])) + M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 2])) res.clear() self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res)) -if __name__ == '__main__': + +if __name__ == "__main__": unittest.main() diff --git a/test/python_unit/collision_manager.py b/test/python_unit/collision_manager.py index e5e2ea557..46f7fa925 100644 --- a/test/python_unit/collision_manager.py +++ b/test/python_unit/collision_manager.py @@ -5,14 +5,14 @@ sphere_obj = fcl.CollisionObject(sphere) M_sphere = fcl.Transform3f.Identity() -M_sphere.setTranslation(np.array([-0.6,0.,0.])) +M_sphere.setTranslation(np.array([-0.6, 0.0, 0.0])) sphere_obj.setTransform(M_sphere) box = fcl.Box(np.array([0.5, 0.5, 0.5])) box_obj = fcl.CollisionObject(box) M_box = fcl.Transform3f.Identity() -M_box.setTranslation(np.array([-0.6,0.,0.])) +M_box.setTranslation(np.array([-0.6, 0.0, 0.0])) box_obj.setTransform(M_box) collision_manager = fcl.DynamicAABBTreeCollisionManager() @@ -27,4 +27,4 @@ callback = fcl.CollisionCallBackDefault() collision_manager.collide(sphere_obj, callback) -assert callback.data.result.numContacts() == 1 \ No newline at end of file +assert callback.data.result.numContacts() == 1 diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py index b7ab201be..c6b6b6ee4 100644 --- a/test/python_unit/geometric_shapes.py +++ b/test/python_unit/geometric_shapes.py @@ -3,92 +3,98 @@ import hppfcl import numpy as np -class TestGeometricShapes(TestCase): +class TestGeometricShapes(TestCase): def test_capsule(self): - capsule = hppfcl.Capsule(1.,2.) + capsule = hppfcl.Capsule(1.0, 2.0) self.assertIsInstance(capsule, hppfcl.Capsule) self.assertIsInstance(capsule, hppfcl.ShapeBase) self.assertIsInstance(capsule, hppfcl.CollisionGeometry) self.assertEqual(capsule.getNodeType(), hppfcl.NODE_TYPE.GEOM_CAPSULE) - self.assertEqual(capsule.radius,1.) - self.assertEqual(capsule.halfLength,1.) - capsule.radius = 3. - capsule.halfLength = 4. - self.assertEqual(capsule.radius,3.) - self.assertEqual(capsule.halfLength,4.) + self.assertEqual(capsule.radius, 1.0) + self.assertEqual(capsule.halfLength, 1.0) + capsule.radius = 3.0 + capsule.halfLength = 4.0 + self.assertEqual(capsule.radius, 3.0) + self.assertEqual(capsule.halfLength, 4.0) com = capsule.computeCOM() self.assertApprox(com, np.zeros(3)) V = capsule.computeVolume() - V_cylinder = capsule.radius * capsule.radius * np.pi * 2. * capsule.halfLength - V_sphere = 4. * np.pi/3 * capsule.radius**3 + V_cylinder = capsule.radius * capsule.radius * np.pi * 2.0 * capsule.halfLength + V_sphere = 4.0 * np.pi / 3 * capsule.radius**3 V_ref = V_cylinder + V_sphere self.assertApprox(V, V_ref) I0 = capsule.computeMomentofInertia() - Iz_cylinder = V_cylinder * capsule.radius**2 / 2. + Iz_cylinder = V_cylinder * capsule.radius**2 / 2.0 Iz_sphere = 0.4 * V_sphere * capsule.radius * capsule.radius Iz_ref = Iz_cylinder + Iz_sphere - Ix_cylinder = V_cylinder*(3 * capsule.radius**2 + 4 * capsule.halfLength**2)/12. - V_hemi = 0.5 * V_sphere # volume of hemisphere - I0x_hemi = 0.5 * Iz_sphere # inertia of hemisphere w.r.t. origin - com_hemi = 3. * capsule.radius / 8. # CoM of hemisphere w.r.t. origin - Icx_hemi = I0x_hemi - V_hemi * com_hemi * com_hemi # inertia of hemisphere w.r.t. CoM - Ix_hemi = Icx_hemi + V_hemi * (capsule.halfLength + com_hemi)**2 # inertia of hemisphere w.r.t. tip of cylinder - Ix_ref = Ix_cylinder + 2*Ix_hemi # total inertia of capsule - I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref]) + Ix_cylinder = ( + V_cylinder * (3 * capsule.radius**2 + 4 * capsule.halfLength**2) / 12.0 + ) + V_hemi = 0.5 * V_sphere # volume of hemisphere + I0x_hemi = 0.5 * Iz_sphere # inertia of hemisphere w.r.t. origin + com_hemi = 3.0 * capsule.radius / 8.0 # CoM of hemisphere w.r.t. origin + Icx_hemi = ( + I0x_hemi - V_hemi * com_hemi * com_hemi + ) # inertia of hemisphere w.r.t. CoM + Ix_hemi = ( + Icx_hemi + V_hemi * (capsule.halfLength + com_hemi) ** 2 + ) # inertia of hemisphere w.r.t. tip of cylinder + Ix_ref = Ix_cylinder + 2 * Ix_hemi # total inertia of capsule + I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref]) self.assertApprox(I0, I0_ref) Ic = capsule.computeMomentofInertiaRelatedToCOM() self.assertApprox(Ic, I0_ref) def test_box1(self): - box = hppfcl.Box(np.array([1.,2.,3.])) + box = hppfcl.Box(np.array([1.0, 2.0, 3.0])) self.assertIsInstance(box, hppfcl.Box) self.assertIsInstance(box, hppfcl.ShapeBase) self.assertIsInstance(box, hppfcl.CollisionGeometry) self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX) - self.assertTrue(np.array_equal(box.halfSide,np.array([.5,1.,1.5]))) - box.halfSide = np.array([4.,5.,6.]) - self.assertTrue(np.array_equal(box.halfSide,np.array([4.,5.,6.]))) + self.assertTrue(np.array_equal(box.halfSide, np.array([0.5, 1.0, 1.5]))) + box.halfSide = np.array([4.0, 5.0, 6.0]) + self.assertTrue(np.array_equal(box.halfSide, np.array([4.0, 5.0, 6.0]))) com = box.computeCOM() self.assertApprox(com, np.zeros(3)) V = box.computeVolume() - x = float(2*box.halfSide[0]) - y = float(2*box.halfSide[1]) - z = float(2*box.halfSide[2]) + x = float(2 * box.halfSide[0]) + y = float(2 * box.halfSide[1]) + z = float(2 * box.halfSide[2]) V_ref = x * y * z self.assertApprox(V, V_ref) I0 = box.computeMomentofInertia() - Ix = V_ref * (y*y + z*z) / 12. - Iy = V_ref * (x*x + z*z) / 12. - Iz = V_ref * (y*y + x*x) / 12. - I0_ref = np.diag([Ix,Iy,Iz]) + Ix = V_ref * (y * y + z * z) / 12.0 + Iy = V_ref * (x * x + z * z) / 12.0 + Iz = V_ref * (y * y + x * x) / 12.0 + I0_ref = np.diag([Ix, Iy, Iz]) self.assertApprox(I0, I0_ref) Ic = box.computeMomentofInertiaRelatedToCOM() - self.assertApprox(Ic, I0_ref) + self.assertApprox(Ic, I0_ref) def test_box2(self): - box = hppfcl.Box(1.,2.,3) + box = hppfcl.Box(1.0, 2.0, 3) self.assertIsInstance(box, hppfcl.Box) self.assertIsInstance(box, hppfcl.ShapeBase) self.assertIsInstance(box, hppfcl.CollisionGeometry) self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX) - self.assertEqual(box.halfSide[0],0.5) - self.assertEqual(box.halfSide[1],1.0) - self.assertEqual(box.halfSide[2],1.5) - + self.assertEqual(box.halfSide[0], 0.5) + self.assertEqual(box.halfSide[1], 1.0) + self.assertEqual(box.halfSide[2], 1.5) + def test_sphere(self): - sphere = hppfcl.Sphere(1.) + sphere = hppfcl.Sphere(1.0) self.assertIsInstance(sphere, hppfcl.Sphere) self.assertIsInstance(sphere, hppfcl.ShapeBase) self.assertIsInstance(sphere, hppfcl.CollisionGeometry) self.assertEqual(sphere.getNodeType(), hppfcl.NODE_TYPE.GEOM_SPHERE) - self.assertEqual(sphere.radius, 1.) - sphere.radius = 2. - self.assertEqual(sphere.radius, 2.) + self.assertEqual(sphere.radius, 1.0) + sphere.radius = 2.0 + self.assertEqual(sphere.radius, 2.0) com = sphere.computeCOM() self.assertApprox(com, np.zeros(3)) V = sphere.computeVolume() - V_ref = 4. * np.pi/3 * sphere.radius**3 + V_ref = 4.0 * np.pi / 3 * sphere.radius**3 self.assertApprox(V, V_ref) I0 = sphere.computeMomentofInertia() I0_ref = 0.4 * V_ref * sphere.radius * sphere.radius * np.identity(3) @@ -97,70 +103,80 @@ def test_sphere(self): self.assertApprox(Ic, I0_ref) def test_cylinder(self): - cylinder = hppfcl.Cylinder(1.,2.) + cylinder = hppfcl.Cylinder(1.0, 2.0) self.assertIsInstance(cylinder, hppfcl.Cylinder) self.assertIsInstance(cylinder, hppfcl.ShapeBase) self.assertIsInstance(cylinder, hppfcl.CollisionGeometry) self.assertEqual(cylinder.getNodeType(), hppfcl.NODE_TYPE.GEOM_CYLINDER) - self.assertEqual(cylinder.radius,1.) - self.assertEqual(cylinder.halfLength,1.) - cylinder.radius = 3. - cylinder.halfLength = 4. - self.assertEqual(cylinder.radius,3.) - self.assertEqual(cylinder.halfLength,4.) + self.assertEqual(cylinder.radius, 1.0) + self.assertEqual(cylinder.halfLength, 1.0) + cylinder.radius = 3.0 + cylinder.halfLength = 4.0 + self.assertEqual(cylinder.radius, 3.0) + self.assertEqual(cylinder.halfLength, 4.0) com = cylinder.computeCOM() self.assertApprox(com, np.zeros(3)) V = cylinder.computeVolume() - V_ref = cylinder.radius * cylinder.radius * np.pi * 2. * cylinder.halfLength + V_ref = cylinder.radius * cylinder.radius * np.pi * 2.0 * cylinder.halfLength self.assertApprox(V, V_ref) I0 = cylinder.computeMomentofInertia() - Ix_ref = V_ref*(3 * cylinder.radius**2 + 4 * cylinder.halfLength**2)/12. - Iz_ref = V_ref * cylinder.radius**2 / 2. - I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref]) + Ix_ref = ( + V_ref * (3 * cylinder.radius**2 + 4 * cylinder.halfLength**2) / 12.0 + ) + Iz_ref = V_ref * cylinder.radius**2 / 2.0 + I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref]) self.assertApprox(I0, I0_ref) Ic = cylinder.computeMomentofInertiaRelatedToCOM() self.assertApprox(Ic, I0_ref) def test_cone(self): - cone = hppfcl.Cone(1.,2.) + cone = hppfcl.Cone(1.0, 2.0) self.assertIsInstance(cone, hppfcl.Cone) self.assertIsInstance(cone, hppfcl.ShapeBase) self.assertIsInstance(cone, hppfcl.CollisionGeometry) self.assertEqual(cone.getNodeType(), hppfcl.NODE_TYPE.GEOM_CONE) - self.assertEqual(cone.radius,1.) - self.assertEqual(cone.halfLength,1.) - cone.radius = 3. - cone.halfLength = 4. - self.assertEqual(cone.radius,3.) - self.assertEqual(cone.halfLength,4.) + self.assertEqual(cone.radius, 1.0) + self.assertEqual(cone.halfLength, 1.0) + cone.radius = 3.0 + cone.halfLength = 4.0 + self.assertEqual(cone.radius, 3.0) + self.assertEqual(cone.halfLength, 4.0) com = cone.computeCOM() - self.assertApprox(com, np.array([0.,0.,-0.5 * cone.halfLength])) + self.assertApprox(com, np.array([0.0, 0.0, -0.5 * cone.halfLength])) V = cone.computeVolume() - V_ref = np.pi * cone.radius**2 * 2. * cone.halfLength / 3. + V_ref = np.pi * cone.radius**2 * 2.0 * cone.halfLength / 3.0 self.assertApprox(V, V_ref) I0 = cone.computeMomentofInertia() - Ix_ref = V_ref * (3./20. * cone.radius**2 + 0.4 * cone.halfLength**2) + Ix_ref = V_ref * (3.0 / 20.0 * cone.radius**2 + 0.4 * cone.halfLength**2) Iz_ref = 0.3 * V_ref * cone.radius**2 - I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref]) + I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref]) self.assertApprox(I0, I0_ref) Ic = cone.computeMomentofInertiaRelatedToCOM() - Icx_ref = V_ref * 3./20. * (cone.radius**2 + cone.halfLength**2) - Ic_ref = np.diag([Icx_ref,Icx_ref,Iz_ref]) + Icx_ref = V_ref * 3.0 / 20.0 * (cone.radius**2 + cone.halfLength**2) + Ic_ref = np.diag([Icx_ref, Icx_ref, Iz_ref]) self.assertApprox(Ic, Ic_ref) def test_convex(self): - verts = hppfcl.StdVec_Vec3f () - faces = hppfcl.StdVec_Triangle () - verts.extend( [ np.array([0, 0, 0]), np.array([0, 1, 0]), np.array([1, 0, 0]), ]) - faces.append(hppfcl.Triangle(0,1,2)) + verts = hppfcl.StdVec_Vec3f() + faces = hppfcl.StdVec_Triangle() + verts.extend( + [ + np.array([0, 0, 0]), + np.array([0, 1, 0]), + np.array([1, 0, 0]), + ] + ) + faces.append(hppfcl.Triangle(0, 1, 2)) convex = hppfcl.Convex(verts, faces) - verts.append (np.array([0, 0, 1])) + verts.append(np.array([0, 0, 1])) try: convexHull = hppfcl.Convex.convexHull(verts, False, None) qhullAvailable = True except Exception as e: - self.assertEqual(str(e), "Library built without qhull. Cannot build object of this type.") + self.assertEqual( + str(e), "Library built without qhull. Cannot build object of this type." + ) qhullAvailable = False if qhullAvailable: @@ -170,7 +186,10 @@ def test_convex(self): try: convexHull = hppfcl.Convex.convexHull(verts[:3], False, None) except Exception as e: - self.assertIn(str(e), "You shouldn't use this function with less than 4 points.") + self.assertIn( + str(e), "You shouldn't use this function with less than 4 points." + ) + -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/test/python_unit/test_case.py b/test/python_unit/test_case.py index e6bec282b..219927a94 100644 --- a/test/python_unit/test_case.py +++ b/test/python_unit/test_case.py @@ -1,7 +1,7 @@ import unittest import numpy as np + class TestCase(unittest.TestCase): def assertApprox(self, a, b, epsilon=1e-6): return self.assertTrue(np.allclose(a, b, epsilon), "%s !~= %s" % (a, b)) - diff --git a/test/scripts/collision-bench.py b/test/scripts/collision-bench.py index 3497c83a2..885995ad3 100644 --- a/test/scripts/collision-bench.py +++ b/test/scripts/collision-bench.py @@ -2,75 +2,122 @@ import csv, sys, numpy as np from math import sqrt -filename = sys.argv[1] +filename = sys.argv[1] -with open(filename, 'r') as file: - reader = csv.reader (file, strict = True) +with open(filename, "r") as file: + reader = csv.reader(file, strict=True) fieldnames = None - #fieldnames = reader.fieldnames - converters = (str, int, int, int, float, lambda x: float(x)*1e-3) + # fieldnames = reader.fieldnames + converters = (str, int, int, int, float, lambda x: float(x) * 1e-3) for row in reader: if fieldnames is None: - fieldnames = [ n.strip() for n in row] + fieldnames = [n.strip() for n in row] values = [] continue - values.append ( [ c(v) for v, c in zip(row, converters) ] ) + values.append([c(v) for v, c in zip(row, converters)]) -request1 = values[:int(len(values)/2)] -request2 = values[int(len(values)/2):] +request1 = values[: int(len(values) / 2)] +request2 = values[int(len(values) / 2) :] Ntransforms = 1 while values[0][0:3] == values[Ntransforms][0:3]: Ntransforms += 1 -splitMethods = ['avg', 'med', 'cen'] -type = ["o", "or", "r", ] -BVs = sorted (list (set ([ v[0] for v in request1[::Ntransforms] ]))) -xvals = [ BVs.index(v[0]) + len(BVs)*v[2] + 3*len(BVs)*v[1] for v in request1[::Ntransforms] ] -cases = [ v[0] + " " + type[v[1]] + " " + splitMethods[v[2]] for v in request1[::Ntransforms] ] +splitMethods = ["avg", "med", "cen"] +type = [ + "o", + "or", + "r", +] +BVs = sorted(list(set([v[0] for v in request1[::Ntransforms]]))) +xvals = [ + BVs.index(v[0]) + len(BVs) * v[2] + 3 * len(BVs) * v[1] + for v in request1[::Ntransforms] +] +cases = [ + v[0] + " " + type[v[1]] + " " + splitMethods[v[2]] for v in request1[::Ntransforms] +] -idx_reorder = sorted (list(range(len(xvals))), key=lambda i: xvals[i]) -def reorder (l): return [ l[i] for i in idx_reorder ] +idx_reorder = sorted(list(range(len(xvals))), key=lambda i: xvals[i]) -xvals_s = reorder (xvals) -cases_s = reorder (cases) + +def reorder(l): + return [l[i] for i in idx_reorder] + + +xvals_s = reorder(xvals) +cases_s = reorder(cases) onlyLB = True # Time plt.figure(0) for i in range(Ntransforms): if not onlyLB: - plt.plot(xvals_s, reorder([ v[5] for v in request1[i::Ntransforms] ]) , '-.o', label=str(i)) - plt.plot(xvals_s, reorder([ v[5] for v in request2[i::Ntransforms] ]) , ':+', label=str(i)+"+lb") + plt.plot( + xvals_s, + reorder([v[5] for v in request1[i::Ntransforms]]), + "-.o", + label=str(i), + ) + plt.plot( + xvals_s, + reorder([v[5] for v in request2[i::Ntransforms]]), + ":+", + label=str(i) + "+lb", + ) plt.legend() plt.xticks(ticks=xvals_s, labels=cases_s, rotation=90) -plt.ylabel('Time (us)') -plt.yscale('log') +plt.ylabel("Time (us)") +plt.yscale("log") # Time plt.figure(2) -for k in range (0, len(request1), Ntransforms): +for k in range(0, len(request1), Ntransforms): if not onlyLB: - plt.plot([ xvals[int(k/Ntransforms)], ], sum([ v[5] for v in request1[k:k+Ntransforms] ])/Ntransforms) - plt.plot([ xvals[int(k/Ntransforms)], ], sum([ v[5] for v in request2[k:k+Ntransforms] ])/Ntransforms) - -plt.plot(xvals_s, reorder ([ sum([ v[5] for v in request2[k:k+Ntransforms] ])/Ntransforms for k in range (0, len(request1), Ntransforms) ])) + plt.plot( + [ + xvals[int(k / Ntransforms)], + ], + sum([v[5] for v in request1[k : k + Ntransforms]]) / Ntransforms, + ) + plt.plot( + [ + xvals[int(k / Ntransforms)], + ], + sum([v[5] for v in request2[k : k + Ntransforms]]) / Ntransforms, + ) + +plt.plot( + xvals_s, + reorder( + [ + sum([v[5] for v in request2[k : k + Ntransforms]]) / Ntransforms + for k in range(0, len(request1), Ntransforms) + ] + ), +) plt.xticks(ticks=xvals_s, labels=cases_s, rotation=90) -plt.ylabel('Time (us)') -plt.yscale('log') +plt.ylabel("Time (us)") +plt.yscale("log") # Distance lower bound plt.figure(1) for i in range(Ntransforms): - if request2[i][3] > 0: continue - plt.plot(xvals_s, reorder([ v[4] for v in request2[i::Ntransforms] ]) , ':+', label=str(i)+"+lb") + if request2[i][3] > 0: + continue + plt.plot( + xvals_s, + reorder([v[4] for v in request2[i::Ntransforms]]), + ":+", + label=str(i) + "+lb", + ) plt.legend() -plt.ylabel('Distance lower bound') +plt.ylabel("Distance lower bound") plt.xticks(ticks=xvals_s, labels=cases_s, rotation=90) plt.show() diff --git a/test/scripts/collision.py b/test/scripts/collision.py index b53a0d551..1604b9637 100644 --- a/test/scripts/collision.py +++ b/test/scripts/collision.py @@ -5,35 +5,39 @@ from gepetto.corbaserver import Client path = None -devel_hpp_dir = os.getenv ('DEVEL_HPP_DIR') +devel_hpp_dir = os.getenv("DEVEL_HPP_DIR") if devel_hpp_dir: - path = devel_hpp_dir + '/src/hpp-fcl/test/fcl_resources' + path = devel_hpp_dir + "/src/hpp-fcl/test/fcl_resources" else: - path = os.getenv ('PWD') + '/fcl_resources' + path = os.getenv("PWD") + "/fcl_resources" -Red = [1, 0, 0, .5] -Green = [0, 1, 0, .5] -Blue = [0, 0, 1, .5] +Red = [1, 0, 0, 0.5] +Green = [0, 1, 0, 0.5] +Blue = [0, 0, 1, 0.5] -c = Client () +c = Client() wid = 0 -sceneName = 'scene' -wid = c.gui.createWindow ('test-fcl') +sceneName = "scene" +wid = c.gui.createWindow("test-fcl") -c.gui.createScene (sceneName) -c.gui.addSceneToWindow (sceneName, wid) +c.gui.createScene(sceneName) +c.gui.addSceneToWindow(sceneName, wid) -c.gui.addMesh ("env", path + "/env.obj") -c.gui.addMesh ("rob", path + "/rob.obj") -c.gui.addToGroup ("env", sceneName) -c.gui.addToGroup ("rob", sceneName) +c.gui.addMesh("env", path + "/env.obj") +c.gui.addMesh("rob", path + "/rob.obj") +c.gui.addToGroup("env", sceneName) +c.gui.addToGroup("rob", sceneName) -q2 = (0,0,0,0,0,0,1) -q1=(-1435.35587657243, 2891.398094594479, 1462.830701842904)+( 0.6741912139367736, -0.2384437590607974, 0.6418622372743962, -0.2768097707389008) +q2 = (0, 0, 0, 0, 0, 0, 1) +q1 = (-1435.35587657243, 2891.398094594479, 1462.830701842904) + ( + 0.6741912139367736, + -0.2384437590607974, + 0.6418622372743962, + -0.2768097707389008, +) -c.gui.applyConfiguration ('env', q1) -c.gui.applyConfiguration ('rob', q2) - -c.gui.refresh () +c.gui.applyConfiguration("env", q1) +c.gui.applyConfiguration("rob", q2) +c.gui.refresh() diff --git a/test/scripts/distance_lower_bound.py b/test/scripts/distance_lower_bound.py index 4eee3c2b6..1e4b29b52 100644 --- a/test/scripts/distance_lower_bound.py +++ b/test/scripts/distance_lower_bound.py @@ -5,35 +5,42 @@ from gepetto.corbaserver import Client path = None -devel_hpp_dir = os.getenv ('DEVEL_HPP_DIR') +devel_hpp_dir = os.getenv("DEVEL_HPP_DIR") if devel_hpp_dir: - path = devel_hpp_dir + '/src/hpp-fcl/test/fcl_resources' + path = devel_hpp_dir + "/src/hpp-fcl/test/fcl_resources" else: - path = os.getenv ('PWD') + '/fcl_resources' + path = os.getenv("PWD") + "/fcl_resources" -Red = [1, 0, 0, .5] -Green = [0, 1, 0, .5] -Blue = [0, 0, 1, .5] +Red = [1, 0, 0, 0.5] +Green = [0, 1, 0, 0.5] +Blue = [0, 0, 1, 0.5] -c = Client () +c = Client() wid = 0 -sceneName = 'scene' -wid = c.gui.createWindow ('test-fcl') +sceneName = "scene" +wid = c.gui.createWindow("test-fcl") -c.gui.createScene (sceneName) -c.gui.addSceneToWindow (sceneName, wid) +c.gui.createScene(sceneName) +c.gui.addSceneToWindow(sceneName, wid) -c.gui.addMesh ("env", path + "/env.obj") -c.gui.addToGroup ("env", sceneName) -c.gui.addBox ("box", 500, 200, 150, Blue) -c.gui.addToGroup ("box", sceneName) +c.gui.addMesh("env", path + "/env.obj") +c.gui.addToGroup("env", sceneName) +c.gui.addBox("box", 500, 200, 150, Blue) +c.gui.addToGroup("box", sceneName) -q2 = (0,0,0,0,0,0,1) -q1=(608.56046341359615, 1624.1152798756957, 2661.5910432301462, 0.8083991299501978, 0.25803832576728564, 0.47026407332553366, 0.24240208429437343) +q2 = (0, 0, 0, 0, 0, 0, 1) +q1 = ( + 608.56046341359615, + 1624.1152798756957, + 2661.5910432301462, + 0.8083991299501978, + 0.25803832576728564, + 0.47026407332553366, + 0.24240208429437343, +) -c.gui.applyConfiguration ('env', q1) -c.gui.applyConfiguration ('rob', q2) - -c.gui.refresh () +c.gui.applyConfiguration("env", q1) +c.gui.applyConfiguration("rob", q2) +c.gui.refresh() diff --git a/test/scripts/geometric_shapes.py b/test/scripts/geometric_shapes.py index f5f03a23f..02dccadea 100644 --- a/test/scripts/geometric_shapes.py +++ b/test/scripts/geometric_shapes.py @@ -2,28 +2,30 @@ from gepetto.corbaserver import Client from gepetto import Quaternion -def translate (tr, t, d): - return [ tr[i] + d*t[i] for i in range(3) ] + tr[3:] -cl = Client () +def translate(tr, t, d): + return [tr[i] + d * t[i] for i in range(3)] + tr[3:] + + +cl = Client() try: cl.gui.getWindowID("fcl") except: cl.gui.createWindow("fcl") -cl.gui.addBox ('fcl/b0', 2, 2, 2, [1,0,0,0.5]) -cl.gui.addBox ('fcl/b1', 2, 2, 2, [0,1,0,0.5]) -cl.gui.setWireFrameMode ('fcl/b1', "WIREFRAME") -cl.gui.addBox ('fcl/b1_0', 2, 2, 2, [0,0 ,1,0.5]) -cl.gui.addBox ('fcl/b1_1', 2, 2, 2, [0,0.5,1,0.5]) +cl.gui.addBox("fcl/b0", 2, 2, 2, [1, 0, 0, 0.5]) +cl.gui.addBox("fcl/b1", 2, 2, 2, [0, 1, 0, 0.5]) +cl.gui.setWireFrameMode("fcl/b1", "WIREFRAME") +cl.gui.addBox("fcl/b1_0", 2, 2, 2, [0, 0, 1, 0.5]) +cl.gui.addBox("fcl/b1_1", 2, 2, 2, [0, 0.5, 1, 0.5]) -cl.gui.addSphere ("fcl/p0", 0.01, [1, 0, 1, 1]) -cl.gui.addSphere ("fcl/p1", 0.01, [0, 1, 1, 1]) +cl.gui.addSphere("fcl/p0", 0.01, [1, 0, 1, 1]) +cl.gui.addSphere("fcl/p1", 0.01, [0, 1, 1, 1]) -cl.gui.addArrow ("fcl/n0", 0.01, 1., [1, 0, 1, 1]) -cl.gui.addArrow ("fcl/n1", 0.01, 1., [0, 1, 1, 1]) +cl.gui.addArrow("fcl/n0", 0.01, 1.0, [1, 0, 1, 1]) +cl.gui.addArrow("fcl/n1", 0.01, 1.0, [0, 1, 1, 1]) -eps = 0. +eps = 0.0 d0 = 1.5183589910964868 + eps n0 = [0.0310588, 0.942603, -0.332467] d1 = 1.7485932899646754 + eps @@ -31,21 +33,39 @@ def translate (tr, t, d): qn0 = Quaternion() qn1 = Quaternion() -qn0.fromTwoVector([1,0,0], n0) -qn1.fromTwoVector([1,0,0], n1) - -pb1 = [ 0.135584, 0.933659, 0.290395, 0.119895, 0.977832, -0.164725, 0.0483272 ] -pb1_0 = translate (pb1, n0, d0) -pb1_1 = translate (pb1, n1, -d1) -cl.gui.applyConfiguration ("fcl/b1", pb1) -cl.gui.applyConfiguration ("fcl/b1_0", pb1_0) -cl.gui.applyConfiguration ("fcl/b1_1", pb1_1) - -cl.gui.applyConfigurations(["fcl/p0","fcl/p1"], [ - [0.832569, 0.259513, -0.239598, 0,0,0,1], - [-0.879579, 0.719545, 0.171906, 0,0,0,1] ]) -cl.gui.applyConfigurations(["fcl/n0","fcl/n1"], [ - ( 0.832569, 0.259513, -0.239598, ) + qn0.toTuple(), - ( -0.879579, 0.719545, 0.171906, ) + qn1.toTuple() ]) +qn0.fromTwoVector([1, 0, 0], n0) +qn1.fromTwoVector([1, 0, 0], n1) + +pb1 = [0.135584, 0.933659, 0.290395, 0.119895, 0.977832, -0.164725, 0.0483272] +pb1_0 = translate(pb1, n0, d0) +pb1_1 = translate(pb1, n1, -d1) +cl.gui.applyConfiguration("fcl/b1", pb1) +cl.gui.applyConfiguration("fcl/b1_0", pb1_0) +cl.gui.applyConfiguration("fcl/b1_1", pb1_1) + +cl.gui.applyConfigurations( + ["fcl/p0", "fcl/p1"], + [ + [0.832569, 0.259513, -0.239598, 0, 0, 0, 1], + [-0.879579, 0.719545, 0.171906, 0, 0, 0, 1], + ], +) +cl.gui.applyConfigurations( + ["fcl/n0", "fcl/n1"], + [ + ( + 0.832569, + 0.259513, + -0.239598, + ) + + qn0.toTuple(), + ( + -0.879579, + 0.719545, + 0.171906, + ) + + qn1.toTuple(), + ], +) cl.gui.refresh() diff --git a/test/scripts/gjk.py b/test/scripts/gjk.py index b92dac605..bdad97536 100644 --- a/test/scripts/gjk.py +++ b/test/scripts/gjk.py @@ -3,21 +3,21 @@ import numpy as np from gepetto.corbaserver import Client -Red = [1, 0, 0, .5] -Green = [0, 1, 0, .5] -Blue = [0, 0, 1, .5] +Red = [1, 0, 0, 0.5] +Green = [0, 1, 0, 0.5] +Blue = [0, 0, 1, 0.5] -c = Client () -wid = len (c.gui.getWindowList ()) - 1 +c = Client() +wid = len(c.gui.getWindowList()) - 1 -sceneName = 'scene/triangles' -if sceneName in c.gui.getNodeList (): - c.gui.deleteNode (sceneName, True) +sceneName = "scene/triangles" +if sceneName in c.gui.getNodeList(): + c.gui.deleteNode(sceneName, True) -wid = c.gui.createWindow ('triangles') +wid = c.gui.createWindow("triangles") -c.gui.createScene (sceneName) -c.gui.addSceneToWindow (sceneName, wid) +c.gui.createScene(sceneName) +c.gui.addSceneToWindow(sceneName, wid) P1 = (-0.6475786872429674, -0.519875255189778, 0.5955961037406681) P2 = (0.069105957031249998, -0.150722900390625, -0.42999999999999999) @@ -25,17 +25,22 @@ Q1 = (-25.655000000000001, -1.2858199462890625, 3.7249809570312502) Q2 = (-10.926, -1.284259033203125, 3.7281499023437501) Q3 = (-10.926, -1.2866180419921875, 3.72335400390625) -tf1 = (-12.824601270753471, -1.6840516940066426, 3.8914453043793844, - -0.26862477561450587, -0.46249645019513175, 0.73064726592483387, - -0.42437287410898855) +tf1 = ( + -12.824601270753471, + -1.6840516940066426, + 3.8914453043793844, + -0.26862477561450587, + -0.46249645019513175, + 0.73064726592483387, + -0.42437287410898855, +) tf2 = (0, 0, 0, 0, 0, 0, 1) -c.gui.addTriangleFace ("triangle1", P1, P2, P3, Red) -c.gui.addTriangleFace ("triangle2", Q1, Q2, Q3, Green) -c.gui.addToGroup ('triangle1', sceneName) -c.gui.addToGroup ('triangle2', sceneName) - -c.gui.applyConfiguration ('triangle1', tf1) -c.gui.applyConfiguration ('triangle2', tf2) -c.gui.refresh () +c.gui.addTriangleFace("triangle1", P1, P2, P3, Red) +c.gui.addTriangleFace("triangle2", Q1, Q2, Q3, Green) +c.gui.addToGroup("triangle1", sceneName) +c.gui.addToGroup("triangle2", sceneName) +c.gui.applyConfiguration("triangle1", tf1) +c.gui.applyConfiguration("triangle2", tf2) +c.gui.refresh() diff --git a/test/scripts/obb.py b/test/scripts/obb.py index 1f9141cd7..7b59e6159 100644 --- a/test/scripts/obb.py +++ b/test/scripts/obb.py @@ -2,41 +2,55 @@ import csv, sys, numpy as np from math import sqrt -filename = sys.argv[1] +filename = sys.argv[1] -with open(filename, 'r') as file: - reader = csv.reader (file, strict = True) +with open(filename, "r") as file: + reader = csv.reader(file, strict=True) fieldnames = None - #fieldnames = reader.fieldnames + # fieldnames = reader.fieldnames for row in reader: if fieldnames is None: - fieldnames = [ n.strip() for n in row] - values = [ [] for _ in fieldnames ] + fieldnames = [n.strip() for n in row] + values = [[] for _ in fieldnames] continue - values[0].append (int(row[0])) + values[0].append(int(row[0])) for i, v in enumerate(row[1:]): - values[i+1].append (float(v)) + values[i + 1].append(float(v)) # Compute mean and variance for each values, for each separating axis -means = [ [ 0., ] * 12 for _ in fieldnames[4:] ] -stddevs = [ [ 0., ] * 12 for _ in fieldnames[4:] ] -nb_occurence = [ 0, ] * 12 +means = [ + [ + 0.0, + ] + * 12 + for _ in fieldnames[4:] +] +stddevs = [ + [ + 0.0, + ] + * 12 + for _ in fieldnames[4:] +] +nb_occurence = [ + 0, +] * 12 for i, id in enumerate(values[0]): nb_occurence[id] += 1 for i, id in enumerate(values[0]): for k, n in enumerate(fieldnames[4:]): - v = values[k+4][i] - means [k][id] += v / nb_occurence[id] + v = values[k + 4][i] + means[k][id] += v / nb_occurence[id] stddevs[k][id] += v * v / nb_occurence[id] for k, n in enumerate(fieldnames[4:]): for id in range(12): - #means [k][id] /= nb_occurence[id] - #stddevs[k][id] = sqrt (stddevs[k][id]) / nb_occurence[id] - means[k][id]) - stddevs[k][id] = sqrt (stddevs[k][id] - means[k][id]*means[k][id]) + # means [k][id] /= nb_occurence[id] + # stddevs[k][id] = sqrt (stddevs[k][id]) / nb_occurence[id] - means[k][id]) + stddevs[k][id] = sqrt(stddevs[k][id] - means[k][id] * means[k][id]) subplots = False Nrows = 1 @@ -47,63 +61,87 @@ avg_time_vs_impl = True if time_vs_sep_axis: - if subplots: plt.subplot (Nrows, Ncols, iplot) - else: plt.figure (iplot) - plt.title ("Time, with std dev, versus separating axis") + if subplots: + plt.subplot(Nrows, Ncols, iplot) + else: + plt.figure(iplot) + plt.title("Time, with std dev, versus separating axis") for k, n in enumerate(fieldnames[4:]): - #plt.errorbar ([ np.linspace(0, 11, 12) + shift for shift in np.linspace (-0.2, 0.2, ) ], means[k], stddevs[k], label=n) - plt.errorbar (np.linspace(0, 11, 12), means[k], stddevs[k], label=n) + # plt.errorbar ([ np.linspace(0, 11, 12) + shift for shift in np.linspace (-0.2, 0.2, ) ], means[k], stddevs[k], label=n) + plt.errorbar(np.linspace(0, 11, 12), means[k], stddevs[k], label=n) # plt.errorbar (np.linspace(0, 11, 12), means[k], [ [ 0 ] * len(stddevs[k]), stddevs[k] ], label=n) - plt.xlim([-0.5,11.5]) - plt.ylabel('Time (ns)') - plt.xlabel('Separating axis') - plt.legend(loc='upper left') + plt.xlim([-0.5, 11.5]) + plt.ylabel("Time (ns)") + plt.xlabel("Separating axis") + plt.legend(loc="upper left") axx = plt.gca().twinx() - axx.hist (values[0], bins=[ i-0.5 for i in range(13) ], bottom=-0.5, cumulative=True, - rwidth=0.5, fill=False, label='Cumulative occurence') - axx.set_ylabel('Nb occurence of a separating axis.') - plt.legend(loc='lower right') + axx.hist( + values[0], + bins=[i - 0.5 for i in range(13)], + bottom=-0.5, + cumulative=True, + rwidth=0.5, + fill=False, + label="Cumulative occurence", + ) + axx.set_ylabel("Nb occurence of a separating axis.") + plt.legend(loc="lower right") iplot += 1 if nb_occ_sep_axis: - if subplots: plt.subplot (Nrows, Ncols, iplot) - else: plt.figure (iplot) - plt.title ("Nb of occurence per separating axis") - plt.hist (values[0], bins=[ i-0.5 for i in range(13) ]) - plt.ylabel('Nb occurence') - plt.xlabel('Separating axis') + if subplots: + plt.subplot(Nrows, Ncols, iplot) + else: + plt.figure(iplot) + plt.title("Nb of occurence per separating axis") + plt.hist(values[0], bins=[i - 0.5 for i in range(13)]) + plt.ylabel("Nb occurence") + plt.xlabel("Separating axis") dlb_id = 1 d_id = 2 - #plt.title ("Time, with std dev, versus distance") - #for k, n in enumerate(fieldnames[4:]): - #plt.plot (values[dlb_id], values[k+4], '.', label=n) + # plt.title ("Time, with std dev, versus distance") + # for k, n in enumerate(fieldnames[4:]): + # plt.plot (values[dlb_id], values[k+4], '.', label=n) iplot += 1 if avg_time_vs_impl: - if subplots: plt.subplot (Nrows, Ncols, iplot) - else: plt.figure (iplot) - plt.title ("Average time versus the implementation") - #plt.boxplot(values[4:], labels=fieldnames[4:], showmeans=True) - _mins = np.min (values[4:], axis=1) - _maxs = np.max (values[4:], axis=1) - _means = np.mean (values[4:], axis=1) - _stddev = np.std (values[4:], axis=1) - _sorted = sorted ( zip(fieldnames[4:], _means, _stddev, _mins, _maxs), key=lambda x: x[1]) + if subplots: + plt.subplot(Nrows, Ncols, iplot) + else: + plt.figure(iplot) + plt.title("Average time versus the implementation") + # plt.boxplot(values[4:], labels=fieldnames[4:], showmeans=True) + _mins = np.min(values[4:], axis=1) + _maxs = np.max(values[4:], axis=1) + _means = np.mean(values[4:], axis=1) + _stddev = np.std(values[4:], axis=1) + _sorted = sorted( + zip(fieldnames[4:], _means, _stddev, _mins, _maxs), key=lambda x: x[1] + ) plt.errorbar( - [ f for f,m,s,l,u in _sorted], - [ m for f,m,s,l,u in _sorted], - [ s for f,m,s,l,u in _sorted], - fmt='go', linestyle='', label='mean and std deviation') - plt.plot ( - [ f for f,m,s,l,u in _sorted], - [ l for f,m,s,l,u in _sorted], - 'b+', ls='', label='min') - plt.plot ( - [ f for f,m,s,l,u in _sorted], - [ u for f,m,s,l,u in _sorted], - 'r+', ls='', label='max') - plt.ylabel('Time (ns)') + [f for f, m, s, l, u in _sorted], + [m for f, m, s, l, u in _sorted], + [s for f, m, s, l, u in _sorted], + fmt="go", + linestyle="", + label="mean and std deviation", + ) + plt.plot( + [f for f, m, s, l, u in _sorted], + [l for f, m, s, l, u in _sorted], + "b+", + ls="", + label="min", + ) + plt.plot( + [f for f, m, s, l, u in _sorted], + [u for f, m, s, l, u in _sorted], + "r+", + ls="", + label="max", + ) + plt.ylabel("Time (ns)") plt.xticks(rotation=20) plt.legend() diff --git a/test/scripts/octree.py b/test/scripts/octree.py index a58092b42..a5c865f14 100644 --- a/test/scripts/octree.py +++ b/test/scripts/octree.py @@ -5,50 +5,54 @@ import os from gepetto.corbaserver import Client -pos = list () -with open ("/home/florent/devel/hpp/src/hpp-fcl/build-rel/test/rob.octree", "r") as f: - r = csv.reader (f, delimiter = ',') +pos = list() +with open("/home/florent/devel/hpp/src/hpp-fcl/build-rel/test/rob.octree", "r") as f: + r = csv.reader(f, delimiter=",") for line in r: - pos.append (map (float, line)) + pos.append(map(float, line)) path = None -devel_hpp_dir = os.getenv ('DEVEL_HPP_DIR') +devel_hpp_dir = os.getenv("DEVEL_HPP_DIR") if devel_hpp_dir: - path = devel_hpp_dir + '/src/hpp-fcl/test/fcl_resources' + path = devel_hpp_dir + "/src/hpp-fcl/test/fcl_resources" else: - path = os.getenv ('PWD') + '/fcl_resources' + path = os.getenv("PWD") + "/fcl_resources" -Red = [1, 0, 0, .5] -Green = [0, 1, 0, .5] -Blue = [0, 0, 1, .5] +Red = [1, 0, 0, 0.5] +Green = [0, 1, 0, 0.5] +Blue = [0, 0, 1, 0.5] -c = Client () +c = Client() wid = 0 -sceneName = 'scene' -wid = c.gui.createWindow ('test-fcl') +sceneName = "scene" +wid = c.gui.createWindow("test-fcl") -c.gui.createScene (sceneName) -c.gui.addSceneToWindow (sceneName, wid) +c.gui.createScene(sceneName) +c.gui.addSceneToWindow(sceneName, wid) -c.gui.addMesh ("env", path + "/env.obj") -c.gui.addMesh ("rob", path + "/rob.obj") -c.gui.addToGroup ("env", sceneName) -c.gui.addToGroup ("rob", sceneName) +c.gui.addMesh("env", path + "/env.obj") +c.gui.addMesh("rob", path + "/rob.obj") +c.gui.addToGroup("env", sceneName) +c.gui.addToGroup("rob", sceneName) # closest points -c.gui.addSphere ("p1", 10, Red) -c.gui.addToGroup ("p1", sceneName) +c.gui.addSphere("p1", 10, Red) +c.gui.addToGroup("p1", sceneName) -q1=(-1373.283643275499, -396.2224237620831, 259.5808934420347)+(0.5956794918568784, -0.2147188057951074, 0.257436335996247, 0.7299234962157893) -q2=(0,0,0,0,0,0,1) +q1 = (-1373.283643275499, -396.2224237620831, 259.5808934420347) + ( + 0.5956794918568784, + -0.2147188057951074, + 0.257436335996247, + 0.7299234962157893, +) +q2 = (0, 0, 0, 0, 0, 0, 1) -c.gui.applyConfiguration ('rob', q1) -c.gui.applyConfiguration ('env', q2) +c.gui.applyConfiguration("rob", q1) +c.gui.applyConfiguration("env", q2) -p1 = (-1990.2983245164919,-105.42114741459312,359.74684390031132,0,0,0,1) - -c.gui.applyConfiguration ("p1", p1) -c.gui.refresh () +p1 = (-1990.2983245164919, -105.42114741459312, 359.74684390031132, 0, 0, 0, 1) +c.gui.applyConfiguration("p1", p1) +c.gui.refresh() diff --git a/test/serialization.cpp b/test/serialization.cpp index 46f371077..e9bfb6af7 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #define BOOST_TEST_MODULE FCL_SERIALIZATION #include #include @@ -67,126 +66,108 @@ namespace utf = boost::unit_test::framework; using namespace hpp::fcl; -template -void saveToBinary(const T & object, - boost::asio::streambuf & buffer) -{ +template +void saveToBinary(const T& object, boost::asio::streambuf& buffer) { boost::archive::binary_oarchive oa(buffer); - oa & object; + oa& object; } -template -inline void loadFromBinary(T & object, - boost::asio::streambuf & buffer) -{ +template +inline void loadFromBinary(T& object, boost::asio::streambuf& buffer) { boost::archive::binary_iarchive ia(buffer); ia >> object; } -template -bool check(const T & value, const T & other) -{ +template +bool check(const T& value, const T& other) { return value == other; } -enum SerializationMode -{ - TXT = 1, - XML = 2, - BIN = 4, - STREAM = 8 -}; - -template -void test_serialization(const T & value, T & other_value, - const int mode = TXT | XML | BIN | STREAM) -{ +enum SerializationMode { TXT = 1, XML = 2, BIN = 4, STREAM = 8 }; + +template +void test_serialization(const T& value, T& other_value, + const int mode = TXT | XML | BIN | STREAM) { const std::string tmp_dir(boost::archive::tmpdir()); const std::string txt_filename = tmp_dir + "file.txt"; const std::string bin_filename = tmp_dir + "file.bin"; // TXT - if(mode & 0x1) - { + if (mode & 0x1) { { std::ofstream ofs(txt_filename.c_str()); - + boost::archive::text_oarchive oa(ofs); oa << value; } - BOOST_CHECK(check(value,value)); - + BOOST_CHECK(check(value, value)); + { std::ifstream ifs(txt_filename.c_str()); boost::archive::text_iarchive ia(ifs); - + ia >> other_value; } - BOOST_CHECK(check(value,other_value)); + BOOST_CHECK(check(value, other_value)); } - + // BIN - if(mode & 0x4) - { + if (mode & 0x4) { { std::ofstream ofs(bin_filename.c_str(), std::ios::binary); boost::archive::binary_oarchive oa(ofs); oa << value; } - BOOST_CHECK(check(value,value)); - + BOOST_CHECK(check(value, value)); + { std::ifstream ifs(bin_filename.c_str(), std::ios::binary); boost::archive::binary_iarchive ia(ifs); - + ia >> other_value; } - BOOST_CHECK(check(value,other_value)); + BOOST_CHECK(check(value, other_value)); } - + // Stream Buffer - if(mode & 0x8) - { + if (mode & 0x8) { boost::asio::streambuf buffer; - saveToBinary(value,buffer); - BOOST_CHECK(check(value,value)); - - loadFromBinary(other_value,buffer); - BOOST_CHECK(check(value,other_value)); + saveToBinary(value, buffer); + BOOST_CHECK(check(value, value)); + + loadFromBinary(other_value, buffer); + BOOST_CHECK(check(value, other_value)); } } -template -void test_serialization(const T & value, - const int mode = TXT | XML | BIN | STREAM) -{ +template +void test_serialization(const T& value, + const int mode = TXT | XML | BIN | STREAM) { T other_value; - test_serialization(value,other_value,mode); + test_serialization(value, other_value, mode); } -BOOST_AUTO_TEST_CASE(test_aabb) -{ - AABB aabb(-Vec3f::Ones(),Vec3f::Ones()); +BOOST_AUTO_TEST_CASE(test_aabb) { + AABB aabb(-Vec3f::Ones(), Vec3f::Ones()); test_serialization(aabb); } -BOOST_AUTO_TEST_CASE(test_collision_data) -{ +BOOST_AUTO_TEST_CASE(test_collision_data) { Contact contact(NULL, NULL, 1, 2, Vec3f::Ones(), Vec3f::Zero(), -10.); test_serialization(contact); - - CollisionRequest collision_request(CONTACT,10); + + CollisionRequest collision_request(CONTACT, 10); test_serialization(collision_request); - + CollisionResult collision_result; collision_result.addContact(contact); collision_result.addContact(contact); collision_result.distance_lower_bound = 0.1; test_serialization(collision_result); - - DistanceRequest distance_request(true,1.,2.); + + DistanceRequest distance_request(true, 1., 2.); test_serialization(distance_request); - + DistanceResult distance_result; distance_result.normal.setOnes(); distance_result.nearest_points[0].setRandom(); @@ -194,16 +175,15 @@ BOOST_AUTO_TEST_CASE(test_collision_data) test_serialization(distance_result); } -BOOST_AUTO_TEST_CASE(test_BVHModel) -{ +BOOST_AUTO_TEST_CASE(test_BVHModel) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); - + loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); - BVHModel m1,m2; + BVHModel m1, m2; m1.beginModel(); m1.addSubModel(p1, t1); @@ -215,94 +195,88 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) m2.endModel(); BOOST_CHECK(m2 == m2); BOOST_CHECK(m1 != m2); - + // Test BVHModel { BVHModel m1_copy; - test_serialization(m1,m1_copy); + test_serialization(m1, m1_copy); } { BVHModel m1_copy; - test_serialization(m1,m1_copy,STREAM); + test_serialization(m1, m1_copy, STREAM); } } -BOOST_AUTO_TEST_CASE(test_HeightField) -{ +BOOST_AUTO_TEST_CASE(test_HeightField) { const FCL_REAL min_altitude = -1.; const FCL_REAL x_dim = 1., y_dim = 2.; const Eigen::DenseIndex nx = 100, ny = 200; const MatrixXf heights = MatrixXf::Random(ny, nx); - - HeightField hfield(x_dim,y_dim,heights,min_altitude); + + HeightField hfield(x_dim, y_dim, heights, min_altitude); // Test HeightField { HeightField hfield_copy; - test_serialization(hfield,hfield_copy); + test_serialization(hfield, hfield_copy); } { HeightField hfield_copy; - test_serialization(hfield,hfield_copy,STREAM); + test_serialization(hfield, hfield_copy, STREAM); } } -BOOST_AUTO_TEST_CASE(test_shapes) -{ - +BOOST_AUTO_TEST_CASE(test_shapes) { { - TriangleP triangle(Vec3f::UnitX(), - Vec3f::UnitY(), - Vec3f::UnitZ()); - TriangleP triangle_copy(Vec3f::Random(),Vec3f::Random(),Vec3f::Random()); - test_serialization(triangle,triangle_copy); + TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), Vec3f::UnitZ()); + TriangleP triangle_copy(Vec3f::Random(), Vec3f::Random(), Vec3f::Random()); + test_serialization(triangle, triangle_copy); } - + { Box box(Vec3f::UnitX()), box_copy(Vec3f::Random()); - test_serialization(box,box_copy); + test_serialization(box, box_copy); } - + { Sphere sphere(1.), sphere_copy(2.); - test_serialization(sphere,sphere_copy); + test_serialization(sphere, sphere_copy); } - + { - Capsule capsule(1.,2.), capsule_copy(10.,10.); - test_serialization(capsule,capsule_copy); + Capsule capsule(1., 2.), capsule_copy(10., 10.); + test_serialization(capsule, capsule_copy); } - + { - Cone cone(1.,2.), cone_copy(10.,10.); - test_serialization(cone,cone_copy); + Cone cone(1., 2.), cone_copy(10., 10.); + test_serialization(cone, cone_copy); } - + { - Cylinder cylinder(1.,2.), cylinder_copy(10.,10.); - test_serialization(cylinder,cylinder_copy); + Cylinder cylinder(1., 2.), cylinder_copy(10., 10.); + test_serialization(cylinder, cylinder_copy); } - + { - Halfspace hs(Vec3f::Random(),1.), hs_copy(Vec3f::Zero(),0.); - test_serialization(hs,hs_copy); + Halfspace hs(Vec3f::Random(), 1.), hs_copy(Vec3f::Zero(), 0.); + test_serialization(hs, hs_copy); } - + { - Plane plane(Vec3f::Random(),1.), plane_copy(Vec3f::Zero(),0.); - test_serialization(plane,plane_copy); + Plane plane(Vec3f::Random(), 1.), plane_copy(Vec3f::Zero(), 0.); + test_serialization(plane, plane_copy); } } -BOOST_AUTO_TEST_CASE(test_memory_footprint) -{ +BOOST_AUTO_TEST_CASE(test_memory_footprint) { Sphere sphere(1.); BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere)); - + std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); - + loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel m1; @@ -310,8 +284,10 @@ BOOST_AUTO_TEST_CASE(test_memory_footprint) m1.beginModel(); m1.addSubModel(p1, t1); m1.endModel(); - - std::cout << "computeMemoryFootprint(m1): " << computeMemoryFootprint(m1) << std::endl; + + std::cout << "computeMemoryFootprint(m1): " << computeMemoryFootprint(m1) + << std::endl; BOOST_CHECK(sizeof(BVHModel) < computeMemoryFootprint(m1)); - BOOST_CHECK(static_cast(m1.memUsage(false)) == computeMemoryFootprint(m1)); + BOOST_CHECK(static_cast(m1.memUsage(false)) == + computeMemoryFootprint(m1)); } diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp index 66abe8a63..0c0e7c907 100644 --- a/test/shape_mesh_consistency.cpp +++ b/test/shape_mesh_consistency.cpp @@ -44,15 +44,13 @@ #include #include "utility.h" - using namespace hpp::fcl; #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; -BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) -{ +BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { Sphere s1(20); Sphere s2(20); BVHModel s1_rss; @@ -68,39 +66,46 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) pose.setTranslation(Vec3f(50, 0, 0)); - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); - for(std::size_t i = 0; i < 10; ++i) - { + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); } pose.setTranslation(Vec3f(40.1, 0, 0)); @@ -108,42 +113,46 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) res.clear(), res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); - for(std::size_t i = 0; i < 10; ++i) - { + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); } } -BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) -{ +BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); @@ -160,81 +169,94 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) pose.setTranslation(Vec3f(50, 0, 0)); - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); - for(std::size_t i = 0; i < 10; ++i) - { + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); } pose.setTranslation(Vec3f(15.1, 0, 0)); - - res.clear(); res1.clear(); + + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); - for(std::size_t i = 0; i < 10; ++i) - { + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); } } -BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) -{ +BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); @@ -251,81 +273,95 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) pose.setTranslation(Vec3f(20, 0, 0)); - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); + res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - - for(std::size_t i = 0; i < 10; ++i) - { + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); + + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); } - - pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( - - res.clear(); res1.clear(); + + pose.setTranslation( + Vec3f(15, 0, 0)); // libccd cannot use small value here :( + + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - - for(std::size_t i = 0; i < 10; ++i) - { + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); } } -BOOST_AUTO_TEST_CASE(consistency_distance_conecone) -{ +BOOST_AUTO_TEST_CASE(consistency_distance_conecone) { Cone s1(5, 10); Cone s2(5, 10); @@ -341,82 +377,95 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) Transform3f pose; pose.setTranslation(Vec3f(20, 0, 0)); - - res.clear(); res1.clear(); + + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - - for(std::size_t i = 0; i < 10; ++i) - { + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); } - - pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( - - res.clear(); res1.clear(); + + pose.setTranslation( + Vec3f(15, 0, 0)); // libccd cannot use small value here :( + + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - - for(std::size_t i = 0; i < 10; ++i) - { + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); } } - -BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { Sphere s1(20); Sphere s2(20); BVHModel s1_rss; @@ -432,83 +481,94 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) pose.setTranslation(Vec3f(50, 0, 0)); - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); - for(std::size_t i = 0; i < 10; ++i) - { + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); } pose.setTranslation(Vec3f(40.1, 0, 0)); - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 4); - for(std::size_t i = 0; i < 10; ++i) - { + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 4); } } -BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { Box s1(20, 40, 50); Box s2(10, 10, 10); @@ -525,81 +585,94 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) pose.setTranslation(Vec3f(50, 0, 0)); - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); - for(std::size_t i = 0; i < 10; ++i) - { + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.01); } pose.setTranslation(Vec3f(15.1, 0, 0)); - - res.clear(); res1.clear(); + + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); - for(std::size_t i = 0; i < 10; ++i) - { + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); } } -BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { Cylinder s1(5, 10); Cylinder s2(5, 10); @@ -608,98 +681,114 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); - + DistanceRequest request; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(20, 0, 0)); - - res.clear(); res1.clear(); + + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - - for(std::size_t i = 0; i < 10; ++i) - { + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) - std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; + if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) + std::cout << "low resolution: " << res1.min_distance << " " + << res.min_distance << std::endl; else - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK( + fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) - std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; + if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) + std::cout << "low resolution: " << res1.min_distance << " " + << res.min_distance << std::endl; else - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK( + fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) - std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; + if (fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) + std::cout << "low resolution: " << res1.min_distance << " " + << res.min_distance << std::endl; else - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK( + fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - + pose.setTranslation(Vec3f(10.1, 0, 0)); - - res.clear(); res1.clear(); + + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - - for(std::size_t i = 0; i < 10; ++i) - { + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); } } -BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { Cone s1(5, 10); Cone s2(5, 10); @@ -716,83 +805,94 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) pose.setTranslation(Vec3f(20, 0, 0)); - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - - for(std::size_t i = 0; i < 10; ++i) - { + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); + + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 0.05); } - + pose.setTranslation(Vec3f(10.1, 0, 0)); - - res.clear(); res1.clear(); + + res.clear(); + res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - - for(std::size_t i = 0; i < 10; ++i) - { + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); + + for (std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); - + Transform3f pose1(t); Transform3f pose2 = t * pose; - res.clear(); res1.clear(); + res.clear(); + res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); - BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); + BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < + 2); } } - - -BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { Sphere s1(20); Sphere s2(10); BVHModel s1_aabb; @@ -805,14 +905,13 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; - // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision @@ -877,7 +976,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - + pose.setTranslation(Vec3f(30, 0, 0)); pose_aabb.setTranslation(Vec3f(30, 0, 0)); pose_obb.setTranslation(Vec3f(30, 0, 0)); @@ -909,10 +1008,12 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - + pose.setTranslation(Vec3f(29.9, 0, 0)); - pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision - pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + pose_aabb.setTranslation( + Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + pose_obb.setTranslation( + Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -942,10 +1043,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-29.9, 0, 0)); - pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision - pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + pose_aabb.setTranslation( + Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + pose_obb.setTranslation( + Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1008,8 +1110,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); @@ -1023,7 +1124,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) generateBVHModel(s1_obb, s1, Transform3f()); generateBVHModel(s2_obb, s2, Transform3f()); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; bool res; @@ -1128,8 +1229,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { Sphere s1(20); Box s2(5, 5, 5); @@ -1143,7 +1243,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f()); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; bool res; @@ -1248,8 +1348,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); @@ -1263,7 +1362,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; bool res; @@ -1335,8 +1434,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(consistency_collision_conecone) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { Cone s1(5, 10); Cone s2(5, 10); @@ -1350,7 +1448,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; bool res; @@ -1486,11 +1584,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) BOOST_CHECK_FALSE(res); } - - - -BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { Sphere s1(20); Sphere s2(10); BVHModel s1_aabb; @@ -1503,7 +1597,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; @@ -1511,7 +1605,6 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) Transform3f pose, pose_aabb, pose_obb; - // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision @@ -1576,7 +1669,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - + pose.setTranslation(Vec3f(30, 0, 0)); pose_aabb.setTranslation(Vec3f(30, 0, 0)); pose_obb.setTranslation(Vec3f(30, 0, 0)); @@ -1608,10 +1701,12 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - + pose.setTranslation(Vec3f(29.9, 0, 0)); - pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision - pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + pose_aabb.setTranslation( + Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + pose_obb.setTranslation( + Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1641,10 +1736,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-29.9, 0, 0)); - pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision - pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + pose_aabb.setTranslation( + Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + pose_obb.setTranslation( + Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1707,8 +1803,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { Box s1(20, 40, 50); Box s2(10, 10, 10); @@ -1722,7 +1817,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) generateBVHModel(s1_obb, s1, Transform3f()); generateBVHModel(s2_obb, s2, Transform3f()); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; @@ -1828,8 +1923,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { Sphere s1(20); Box s2(5, 5, 5); @@ -1843,7 +1937,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f()); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; @@ -1949,8 +2043,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { Cylinder s1(5, 10); Cylinder s2(5, 10); @@ -1964,7 +2057,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; @@ -2037,8 +2130,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) -{ +BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { Cone s1(5, 10); Cone s2(5, 10); @@ -2052,7 +2144,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); - CollisionRequest request (false, 1, false); + CollisionRequest request(false, 1, false); CollisionResult result; diff --git a/test/simple.cpp b/test/simple.cpp index b9ec455da..99a7ee521 100644 --- a/test/simple.cpp +++ b/test/simple.cpp @@ -11,23 +11,19 @@ using namespace hpp::fcl; static FCL_REAL epsilon = 1e-6; -static bool approx(FCL_REAL x, FCL_REAL y) -{ - return std::abs(x - y) < epsilon; -} +static bool approx(FCL_REAL x, FCL_REAL y) { return std::abs(x - y) < epsilon; } -BOOST_AUTO_TEST_CASE(projection_test_line) -{ +BOOST_AUTO_TEST_CASE(projection_test_line) { Vec3f v1(0, 0, 0); Vec3f v2(2, 0, 0); - + Vec3f p(1, 0, 0); Project::ProjectResult res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); - + p = Vec3f(-1, 0, 0); res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 1); @@ -41,11 +37,9 @@ BOOST_AUTO_TEST_CASE(projection_test_line) BOOST_CHECK(approx(res.sqr_distance, 1)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 1)); - } -BOOST_AUTO_TEST_CASE(projection_test_triangle) -{ +BOOST_AUTO_TEST_CASE(projection_test_triangle) { Vec3f v1(0, 0, 1); Vec3f v2(0, 1, 0); Vec3f v3(1, 0, 0); @@ -53,11 +47,11 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) Vec3f p(1, 1, 1); Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 7); - BOOST_CHECK(approx(res.sqr_distance, 4/3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1/3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1/3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1/3.0)); - + BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + p = Vec3f(0, 0, 1.5); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 1); @@ -107,8 +101,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) BOOST_CHECK(approx(res.parameterization[2], 0)); } -BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) -{ +BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { Vec3f v1(0, 0, 1); Vec3f v2(0, 1, 0); Vec3f v3(1, 0, 0); @@ -126,38 +119,38 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) p = Vec3f(0, 0, 0); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 7); - BOOST_CHECK(approx(res.sqr_distance, 1/3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1/3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1/3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1/3.0)); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3f(0, 1, 1); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 11); - BOOST_CHECK(approx(res.sqr_distance, 1/3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1/3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1/3.0)); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 1/3.0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); p = Vec3f(1, 1, 0); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 14); - BOOST_CHECK(approx(res.sqr_distance, 1/3.0)); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1/3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1/3.0)); - BOOST_CHECK(approx(res.parameterization[3], 1/3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); p = Vec3f(1, 0, 1); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 13); - BOOST_CHECK(approx(res.sqr_distance, 1/3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1/3.0)); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 1/3.0)); - BOOST_CHECK(approx(res.parameterization[3], 1/3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); p = Vec3f(1.5, 1.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); @@ -221,7 +214,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); - + p = Vec3f(-0.5, 0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 3); @@ -239,7 +232,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); - + p = Vec3f(0.5, 0.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 6); @@ -248,5 +241,4 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0)); - } diff --git a/test/utility.cpp b/test/utility.cpp index 8efc1a0bc..8f7f73805 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -12,13 +12,10 @@ #include #include -namespace hpp -{ -namespace fcl -{ +namespace hpp { +namespace fcl { -BenchTimer::BenchTimer() -{ +BenchTimer::BenchTimer() { #ifdef _WIN32 QueryPerformanceFrequency(&frequency); startCount.QuadPart = 0; @@ -33,15 +30,10 @@ BenchTimer::BenchTimer() endTimeInMicroSec = 0; } +BenchTimer::~BenchTimer() {} -BenchTimer::~BenchTimer() -{ -} - - -void BenchTimer::start() -{ - stopped = 0; // reset stop flag +void BenchTimer::start() { + stopped = 0; // reset stop flag #ifdef _WIN32 QueryPerformanceCounter(&startCount); #else @@ -49,10 +41,8 @@ void BenchTimer::start() #endif } - -void BenchTimer::stop() -{ - stopped = 1; // set timer stopped flag +void BenchTimer::stop() { + stopped = 1; // set timer stopped flag #ifdef _WIN32 QueryPerformanceCounter(&endCount); @@ -61,64 +51,52 @@ void BenchTimer::stop() #endif } - -double BenchTimer::getElapsedTimeInMicroSec() -{ +double BenchTimer::getElapsedTimeInMicroSec() { #ifdef _WIN32 - if(!stopped) - QueryPerformanceCounter(&endCount); + if (!stopped) QueryPerformanceCounter(&endCount); startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart); endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart); #else - if(!stopped) - gettimeofday(&endCount, NULL); + if (!stopped) gettimeofday(&endCount, NULL); - startTimeInMicroSec = ((double)startCount.tv_sec * 1000000.0) + (double)startCount.tv_usec; - endTimeInMicroSec = ((double)endCount.tv_sec * 1000000.0) + (double)endCount.tv_usec; + startTimeInMicroSec = + ((double)startCount.tv_sec * 1000000.0) + (double)startCount.tv_usec; + endTimeInMicroSec = + ((double)endCount.tv_sec * 1000000.0) + (double)endCount.tv_usec; #endif return endTimeInMicroSec - startTimeInMicroSec; } - -double BenchTimer::getElapsedTimeInMilliSec() -{ +double BenchTimer::getElapsedTimeInMilliSec() { return this->getElapsedTimeInMicroSec() * 0.001; } - -double BenchTimer::getElapsedTimeInSec() -{ +double BenchTimer::getElapsedTimeInSec() { return this->getElapsedTimeInMicroSec() * 0.000001; } +double BenchTimer::getElapsedTime() { return this->getElapsedTimeInMilliSec(); } -double BenchTimer::getElapsedTime() -{ - return this->getElapsedTimeInMilliSec(); -} - - -const Eigen::IOFormat vfmt = Eigen::IOFormat (Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", ""); -const Eigen::IOFormat pyfmt = Eigen::IOFormat (Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); +const Eigen::IOFormat vfmt = Eigen::IOFormat( + Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", ""); +const Eigen::IOFormat pyfmt = Eigen::IOFormat( + Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); const Vec3f UnitX = Vec3f(1, 0, 0); const Vec3f UnitY = Vec3f(0, 1, 0); const Vec3f UnitZ = Vec3f(0, 0, 1); -FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) -{ +FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) { FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1); return (t * (rmax - rmin) + rmin); } -void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles) -{ - +void loadOBJFile(const char* filename, std::vector& points, + std::vector& triangles) { FILE* file = fopen(filename, "rb"); - if(!file) - { + if (!file) { std::cerr << "file not exist" << std::endl; return; } @@ -126,65 +104,47 @@ void loadOBJFile(const char* filename, std::vector& points, std::vector& points, std::vector& points, std::vector& triangles) -{ +void saveOBJFile(const char* filename, std::vector& points, + std::vector& triangles) { std::ofstream os(filename); - if(!os) - { + if (!os) { std::cerr << "file not exist" << std::endl; return; } - for(std::size_t i = 0; i < points.size(); ++i) - { - os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2] << std::endl; + for (std::size_t i = 0; i < points.size(); ++i) { + os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2] + << std::endl; } - for(std::size_t i = 0; i < triangles.size(); ++i) - { - os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " << triangles[i][2] + 1 << std::endl; + for (std::size_t i = 0; i < triangles.size(); ++i) { + os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " + << triangles[i][2] + 1 << std::endl; } os.close(); } #ifdef HPP_FCL_HAS_OCTOMAP -OcTree loadOctreeFile (const std::string& filename, const FCL_REAL& resolution) - { - octomap::OcTreePtr_t octree (new octomap::OcTree (filename)); - if (octree->getResolution() != resolution) { - std::ostringstream oss; - oss << "Resolution of the OcTree is " << octree->getResolution() << - " and not " << resolution; - throw std::invalid_argument(oss.str()); - } - return hpp::fcl::OcTree (octree); +OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) { + octomap::OcTreePtr_t octree(new octomap::OcTree(filename)); + if (octree->getResolution() != resolution) { + std::ostringstream oss; + oss << "Resolution of the OcTree is " << octree->getResolution() + << " and not " << resolution; + throw std::invalid_argument(oss.str()); } + return hpp::fcl::OcTree(octree); +} #endif -void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) -{ +void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) { FCL_REAL c1 = cos(a); FCL_REAL c2 = cos(b); FCL_REAL c3 = cos(c); @@ -245,13 +201,11 @@ void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) FCL_REAL s2 = sin(b); FCL_REAL s3 = sin(c); - R << c1 * c2, - c2 * s1, s2, - c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3, - s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3; + R << c1 * c2, -c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, + -c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3; } -void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) -{ +void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); @@ -267,12 +221,11 @@ void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) transform.setTransform(R, T); } - -void generateRandomTransforms(FCL_REAL extents[6], std::vector& transforms, std::size_t n) -{ +void generateRandomTransforms(FCL_REAL extents[6], + std::vector& transforms, + std::size_t n) { transforms.resize(n); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); @@ -291,13 +244,14 @@ void generateRandomTransforms(FCL_REAL extents[6], std::vector& tra } } - -void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n) -{ +void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], + FCL_REAL delta_rot, + std::vector& transforms, + std::vector& transforms2, + std::size_t n) { transforms.resize(n); transforms2.resize(n); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); @@ -331,42 +285,44 @@ void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_ } } -bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_) -{ +bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, + void* cdata_) { CollisionData* cdata = static_cast(cdata_); const CollisionRequest& request = cdata->request; CollisionResult& result = cdata->result; - if(cdata->done) return true; + if (cdata->done) return true; collide(o1, o2, request, result); - if((result.isCollision()) && - (result.numContacts() >= request.num_max_contacts)) + if ((result.isCollision()) && + (result.numContacts() >= request.num_max_contacts)) cdata->done = true; return cdata->done; } -bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, FCL_REAL& dist) -{ +bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, + void* cdata_, FCL_REAL& dist) { DistanceData* cdata = static_cast(cdata_); const DistanceRequest& request = cdata->request; DistanceResult& result = cdata->result; - if(cdata->done) { dist = result.min_distance; return true; } + if (cdata->done) { + dist = result.min_distance; + return true; + } distance(o1, o2, request, result); dist = result.min_distance; - if(dist <= 0) return true; // in collision or in touch + if (dist <= 0) return true; // in collision or in touch return cdata->done; } -std::string getNodeTypeName(NODE_TYPE node_type) -{ +std::string getNodeTypeName(NODE_TYPE node_type) { if (node_type == BV_UNKNOWN) return std::string("BV_UNKNOWN"); else if (node_type == BV_AABB) @@ -409,8 +365,7 @@ std::string getNodeTypeName(NODE_TYPE node_type) return std::string("invalid"); } -Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) -{ +Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) { Quaternion3f q; q.w() = w; q.x() = x; @@ -419,84 +374,81 @@ Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) return q; } -std::ostream& operator<< (std::ostream& os, const Transform3f& tf) -{ - return os << "[ " << - tf.getTranslation().format(vfmt) - << ", " - << tf.getQuatRotation().coeffs().format(vfmt) - << " ]" ; +std::ostream& operator<<(std::ostream& os, const Transform3f& tf) { + return os << "[ " << tf.getTranslation().format(vfmt) << ", " + << tf.getQuatRotation().coeffs().format(vfmt) << " ]"; } -std::size_t getNbRun (const int& argc, char const* const* argv, std::size_t defaultValue) -{ +std::size_t getNbRun(const int& argc, char const* const* argv, + std::size_t defaultValue) { for (int i = 0; i < argc; ++i) if (strcmp(argv[i], "--nb-run") == 0) - if (i+1 != argc) - return (std::size_t)strtol(argv[i+1], NULL, 10); + if (i + 1 != argc) return (std::size_t)strtol(argv[i + 1], NULL, 10); return defaultValue; } -void generateEnvironments(std::vector& env, FCL_REAL env_scale, std::size_t n) -{ - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; +void generateEnvironments(std::vector& env, + FCL_REAL env_scale, std::size_t n) { + FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, + env_scale, -env_scale, env_scale}; std::vector transforms(n); generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { Box* box = new Box(5, 10, 20); - env.push_back(new CollisionObject(shared_ptr(box), transforms[i])); + env.push_back( + new CollisionObject(shared_ptr(box), transforms[i])); } generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { Sphere* sphere = new Sphere(30); - env.push_back(new CollisionObject(shared_ptr(sphere), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(sphere), + transforms[i])); } generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { Cylinder* cylinder = new Cylinder(10, 40); - env.push_back(new CollisionObject(shared_ptr(cylinder), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(cylinder), + transforms[i])); } } -void generateEnvironmentsMesh(std::vector& env, FCL_REAL env_scale, std::size_t n) -{ - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; +void generateEnvironmentsMesh(std::vector& env, + FCL_REAL env_scale, std::size_t n) { + FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, + env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Box box(5, 10, 20); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f::Identity()); - env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(model), + transforms[i])); } generateRandomTransforms(extents, transforms, n); Sphere sphere(30); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16); - env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(model), + transforms[i])); } generateRandomTransforms(extents, transforms, n); Cylinder cylinder(10, 40); - for(std::size_t i = 0; i < n; ++i) - { + for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16); - env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(shared_ptr(model), + transforms[i])); } } -} +} // namespace fcl -} // namespace hpp +} // namespace hpp diff --git a/test/utility.h b/test/utility.h index 78f108793..2845bcb48 100644 --- a/test/utility.h +++ b/test/utility.h @@ -48,50 +48,55 @@ #endif #ifdef _WIN32 -#define NOMINMAX // required to avoid compilation errors with Visual Studio 2010 +#define NOMINMAX // required to avoid compilation errors with Visual Studio + // 2010 #include #else #include #endif -#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ - BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \ - "check " #Va ".isApprox(" #Vb ") failed " \ - "[\n" << (Va).transpose() << "\n!=\n" << (Vb).transpose() << "\n]") +#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ + BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \ + "check " #Va ".isApprox(" #Vb \ + ") failed " \ + "[\n" \ + << (Va).transpose() << "\n!=\n" \ + << (Vb).transpose() << "\n]") #define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \ - BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \ - "check " #Va ".isApprox(" #Vb ") failed " \ - "[\n" << (Va) << "\n!=\n" << (Vb) << "\n]") - -namespace octomap{ + BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), "check " #Va \ + ".isApprox(" #Vb \ + ") failed " \ + "[\n" \ + << (Va) << "\n!=\n" \ + << (Vb) << "\n]") + +namespace octomap { #ifdef HPP_FCL_HAS_OCTOMAP typedef hpp::fcl::shared_ptr OcTreePtr_t; #endif -} -namespace hpp -{ -namespace fcl -{ - -class BenchTimer -{ -public: +} // namespace octomap +namespace hpp { +namespace fcl { + +class BenchTimer { + public: BenchTimer(); ~BenchTimer(); - void start(); ///< start timer - void stop(); ///< stop the timer - double getElapsedTime(); ///< get elapsed time in milli-second - double getElapsedTimeInSec(); ///< get elapsed time in second (same as getElapsedTime) - double getElapsedTimeInMilliSec(); ///< get elapsed time in milli-second - double getElapsedTimeInMicroSec(); ///< get elapsed time in micro-second - -private: - double startTimeInMicroSec; ///< starting time in micro-second - double endTimeInMicroSec; ///< ending time in micro-second - int stopped; ///< stop flag + void start(); ///< start timer + void stop(); ///< stop the timer + double getElapsedTime(); ///< get elapsed time in milli-second + double getElapsedTimeInSec(); ///< get elapsed time in second (same as + ///< getElapsedTime) + double getElapsedTimeInMilliSec(); ///< get elapsed time in milli-second + double getElapsedTimeInMicroSec(); ///< get elapsed time in micro-second + + private: + double startTimeInMicroSec; ///< starting time in micro-second + double endTimeInMicroSec; ///< ending time in micro-second + int stopped; ///< stop flag #ifdef _WIN32 - LARGE_INTEGER frequency; ///< ticks per second + LARGE_INTEGER frequency; ///< ticks per second LARGE_INTEGER startCount; LARGE_INTEGER endCount; #else @@ -100,15 +105,13 @@ class BenchTimer #endif }; -struct TStruct -{ +struct TStruct { std::vector records; double overall_time; TStruct() { overall_time = 0; } - void push_back(double t) - { + void push_back(double t) { records.push_back(t); overall_time += t; } @@ -122,53 +125,75 @@ extern const Vec3f UnitY; extern const Vec3f UnitZ; /// @brief Load an obj mesh file -void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles); +void loadOBJFile(const char* filename, std::vector& points, + std::vector& triangles); -void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles); +void saveOBJFile(const char* filename, std::vector& points, + std::vector& triangles); #ifdef HPP_FCL_HAS_OCTOMAP -fcl::OcTree loadOctreeFile (const std::string& filename, const FCL_REAL& resolution); +fcl::OcTree loadOctreeFile(const std::string& filename, + const FCL_REAL& resolution); #endif -/// @brief Generate one random transform whose translation is constrained by extents and rotation without constraints. -/// The translation is (x, y, z), and extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= z <= extents[5] +/// @brief Generate one random transform whose translation is constrained by +/// extents and rotation without constraints. The translation is (x, y, z), and +/// extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= +/// z <= extents[5] void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform); -/// @brief Generate n random transforms whose translations are constrained by extents. -void generateRandomTransforms(FCL_REAL extents[6], std::vector& transforms, std::size_t n); - -/// @brief Generate n random transforms whose translations are constrained by extents. Also generate another transforms2 which have additional random translation & rotation to the transforms generated. -void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n); - -/// @ brief Structure for minimum distance between two meshes and the corresponding nearest point pair -struct DistanceRes -{ +/// @brief Generate n random transforms whose translations are constrained by +/// extents. +void generateRandomTransforms(FCL_REAL extents[6], + std::vector& transforms, + std::size_t n); + +/// @brief Generate n random transforms whose translations are constrained by +/// extents. Also generate another transforms2 which have additional random +/// translation & rotation to the transforms generated. +void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], + FCL_REAL delta_rot, + std::vector& transforms, + std::vector& transforms2, + std::size_t n); + +/// @ brief Structure for minimum distance between two meshes and the +/// corresponding nearest point pair +struct DistanceRes { double distance; Vec3f p1; Vec3f p2; }; -/// @brief Default collision callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. -bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata); +/// @brief Default collision callback for two objects o1 and o2 in broad phase. +/// return value means whether the broad phase can stop now. +bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, + void* cdata); -/// @brief Default distance callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. also return dist, i.e. the bmin distance till now -bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist); +/// @brief Default distance callback for two objects o1 and o2 in broad phase. +/// return value means whether the broad phase can stop now. also return dist, +/// i.e. the bmin distance till now +bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, + void* cdata, FCL_REAL& dist); std::string getNodeTypeName(NODE_TYPE node_type); Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z); -std::ostream& operator<< (std::ostream& os, const Transform3f& tf); +std::ostream& operator<<(std::ostream& os, const Transform3f& tf); /// Get the argument --nb-run from argv -std::size_t getNbRun (const int& argc, char const* const* argv, std::size_t defaultValue); +std::size_t getNbRun(const int& argc, char const* const* argv, + std::size_t defaultValue); -void generateEnvironments(std::vector& env, FCL_REAL env_scale, std::size_t n); +void generateEnvironments(std::vector& env, + FCL_REAL env_scale, std::size_t n); -void generateEnvironmentsMesh(std::vector& env, FCL_REAL env_scale, std::size_t n); +void generateEnvironmentsMesh(std::vector& env, + FCL_REAL env_scale, std::size_t n); -} +} // namespace fcl -} // namespace hpp +} // namespace hpp #endif