doc/generate_distance_plot.py

+import matplotlib.pyplot as plt
+import numpy as np
+
+interactive = False
+
+m = 1.0
+b = 1.2
+
+mb = m + b
+
+X = np.array([-mb / 2, 0, m, mb, 2 * mb])
+# X = np.linspace(-1, 4., 21)
+
+
+def dlb(d):
+    if d < 0:
+        return None
+    if d > mb:
+        u = d - mb
+        return mb - m + u / 2
+    return d - m
+
+
+plt.figure(figsize=(9, 3.5))
+# plt.plot(X, X-m, ":k")
+# plt.plot([m+b, X[-1]], [b, b], ":k")
+plt.fill_between(
+    [m + b, X[-1]],
+    [b, b],
+    [b, X[-1] - m],
+    alpha=0.2,
+    hatch="|",
+    facecolor="g",
+    label="Distance lower band area",
+)
+plt.plot(X, [dlb(x) for x in X], "-g", label="distance lower bound")
+# plt.plot([X[0], m, m, X[-1]], [0, 0, b, b], ":k")
+plt.axvspan(X[0], m, alpha=0.5, hatch="\\", facecolor="r", label="Collision area")
+
+
+ax = plt.gca()
+ax.set_xlabel("Object distance")
+ax.set_xticks([0, m, mb])
+ax.set_xticklabels(["0", "security margin", "security margin\n+ break distance"])
+ax.set_yticks([0, b])
+ax.set_yticklabels(["0", "break distance"])
+ax.grid(which="major", ls="solid")
+ax.grid(which="minor", ls="dashed")
+
+plt.axvline(0, ls="solid")
+# plt.axvline(m, ls="dashed", label="margin")
+# plt.axvline(mb, ls="dashed")
+plt.axhline(0.0, ls="solid")
+
+plt.title("Collision and distance lower band")
+plt.legend(loc="lower right")
+if interactive:
+    plt.show()
+else:
+    import os.path as path
+
+    dir_path = path.dirname(path.realpath(__file__))
+    plt.savefig(
+        path.join(dir_path, "distance_computation.png"),
+        bbox_inches="tight",
+        orientation="landscape",
+    )

doc/gjk.py

+#!/usr/bin/env python3
+import pdb
+import sys
+
+# ABC = AB^AC
+# (ABC^AJ).a = (j.c - j.b) a.a + (j.a - j.c) b.a + (j.b - j.a) c.a, for j = b or c
+
+segment_fmt = "{j}a_aa"
+plane_fmt = ""
+edge_fmt = "{j}a * {b}a_{c}a + {j}{b} * {c}a_aa - {j}{c} * {b}a_aa"
+
+# These checks must be negative and not positive, as in the cheat sheet.
+# They are the same as in the cheat sheet, except that we consider (...).dot(A)
+# instead of (...).dot(-A)
+plane_tests = ["C.dot (a_cross_b)", "D.dot(a_cross_c)", "-D.dot(a_cross_b)"]
+checks = (
+    plane_tests
+    + [edge_fmt.format(**{"j": j, "b": "b", "c": "c"}) for j in ["b", "c"]]
+    + [edge_fmt.format(**{"j": j, "b": "c", "c": "d"}) for j in ["c", "d"]]
+    + [edge_fmt.format(**{"j": j, "b": "d", "c": "b"}) for j in ["d", "b"]]
+    + [segment_fmt.format(**{"j": j}) for j in ["b", "c", "d"]]
+)
+checks_hr = (
+    ["ABC.AO >= 0", "ACD.AO >= 0", "ADB.AO >= 0"]
+    + ["(ABC ^ {}).AO >= 0".format(n) for n in ["AB", "AC"]]
+    + ["(ACD ^ {}).AO >= 0".format(n) for n in ["AC", "AD"]]
+    + ["(ADB ^ {}).AO >= 0".format(n) for n in ["AD", "AB"]]
+    + ["AB.AO >= 0", "AC.AO >= 0", "AD.AO >= 0"]
+)
+
+# weights of the checks.
+weights = (
+    [
+        2,
+    ]
+    * 3
+    + [
+        3,
+    ]
+    * 6
+    + [
+        1,
+    ]
+    * 3
+)
+
+# Segment tests first, because they have lower weight.
+# tests = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, ]
+tests = [
+    9,
+    10,
+    11,
+    0,
+    1,
+    2,
+    3,
+    4,
+    5,
+    6,
+    7,
+    8,
+]
+assert len(tests) == len(checks)
+assert sorted(tests) == list(range(len(tests)))
+
+regions = [
+    "ABC",
+    "ACD",
+    "ADB",
+    "AB",
+    "AC",
+    "AD",
+    "A",
+    "Inside",
+]
+cases = list(range(len(regions)))
+
+# The following 3 lists refer to table doc/GJK_tetrahedra_boolean_table.ods
+
+# A check ID is (+/- (index+1)) where a minus sign encodes a NOT operation
+# and index refers to an index in list checks.
+
+# definitions is a list of list of check IDs to be ANDed.
+# For instance, a0.a3.!a4 -> [ 1, 4, -5]
+definitions = [
+    [1, 4, -5],
+    [2, 6, -7],
+    [3, 8, -9],
+    [-4, 9, 10],
+    [-6, 5, 11],
+    [-8, 7, 12],
+    [-10, -11, -12],
+    [-1, -2, -3],
+]
+# conditions is a list of (list of (list of check IDs to be ANDed) to be ORed).
+conditions = [
+    [],
+    [],
+    [],
+    [],
+    [],
+    [],
+    [],
+    [],  # [ [10, 11, 12], ], # I don't think this is always true...
+]
+# rejections is a list of (list of (list of check IDs to be ANDed) to be ORed).
+rejections = [
+    [
+        [2, 6, 7],
+        [3, -8, -9],
+    ],
+    [
+        [3, 8, 9],
+        [1, -4, -5],
+    ],
+    [
+        [1, 4, 5],
+        [2, -6, -7],
+    ],
+    [
+        [-1, -3],
+    ],
+    [
+        [-2, -1],
+    ],
+    [
+        [-3, -2],
+    ],
+    [
+        [4, -5],
+        [6, -7],
+        [8, -9],
+    ],
+    [],
+]
+
+implications = [
+    [
+        [
+            4,
+            5,
+            10,
+        ],
+        [11],
+    ],
+    [
+        [
+            6,
+            7,
+            11,
+        ],
+        [12],
+    ],
+    [
+        [
+            8,
+            9,
+            12,
+        ],
+        [10],
+    ],
+    [
+        [
+            -4,
+            -5,
+            11,
+        ],
+        [10],
+    ],
+    [
+        [
+            -6,
+            -7,
+            12,
+        ],
+        [11],
+    ],
+    [
+        [
+            -8,
+            -9,
+            10,
+        ],
+        [12],
+    ],
+    [[1, 4, 5, 6], [-7]],
+    [[2, 6, 9, 8], [-9]],
+    [[3, 8, 9, 4], [-5]],
+    [
+        [
+            -4,
+            5,
+            10,
+        ],
+        [-11],
+    ],
+    [
+        [
+            4,
+            -5,
+            -10,
+        ],
+        [11],
+    ],
+    [
+        [
+            -6,
+            7,
+            11,
+        ],
+        [-12],
+    ],
+    [
+        [
+            6,
+            -7,
+            -11,
+        ],
+        [12],
+    ],
+    [
+        [
+            -8,
+            9,
+            12,
+        ],
+        [-10],
+    ],
+    [
+        [
+            8,
+            -9,
+            -12,
+        ],
+        [10],
+    ],
+    [[10, 3, 9, -12, 4, -5], [1]],
+    [[10, -3, 1, -4], [9]],
+    [[10, -3, -1, 2, -6, 11], [5]],
+    [[-10, 11, 2, -12, -5, -1], [6]],
+    [[-10, 11, -2, 1, 5], [-6]],
+    [[-10, -11, 12, 1, -7, -2, 4], [-5]],
+    [[-10, -11, 12, -3, 2, 7], [-8]],
+    [[-10, -11, 12, -3, -2], [-1]],
+]
+
+
+def set_test_values(current_tests, test_values, itest, value):
+    def satisfies(values, indices):
+        for k in indices:
+            if k > 0 and values[k - 1] is not True:
+                return False
+            if k < 0 and values[-k - 1] is not False:
+                return False
+        return True
+
+    remaining_tests = list(current_tests)
+    next_test_values = list(test_values)
+
+    remaining_tests.remove(itest)
+    next_test_values[itest] = value
+    rerun = True
+    while rerun:
+        rerun = False
+        for impl in implications:
+            if satisfies(next_test_values, impl[0]):
+                for id in impl[1]:
+                    k = (id - 1) if id > 0 else (-id - 1)
+                    if k in remaining_tests:
+                        next_test_values[k] = id > 0
+                        remaining_tests.remove(k)
+                        rerun = True
+                    else:
+                        if next_test_values[k] != (id > 0):
+                            raise ValueError("Absurd case")
+    return remaining_tests, next_test_values
+
+
+def set_tests_values(current_tests, test_values, itests, values):
+    for itest, value in zip(itests, values):
+        current_tests, test_values = set_test_values(
+            current_tests, test_values, itest, value
+        )
+    return current_tests, test_values
+
+
+def apply_test_values(cases, test_values):
+    def canSatisfy(values, indices):
+        for k in indices:
+            if k > 0 and values[k - 1] is False:
+                return False
+            if k < 0 and values[-k - 1] is True:
+                return False
+        return True
+
+    def satisfies(values, indices):
+        for k in indices:
+            if k > 0 and values[k - 1] is not True:
+                return False
+            if k < 0 and values[-k - 1] is not False:
+                return False
+        return True
+
+    # Check all cases.
+    left_cases = []
+    for case in cases:
+        defi = definitions[case]
+        conds = conditions[case]
+        rejs = rejections[case]
+        if satisfies(test_values, defi):
+            # A definition is True, stop recursion
+            return [case]
+        if not canSatisfy(test_values, defi):
+            continue
+        for cond in conds:
+            if satisfies(test_values, cond):
+                # A condition is True, stop recursion
+                return [case]
+        append = True
+        for rej in rejs:
+            if satisfies(test_values, rej):
+                # A rejection is True, discard this case
+                append = False
+                break
+        if append:
+            left_cases.append(case)
+    return left_cases
+
+
+def max_number_of_tests(
+    current_tests,
+    cases,
+    test_values=[
+        None,
+    ]
+    * len(tests),
+    prevBestScore=float("inf"),
+    prevScore=0,
+):
+    for test in current_tests:
+        assert test_values[test] is None, "Test " + str(test) + " already performed"
+
+    left_cases = apply_test_values(cases, test_values)
+
+    if len(left_cases) == 1:
+        return prevScore, {
+            "case": left_cases[0],
+        }
+    elif len(left_cases) == 0:
+        return prevScore, {
+            "case": None,
+            "comments": [
+                "applied " + str(test_values),
+                "to " + ", ".join([regions[c] for c in cases]),
+            ],
+        }
+
+    assert len(current_tests) > 0, "No more test but " + str(left_cases) + " remains"
+
+    currentBestScore = prevBestScore
+    bestScore = float("inf")
+    bestOrder = [None, None]
+    for i, test in enumerate(current_tests):
+        assert bestScore >= currentBestScore
+
+        currentScore = prevScore + len(left_cases) * weights[test]
+        # currentScore = prevScore + weights[test]
+        if currentScore > currentBestScore:  # Cannot do better -> stop
+            continue
+
+        try:
+            remaining_tests, next_test_values = set_test_values(
+                current_tests, test_values, test, True
+            )
+        except ValueError:
+            remaining_tests = None
+
+        if remaining_tests is not None:
+            # Do not put this in try catch as I do not want other ValueError to be
+            # understood as an infeasible branch.
+            score_if_t, order_if_t = max_number_of_tests(
+                remaining_tests,
+                left_cases,
+                next_test_values,
+                currentBestScore,
+                currentScore,
+            )
+            if score_if_t >= currentBestScore:  # True didn't do better -> stop
+                continue
+        else:
+            score_if_t, order_if_t = prevScore, None
+
+        try:
+            remaining_tests, next_test_values = set_test_values(
+                current_tests, test_values, test, False
+            )
+        except ValueError:
+            remaining_tests = None
+
+        if remaining_tests is not None:
+            # Do not put this in try catch as I do not want other ValueError to be
+            # understood as an infeasible branch.
+            score_if_f, order_if_f = max_number_of_tests(
+                remaining_tests,
+                left_cases,
+                next_test_values,
+                currentBestScore,
+                currentScore,
+            )
+        else:
+            score_if_f, order_if_f = prevScore, None
+
+        currentScore = max(score_if_t, score_if_f)
+        if currentScore < bestScore:
+            if currentScore < currentBestScore:
+                bestScore = currentScore
+                bestOrder = {"test": test, "true": order_if_t, "false": order_if_f}
+                # pdb.set_trace()
+                currentBestScore = currentScore
+                if len(tests) == len(current_tests):
+                    print("New best score: {}".format(currentBestScore))
+
+    return bestScore, bestOrder
+
+
+def printComments(order, indent, file):
+    if "comments" in order:
+        for comment in order["comments"]:
+            print(indent + "// " + comment, file=file)
+
+
+def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]):
+    if start:
+        print(
+            "bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)",
+            file=file,
+        )
+        print("{", file=file)
+        print(
+            indent + "// The code of this function was generated using doc/gjk.py",
+            file=file,
+        )
+        print(indent + "const vertex_id_t a = 3, b = 2, c = 1, d = 0;", file=file)
+        for v in "abcd":
+            print(
+                indent
+                + "const Vec3s& {} (current.vertex[{}]->w);".format(v.upper(), v),
+                file=file,
+            )
+        print(indent + "const CoalScalar aa = A.squaredNorm();".format(), file=file)
+        for v in "dcb":
+            for m in "abcd":
+                if m <= v:
+                    print(
+                        indent
+                        + "const CoalScalar {0}{1}    = {2}.dot({3});".format(
+                            v, m, v.upper(), m.upper()
+                        ),
+                        file=file,
+                    )
+                else:
+                    print(
+                        indent + "const CoalScalar& {0}{1}    = {1}{0};".format(v, m),
+                        file=file,
+                    )
+            print(indent + "const CoalScalar {0}a_aa = {0}a - aa;".format(v), file=file)
+        for l0, l1 in zip("bcd", "cdb"):
+            print(
+                indent + "const CoalScalar {0}a_{1}a = {0}a - {1}a;".format(l0, l1),
+                file=file,
+            )
+        for v in "bc":
+            print(
+                indent + "const Vec3s a_cross_{0} = A.cross({1});".format(v, v.upper()),
+                file=file,
+            )
+        print("", file=file)
+        print("#define REGION_INSIDE()               " + indent + "\\", file=file)
+        print(indent + "  ray.setZero();                      \\", file=file)
+        print(indent + "  next.vertex[0] = current.vertex[d]; \\", file=file)
+        print(indent + "  next.vertex[1] = current.vertex[c]; \\", file=file)
+        print(indent + "  next.vertex[2] = current.vertex[b]; \\", file=file)
+        print(indent + "  next.vertex[3] = current.vertex[a]; \\", file=file)
+        print(indent + "  next.rank=4;                        \\", file=file)
+        print(indent + "  return true;", file=file)
+        print("", file=file)
+
+    if "case" in order:
+        case = order["case"]
+        if case is None:
+            print(
+                indent + "// There are no case corresponding to this set of tests.",
+                file=file,
+            )
+            printComments(order, indent, file)
+            print(indent + "assert(false);", file=file)
+            return
+        region = regions[case]
+        print(indent + "// Region " + region, file=file)
+        printComments(order, indent, file)
+        toFree = ["b", "c", "d"]
+        if region == "Inside":
+            print(indent + "REGION_INSIDE()", file=file)
+            toFree = []
+        elif region == "A":
+            print(indent + "originToPoint (current, a, A, next, ray);", file=file)
+        elif len(region) == 2:
+            region[0]
+            B = region[1]
+            print(
+                indent
+                + "originToSegment "
+                "(current, a, {b}, A, {B}, {B}-A, -{b}a_aa, next, ray);".format(
+                    **{"b": B.lower(), "B": B}
+                ),
+                file=file,
+            )
+            toFree.remove(B.lower())
+        elif len(region) == 3:
+            B = region[1]
+            C = region[2]
+            test = plane_tests[["ABC", "ACD", "ADB"].index(region)]
+            if test.startswith("-"):
+                test = test[1:]
+            else:
+                test = "-" + test
+            print(
+                indent
+                + "originToTriangle "
+                "(current, a, {b}, {c}, ({B}-A).cross({C}-A), {t}, next, ray);".format(
+                    **{"b": B.lower(), "c": C.lower(), "B": B, "C": C, "t": test}
+                ),
+                file=file,
+            )
+            toFree.remove(B.lower())
+            toFree.remove(C.lower())
+        else:
+            assert False, "Unknown region " + region
+        for pt in toFree:
+            print(
+                indent + "free_v[nfree++] = current.vertex[{}];".format(pt), file=file
+            )
+    else:
+        assert "test" in order and "true" in order and "false" in order
+        check = checks[order["test"]]
+        check_hr = checks_hr[order["test"]]
+        printComments(order, indent, file)
+        nextTests_t = curTests + [
+            "a" + str(order["test"] + 1),
+        ]
+        nextTests_f = curTests + [
+            "!a" + str(order["test"] + 1),
+        ]
+        if order["true"] is None:
+            if order["false"] is None:
+                print(
+                    indent
+                    + """assert(false && "Case {} should never happen.");""".format(
+                        check_hr
+                    )
+                )
+            else:
+                print(
+                    indent
+                    + "assert(!({} <= 0)); // Not {} / {}".format(
+                        check, check_hr, ".".join(nextTests_f)
+                    ),
+                    file=file,
+                )
+                printOrder(
+                    order["false"],
+                    indent=indent,
+                    start=False,
+                    file=file,
+                    curTests=nextTests_f,
+                )
+        elif order["false"] is None:
+            print(
+                indent
+                + "assert({} <= 0); // {} / {}".format(
+                    check, check_hr, ".".join(nextTests_t)
+                ),
+                file=file,
+            )
+            printOrder(
+                order["true"],
+                indent=indent,
+                start=False,
+                file=file,
+                curTests=nextTests_t,
+            )
+        else:
+            print(
+                indent
+                + "if ({} <= 0) {{ // if {} / {}".format(
+                    check, check_hr, ".".join(nextTests_t)
+                ),
+                file=file,
+            )
+            printOrder(
+                order["true"],
+                indent=indent + "  ",
+                start=False,
+                file=file,
+                curTests=nextTests_t,
+            )
+            print(
+                indent
+                + "}} else {{ // not {} / {}".format(check_hr, ".".join(nextTests_f)),
+                file=file,
+            )
+            printOrder(
+                order["false"],
+                indent=indent + "  ",
+                start=False,
+                file=file,
+                curTests=nextTests_f,
+            )
+            print(indent + "}} // end of {}".format(check_hr), file=file)
+
+    if start:
+        print("", file=file)
+        print("#undef REGION_INSIDE", file=file)
+        print(indent + "return false;", file=file)
+        print("}", file=file)
+
+
+def unit_tests():
+    # a4, a5, a10, a11, a12
+    cases = list(range(len(regions)))
+    pdb.set_trace()
+    left_cases = apply_test_values(
+        cases,
+        test_values=[
+            None,
+            None,
+            None,
+            True,
+            True,
+            None,
+            None,
+            None,
+            None,
+            True,
+            True,
+            True,
+        ],
+    )
+    assert len(left_cases) > 1
+
+
+# unit_tests()
+
+score, order = max_number_of_tests(tests, cases)
+
+print(score)
+printOrder(order, indent="  ")
+
+# TODO add weights such that:
+# - it is preferred to have all the use of one check in one branch.
+#   idea: ponderate by the number of remaining tests.

doc/mesh-mesh-collision-call.dot

+digraph CD  {
+
+	rankdir = BT
+	compound=true
+        size = 11.7
+
+        "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
+        "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
+        "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
+        "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" [shape = box]
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" [shape = box]
+        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" [shape = box]
+        "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [shape = box]
+        "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box]
+
+         "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"
+        "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
+        "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"-> "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)"
+        "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)"
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+        "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)"
+        "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result"
+"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
+      "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [color=red]
+      "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" [color = red]
+      "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
+      "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
+      "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n  -request\n  - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const"
+}
\ No newline at end of file

doc/notes

+In include/hpp/fcl/traversal:
+
+  - some initialize method compute the position of all triangles in
+    global frame before performing collision test,
+
+    - bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshCollisionTraversalNode<BV>& node,...
+
+  - other do not.
+
+    - bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node, ...
+    - bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node, ...
+    - bool initialize(MeshCollisionTraversalNodeOBB& node, ...
+    - bool initialize(MeshCollisionTraversalNodeRSS& node, ...
+    - bool initialize(MeshCollisionTraversalNodekIOS& node, ...
+    - bool initialize(MeshCollisionTraversalNodeOBBRSS& node, ...
+
+-------------------------------------------------------------------------------
+
+classes
+  - MeshCollisionTraversalNodeRSS,
+  - MeshCollisionTraversalNodekIOS,
+  - MeshCollisionTraversalNodeOBBRSS
+derive from
+  - MeshCollisionTraversalNode <RSS>,
+  - MeshCollisionTraversalNode <kIOS>,
+  - MeshCollisionTraversalNode <OBBRSS>.
+They store the relative position and orientation between two objects to test for
+collision. before calling overlap function, this additional information computes
+the relative positions of the bounding volumes.
+
+-------------------------------------------------------------------------------
+
+For primitive shapes, collision and distance computation are very close.
+ShapeShapeCollide calls indeed ShapeShapeDistance. It would be convenient to
+merge CollisionRequest and DistanceRequest on the one hand and CollisionResult
+and DistanceResult on the other hand into two classes InteractionRequest and
+InteractionResult.
\ No newline at end of file

doc/python/doxygen-boost.hh

+#ifndef DOXYGEN_BOOST_DOC_HH
+#define DOXYGEN_BOOST_DOC_HH
+
+#ifndef DOXYGEN_DOC_HH
+#error "You should have included doxygen.hh first."
+#endif  // DOXYGEN_DOC_HH
+
+#include <boost/python.hpp>
+
+namespace doxygen {
+
+namespace visitor {
+
+template <typename function_type,
+          typename policy_type = boost::python::default_call_policies>
+struct member_func_impl : boost::python::def_visitor<
+                              member_func_impl<function_type, policy_type> > {
+  member_func_impl(const char* n, const function_type& f)
+      : name(n), function(f) {}
+
+  member_func_impl(const char* n, const function_type& f, policy_type p)
+      : name(n), function(f), policy(p) {}
+
+  template <class classT>
+  inline void visit(classT& c) const {
+    // Either a boost::python::keyword<N> object or a void_ object
+    call(c, member_func_args(function));
+  }
+
+  template <class classT, std::size_t nkeywords>
+  inline void call(
+      classT& c, const boost::python::detail::keywords<nkeywords>& args) const {
+    c.def(name, function, member_func_doc(function), args, policy);
+  }
+
+  template <class classT>
+  inline void call(classT& c, const void_&) const {
+    c.def(name, function, member_func_doc(function), policy);
+  }
+
+  const char* name;
+  const function_type& function;
+  policy_type policy;
+};
+
+// TODO surprisingly, this does not work when defined here but it works when
+// defined after the generated files are included.
+template <typename function_type>
+inline member_func_impl<function_type> member_func(
+    const char* name, const function_type& function) {
+  return member_func_impl<function_type>(name, function);
+}
+
+template <typename function_type, typename policy_type>
+inline member_func_impl<function_type, policy_type> member_func(
+    const char* name, const function_type& function,
+    const policy_type& policy) {
+  return member_func_impl<function_type, policy_type>(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<                                           \
+            init_##nargs##_impl<Class BOOST_PP_COMMA_IF(nargs)                \
+                                    BOOST_PP_ENUM_PARAMS(nargs, Arg)> > {     \
+    typedef constructor_##nargs##_impl<Class BOOST_PP_COMMA_IF(nargs)         \
+                                           BOOST_PP_ENUM_PARAMS(nargs, Arg)>  \
+        constructor;                                                          \
+    typedef boost::python::init<BOOST_PP_ENUM_PARAMS(nargs, Arg)> init_base;  \
+                                                                              \
+    template <class classT>                                                   \
+    inline void visit(classT& c) const {                                      \
+      call(c, constructor::args());                                           \
+    }                                                                         \
+                                                                              \
+    template <class classT>                                                   \
+    void call(classT& c,                                                      \
+              const boost::python::detail::keywords<nargs + 1>& args) const { \
+      c.def(init_base(constructor::doc(), args));                             \
+    }                                                                         \
+                                                                              \
+    template <class classT>                                                   \
+    void call(classT& c, const void_&) const {                                \
+      c.def(init_base(constructor::doc()));                                   \
+    }                                                                         \
+  };                                                                          \
+                                                                              \
+  template <typename Class BOOST_PP_COMMA_IF(nargs)                           \
+                BOOST_PP_ENUM_PARAMS(nargs, class Arg)>                       \
+  inline init_##nargs##_impl<Class BOOST_PP_COMMA_IF(nargs)                   \
+                                 BOOST_PP_ENUM_PARAMS(nargs, Arg)>            \
+  init() {                                                                    \
+    return init_##nargs##_impl<Class BOOST_PP_COMMA_IF(nargs)                 \
+                                   BOOST_PP_ENUM_PARAMS(nargs, Arg)>();       \
+  }
+
+BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR,
+                DOXYGEN_DOC_DECLARE_INIT_VISITOR, ~)
+#undef DOXYGEN_DOC_DECLARE_INIT_VISITOR
+
+}  // namespace visitor
+
+template <typename Func>
+void def(const char* name, Func func) {
+  boost::python::def(name, func, member_func_doc(func));
+}
+
+}  // namespace doxygen
+
+#endif  // DOXYGEN_BOOST_DOC_HH

doc/python/doxygen.hh

+#ifndef DOXYGEN_DOC_HH
+#define DOXYGEN_DOC_HH
+
+#include <boost/preprocessor/repetition.hpp>
+#include <boost/preprocessor/punctuation/comma_if.hpp>
+
+#include <boost/mpl/void.hpp>
+
+#ifndef DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR
+#define DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR 10
+#endif
+
+namespace doxygen {
+typedef boost::mpl::void_ void_;
+
+template <typename _class>
+struct class_doc_impl {
+  static inline const char* run() { return ""; }
+  static inline const char* attribute(const char*) { return ""; }
+};
+
+template <typename _class>
+inline const char* class_doc() {
+  return class_doc_impl<_class>::run();
+}
+
+template <typename _class>
+inline const char* class_attrib_doc(const char* name) {
+  return class_doc_impl<_class>::attribute(name);
+}
+
+template <typename FuncPtr>
+inline const char* member_func_doc(FuncPtr) {
+  return "";
+}
+
+template <typename FuncPtr>
+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, ~)
+#undef DOXYGEN_DOC_DECLARE_CONSTRUCTOR
+/*
+template <typename Class>
+inline const char* constructor_doc ()
+{
+  return "";
+}
+*/
+
+template <typename Class>
+struct destructor_doc_impl {
+  static inline const char* run() { return ""; }
+};
+
+template <typename Class>
+inline const char* destructor_doc() {
+  return destructor_doc_impl<Class>::run();
+}
+
+/* TODO class attribute can be handled by
+
+template <typename Class, typename AttributeType>
+const char* attribute_doc (AttributeType Class::* ptr)
+{
+  // Body looks like
+  // if (ptr == &Class::attributeName)
+  //   return "attrib documentation";
+  return "undocumented";
+}
+*/
+
+}  // namespace doxygen
+
+#endif  // DOXYGEN_DOC_HH

doc/python/doxygen_xml_parser.py

+#!/usr/bin/python3
+# ruff: noqa: E501
+
+from __future__ import print_function
+from lxml import etree
+from os import path
+from xml_docstring import XmlDocString
+import sys
+
+template_file_header = """#ifndef DOXYGEN_AUTODOC_{header_guard}
+#define DOXYGEN_AUTODOC_{header_guard}
+
+#include "{path}/doxygen.hh"
+"""
+template_file_footer = """
+#endif // DOXYGEN_AUTODOC_{header_guard}
+"""
+
+template_class_doc = """
+template <{tplargs}>
+struct class_doc_impl< {classname} >
+{{
+static inline const char* run ()
+{{
+  return "{docstring}";
+}}
+static inline const char* attribute (const char* attrib)
+{{{attributes}
+  (void)attrib; // turn off unused parameter warning.
+  return "";
+}}
+}};"""
+template_class_attribute_body = """
+  if (strcmp(attrib, "{attribute}") == 0)
+    return "{docstring}";"""
+template_constructor_doc = """
+template <{tplargs}>
+struct constructor_{nargs}_impl< {classname_prefix}{comma}{argsstring} >
+{{
+static inline const char* doc ()
+{{
+  return "{docstring}";
+}}
+static inline boost::python::detail::keywords<{nargs}+1> args ()
+{{
+  return ({argnamesstring});
+}}
+}};"""
+template_destructor_doc = """
+template <{tplargs}>
+struct destructor_doc_impl < {classname_prefix} >
+{{
+static inline const char* run ()
+{{
+  return "{docstring}";
+}}
+}};"""
+template_member_func_doc = """
+{template}inline const char* member_func_doc ({rettype} ({classname_prefix}*function_ptr) {argsstring})
+{{{body}
+  return "";
+}}"""
+template_member_func_doc_body = """
+  if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername}))
+    return "{docstring}";"""
+template_member_func_args = """
+{template}inline boost::python::detail::keywords<{n}> member_func_args ({rettype} ({classname_prefix}*function_ptr) {argsstring})
+{{{body}
+  return ({default_args});
+}}"""
+template_member_func_args_body = """
+  if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername}))
+    return ({args});"""
+template_static_func_doc = """
+{template}inline const char* member_func_doc ({rettype} (*function_ptr) {argsstring})
+{{{body}
+  return "";
+}}"""
+template_static_func_doc_body = """
+  if (function_ptr == static_cast<{rettype} (*) {argsstring}>(&{namespace}::{membername}))
+    return "{docstring}";"""
+template_open_namespace = """namespace {namespace} {{"""
+template_close_namespace = """}} // namespace {namespace}"""
+template_include_intern = """#include <doxygen_autodoc/{filename}>
+"""
+template_include_extern = """#include <{filename}>
+"""
+
+
+def _templateParamToDict(param):
+    type_ = param.find("type")
+    declname = param.find("declname")
+    defname = param.find("defname")
+    # FIXME type may contain references in two ways:
+    # - the real param type
+    # - the name of the template argument is recognized as the name of a type...
+    if defname is None and declname is None:
+        typetext = type_.text
+        if typetext is None:
+            typetext = ""
+        for c in type_.iter():
+            if c == type_:
+                continue
+            if c.text is not None:
+                typetext += c.text
+            if c.tail is not None:
+                typetext += c.tail
+        if typetext.startswith("typename") or typetext.startswith("class"):
+            if sys.version_info.major == 2:
+                s = typetext.split()
+                return {"type": s[0].strip(), "name": typetext[len(s[0]) :].strip()}
+            else:
+                s = typetext.split(maxsplit=1)
+                assert len(s) == 2
+                return {"type": s[0].strip(), "name": s[1].strip()}
+        else:
+            return {"type": type_.text, "name": ""}
+    else:
+        if type_.text is None:
+            # type_ is not a typename but an existing type
+            type_ = type_.find("ref")
+        assert defname.text == declname.text
+        return {"type": type_.text, "name": defname.text}
+
+
+def makeHeaderGuard(filename):
+    return filename.upper().replace(".", "_").replace("/", "_").replace("-", "_")
+
+
+def format_description(brief, detailed):
+    b = [el.text.strip() for el in brief.iter() if el.text] if brief is not None else []
+    d = (
+        [el.text.strip() for el in detailed.iter() if el.text]
+        if detailed is not None
+        else []
+    )
+    text = "".join(b)
+    if d:
+        text += "\n" + "".join(d)
+    return text
+
+
+class Reference(object):
+    def __init__(self, index, id=None, name=None):
+        self.id = id
+        self.name = name
+        self.index = index
+
+    def xmlToType(self, node, array=None, parentClass=None, tplargs=None):
+        """
+        - node:
+        - parentClass: a class
+        - tplargs: if one of the args is parentClass and no template arguments are provided,
+                   set the template arguments to this value
+        - array: content of the sibling tag 'array'
+        """
+        if node.text is not None:
+            t = node.text.strip()
+        else:
+            t = ""
+        for c in node.iterchildren():
+            if c.tag == "ref":
+                refid = c.attrib["refid"]
+                if parentClass is not None and refid == parentClass.id:
+                    t += " " + parentClass.name
+                    if c.tail is not None and c.tail.lstrip()[0] != "<":
+                        if tplargs is not None:
+                            t += tplargs
+                elif (
+                    parentClass is not None
+                    and isinstance(parentClass, ClassCompound)
+                    and parentClass.hasTypeDef(c.text.strip())
+                ):
+                    parent_has_templates = len(parentClass.template_params) > 0
+                    if parent_has_templates:
+                        t += " typename " + parentClass._className() + "::"
+                    else:
+                        t += " " + parentClass._className() + "::"
+                    self_has_templates = (
+                        c.tail is not None and c.tail.strip().find("<") != -1
+                    )
+                    if self_has_templates:
+                        t += " template "
+                    t += c.text.strip()
+                elif self.index.hasref(refid):
+                    t += " " + self.index.getref(refid).name
+                else:
+                    self.index.output.warn("Unknown reference: ", c.text, refid)
+                    t += " " + c.text.strip()
+            else:
+                if c.text is not None:
+                    t += " " + c.text.strip()
+            if c.tail is not None:
+                t += " " + c.tail.strip()
+        if array is not None:
+            t += array.text
+        return t
+
+
+# Only for function as of now.
+class MemberDef(Reference):
+    def __init__(self, index, memberdefxml, parent):
+        super(MemberDef, self).__init__(
+            index=index,
+            id=memberdefxml.attrib["id"],
+            name=memberdefxml.find("definition").text,
+        )
+        self.parent = parent
+
+        self.xml = memberdefxml
+        self.const = memberdefxml.attrib["const"] == "yes"
+        self.static = memberdefxml.attrib["static"] == "yes"
+        self.rettype = memberdefxml.find("type")
+        self.params = tuple(
+            [
+                (param.find("type"), param.find("declname"), param.find("array"))
+                for param in self.xml.findall("param")
+            ]
+        )
+        self.special = (
+            self.rettype.text is None and len(self.rettype.getchildren()) == 0
+        )
+        # assert self.special or len(self.rettype.text) > 0
+
+        self._templateParams(self.xml.find("templateparamlist"))
+
+    def _templateParams(self, tpl):
+        if tpl is not None:
+            self.template_params = tuple(
+                [_templateParamToDict(param) for param in tpl.iterchildren(tag="param")]
+            )
+        else:
+            self.template_params = tuple()
+
+    def prototypekey(self):
+        prototype = (
+            self.xmlToType(self.rettype, parentClass=self.parent),
+            tuple([tuple(t.items()) for t in self.template_params]),
+            tuple(
+                [
+                    self.xmlToType(param.find("type"), parentClass=self.parent)
+                    for param in self.xml.findall("param")
+                ]
+            ),
+            self.const,
+        )
+        return prototype
+
+    def s_prototypeArgs(self):
+        return "({0}){1}".format(self.s_args(), " const" if self.const else "")
+
+    def s_args(self):
+        # If the class is templated, check if one of the argument is the class itself.
+        # If so, we must add the template arguments to the class (if there is none)
+
+        if len(self.parent.template_params) > 0:
+            tplargs = (
+                " <"
+                + ", ".join([d["name"] for d in self.parent.template_params])
+                + " > "
+            )
+            args = ", ".join(
+                [
+                    self.xmlToType(
+                        type, array, parentClass=self.parent, tplargs=tplargs
+                    )
+                    for type, declname, array in self.params
+                ]
+            )
+        else:
+            args = ", ".join(
+                [
+                    self.xmlToType(type, array, parentClass=self.parent)
+                    for type, declname, array in self.params
+                ]
+            )
+        return args
+
+    def s_tpldecl(self):
+        if len(self.template_params) == 0:
+            return ""
+        return ", ".join([d["type"] + " " + d["name"] for d in self.template_params])
+
+    def s_rettype(self):
+        assert (
+            not self.special
+        ), "Member {} ({}) is a special function and no return type".format(
+            self.name, self.id
+        )
+        if len(self.parent.template_params) > 0:
+            tplargs = (
+                " <"
+                + ", ".join([d["name"] for d in self.parent.template_params])
+                + " > "
+            )
+        else:
+            tplargs = None
+        if isinstance(self.parent, ClassCompound):
+            return self.xmlToType(
+                self.rettype, parentClass=self.parent, tplargs=tplargs
+            )
+        else:
+            return self.xmlToType(self.rettype)
+
+    def s_name(self):
+        return self.xml.find("name").text.strip()
+
+    def s_docstring(self):
+        return self.index.xml_docstring.getDocString(
+            self.xml.find("briefdescription"),
+            self.xml.find("detaileddescription"),
+            self.index.output,
+        )
+
+    def n_args(self):
+        return len(self.params)
+
+    def s_argnamesstring(self):
+        def getdeclname(i, declname):
+            if declname is None or declname.text is None or declname.text.strip() == "":
+                return "arg{}".format(i)
+            return declname.text.strip()
+
+        arg = """boost::python::arg("{}")"""
+        argnames = [
+            "self",
+        ] + [getdeclname(i, declname) for i, (_, declname, _) in enumerate(self.params)]
+        return ", ".join([arg.format(n) for n in argnames])
+
+    def include(self):
+        loc = self.xml.find("location")
+        # The location is based on $CMAKE_SOURCE_DIR. Remove first directory.
+        return loc.attrib["file"].split("/", 1)[1]
+
+
+class CompoundBase(Reference):
+    def __init__(self, compound, index):
+        self.compound = compound
+        self.filename = path.join(index.directory, compound.attrib["refid"] + ".xml")
+        self.tree = etree.parse(self.filename)
+        self.definition = self.tree.getroot().find("compounddef")
+        super(CompoundBase, self).__init__(
+            index,
+            id=self.definition.attrib["id"],
+            name=self.definition.find("compoundname").text,
+        )
+
+
+class NamespaceCompound(CompoundBase):
+    def __init__(self, *args):
+        super(NamespaceCompound, self).__init__(*args)
+        self.typedefs = []
+        self.enums = []
+        self.static_funcs = []
+        self.template_params = tuple()
+
+        # Add references
+        for section in self.definition.iterchildren("sectiondef"):
+            assert "kind" in section.attrib
+            kind = section.attrib["kind"]
+            if kind == "enum":
+                self.parseEnumSection(section)
+            elif kind == "typedef":
+                self.parseTypedefSection(section)
+            elif kind == "func":
+                self.parseFuncSection(section)
+
+    def parseEnumSection(self, section):
+        for member in section.iterchildren("memberdef"):
+            ref = Reference(
+                index=self.index,
+                id=member.attrib["id"],
+                name=self.name + "::" + member.find("name").text,
+            )
+            self.index.registerReference(ref)
+            self.enums.append(member)
+            for value in member.iterchildren("enumvalue"):
+                ref = Reference(
+                    index=self.index,
+                    id=value.attrib["id"],
+                    name=self.name + "::" + member.find("name").text,
+                )
+
+    def parseTypedefSection(self, section):
+        for member in section.iterchildren("memberdef"):
+            ref = Reference(
+                index=self.index,
+                id=member.attrib["id"],
+                name=self.name + "::" + member.find("name").text,
+            )
+            self.index.registerReference(ref)
+            self.typedefs.append(member)
+
+    def parseFuncSection(self, section):
+        for member in section.iterchildren("memberdef"):
+            self.static_funcs.append(MemberDef(self.index, member, self))
+
+    def innerNamespace(self):
+        return self.name
+
+    def write(self, output):
+        pass
+
+
+class ClassCompound(CompoundBase):
+    def __init__(self, *args):
+        super(ClassCompound, self).__init__(*args)
+        self.member_funcs = list()
+        self.static_funcs = list()
+        self.special_funcs = list()
+        self.attributes = list()
+
+        self.struct = self.compound.attrib["kind"] == "struct"
+        self.public = self.definition.attrib["prot"] == "public"
+        self.template_specialization = self.name.find("<") > 0
+        self.typedef = dict()
+
+        # Handle templates
+        self._templateParams(self.definition.find("templateparamlist"))
+        for memberdef in self.definition.iter(tag="memberdef"):
+            if memberdef.attrib["prot"] != "public":
+                continue
+            if memberdef.attrib["kind"] == "variable":
+                self._attribute(memberdef)
+            elif memberdef.attrib["kind"] == "typedef":
+                ref = Reference(
+                    index=self.index,
+                    id=memberdef.attrib["id"],
+                    name=self._className() + "::" + memberdef.find("name").text,
+                )
+                self.index.registerReference(ref)
+                self.typedef[memberdef.find("name").text.strip()] = True
+
+            elif memberdef.attrib["kind"] == "enum":
+                if memberdef.find("name").text is None:
+                    ref_name = self._className() + "::" + "anonymous_enum"
+                else:
+                    ref_name = self._className() + "::" + memberdef.find("name").text
+                ref = Reference(
+                    index=self.index,
+                    id=memberdef.attrib["id"],
+                    name=ref_name,
+                )
+                self.index.registerReference(ref)
+                for value in memberdef.iterchildren("enumvalue"):
+                    value_ref = Reference(
+                        index=self.index,
+                        id=value.attrib["id"],
+                        name=ref.name,
+                    )
+                    self.index.registerReference(value_ref)
+            elif memberdef.attrib["kind"] == "function":
+                self._memberfunc(memberdef)
+
+    def _templateParams(self, tpl):
+        if tpl is not None:
+            self.template_params = tuple(
+                [_templateParamToDict(param) for param in tpl.iterchildren(tag="param")]
+            )
+        else:
+            self.template_params = tuple()
+
+    def _templateDecl(self):
+        if not hasattr(self, "template_params") or len(self.template_params) == 0:
+            return ""
+        return ", ".join([d["type"] + " " + d["name"] for d in self.template_params])
+
+    def _className(self):
+        if not hasattr(self, "template_params") or len(self.template_params) == 0:
+            return self.name
+        return (
+            self.name
+            + " <"
+            + ", ".join([d["name"] for d in self.template_params])
+            + " >"
+        )
+
+    def hasTypeDef(self, typename):
+        return typename in self.typedef
+
+    def innerNamespace(self):
+        return self._className()
+
+    def _memberfunc(self, member):
+        m = MemberDef(self.index, member, self)
+        if m.special:
+            self.special_funcs.append(m)
+        elif m.static:
+            self.static_funcs.append(m)
+        else:
+            self.member_funcs.append(m)
+
+    def _writeClassDoc(self, output):
+        docstring = self.index.xml_docstring.getDocString(
+            self.definition.find("briefdescription"),
+            self.definition.find("detaileddescription"),
+            self.index.output,
+        )
+        attribute_docstrings = ""
+        for member in self.attributes:
+            _dc = self.index.xml_docstring.getDocString(
+                member.find("briefdescription"),
+                member.find("detaileddescription"),
+                self.index.output,
+            )
+            if len(_dc) == 0:
+                continue
+            attribute_docstrings += template_class_attribute_body.format(
+                attribute=member.find("name").text,
+                docstring=_dc,
+            )
+        if len(docstring) == 0 and len(attribute_docstrings) == 0:
+            return
+        output.out(
+            template_class_doc.format(
+                tplargs=self._templateDecl(),
+                classname=self._className(),
+                docstring=docstring,
+                attributes=attribute_docstrings,
+            )
+        )
+
+    def write(self, output):
+        if not self.public:
+            return
+        if self.template_specialization:
+            output.warn(
+                "Disable class {} because template argument are not resolved for templated class specialization.".format(
+                    self.name
+                )
+            )
+            return
+
+        include = self.definition.find("includes")
+        if include is None:
+            output.err("Does not know where to write doc of", self.name)
+            return
+        output.open(include.text)
+        output.out(template_include_extern.format(filename=include.text))
+        output.out(template_open_namespace.format(namespace="doxygen"))
+
+        # Write class doc
+        self._writeClassDoc(output)
+
+        # Group member function by prototype
+        member_funcs = dict()
+        for m in self.member_funcs:
+            prototype = m.prototypekey()
+            if prototype in member_funcs:
+                member_funcs[prototype].append(m)
+            else:
+                member_funcs[prototype] = [
+                    m,
+                ]
+
+        classname_prefix = self._className() + "::"
+
+        for member in self.special_funcs:
+            docstring = member.s_docstring()
+            argnamesstring = member.s_argnamesstring()
+            if len(docstring) == 0 and len(argnamesstring) == 0:
+                continue
+            if member.s_name()[0] == "~":
+                output.out(
+                    template_destructor_doc.format(
+                        tplargs=self._templateDecl(),
+                        classname_prefix=self._className(),
+                        docstring=docstring,
+                    )
+                )
+            else:
+                output.out(
+                    template_constructor_doc.format(
+                        tplargs=", ".join(
+                            [
+                                d["type"] + " " + d["name"]
+                                for d in self.template_params + member.template_params
+                            ]
+                        ),
+                        nargs=len(member.params),
+                        comma=", " if len(member.params) > 0 else "",
+                        classname_prefix=self._className(),
+                        argsstring=member.s_args(),
+                        docstring=docstring,
+                        argnamesstring=argnamesstring,
+                    )
+                )
+
+        for prototype, members in member_funcs.items():
+            # remove undocumented members
+            documented_members = []
+            docstrings = []
+            argnamesstrings = []
+            for member in members:
+                docstring = member.s_docstring()
+                argnamesstring = member.s_argnamesstring()
+                if len(docstring) == 0 and len(argnamesstring) == 0:
+                    continue
+                documented_members.append(member)
+                docstrings.append(docstring)
+                argnamesstrings.append(argnamesstring)
+            if len(documented_members) == 0:
+                continue
+
+            # Write docstrings
+            body = "".join(
+                [
+                    template_member_func_doc_body.format(
+                        classname_prefix=classname_prefix,
+                        membername=member.s_name(),
+                        docstring=docstring,
+                        rettype=member.s_rettype(),
+                        argsstring=member.s_prototypeArgs(),
+                    )
+                    for member, docstring in zip(documented_members, docstrings)
+                ]
+            )
+
+            member = members[0]
+            tplargs = ", ".join(
+                [
+                    d["type"] + " " + d["name"]
+                    for d in self.template_params + member.template_params
+                ]
+            )
+            output.out(
+                template_member_func_doc.format(
+                    template=(
+                        "template <{}>\n".format(tplargs) if len(tplargs) > 0 else ""
+                    ),
+                    rettype=member.s_rettype(),
+                    classname_prefix=classname_prefix,
+                    argsstring=member.s_prototypeArgs(),
+                    body=body,
+                )
+            )
+
+            # Write argnamesstrings
+            body = "".join(
+                [
+                    template_member_func_args_body.format(
+                        classname_prefix=classname_prefix,
+                        membername=member.s_name(),
+                        args=argnamesstring,
+                        rettype=member.s_rettype(),
+                        argsstring=member.s_prototypeArgs(),
+                    )
+                    for member, argnamesstring in zip(
+                        documented_members, argnamesstrings
+                    )
+                ]
+            )
+
+            n_args = member.n_args()
+
+            default_args = ", ".join(
+                [
+                    """boost::python::arg("self")""",
+                ]
+                + ["""boost::python::arg("arg{}")""".format(i) for i in range(n_args)]
+            )
+            output.out(
+                template_member_func_args.format(
+                    template=(
+                        "template <{}>\n".format(tplargs) if len(tplargs) > 0 else ""
+                    ),
+                    rettype=member.s_rettype(),
+                    n=n_args + 1,
+                    default_args=default_args,
+                    classname_prefix=classname_prefix,
+                    argsstring=member.s_prototypeArgs(),
+                    body=body,
+                )
+            )
+
+        output.out(template_close_namespace.format(namespace="doxygen"))
+        output.close()
+
+    def _attribute(self, member):
+        self.attributes.append(member)
+
+
+class Index:
+    """
+    This class is responsible for generating the list of all C++-usable documented elements.
+    """
+
+    def __init__(self, input, output):
+        self.tree = etree.parse(input)
+        self.directory = path.dirname(input)
+        self.xml_docstring = XmlDocString(self)
+        self.compounds = list()
+        self.references = dict()
+        self.output = output
+
+    def parseCompound(self):
+        for compound in self.tree.getroot().iterchildren("compound"):
+            if compound.attrib["kind"] in ["class", "struct"]:
+                obj = ClassCompound(compound, self)
+            elif compound.attrib["kind"] == "namespace":
+                obj = NamespaceCompound(compound, self)
+            if obj.id not in self.compounds:
+                self.compounds.append(obj.id)
+            self.registerReference(obj)
+
+    def write(self):
+        # Header
+
+        self.output.open("doxygen_xml_parser_for_cmake.hh")
+        # self.output.out ("// Generated on {}".format (asctime()))
+        self.output.close()
+
+        # Implement template specialization for classes and member functions
+        for id in self.compounds:
+            compound = self.references[id]
+            compound.write(self.output)
+
+        self.output.open("functions.h")
+
+        # Implement template specialization for static functions
+        static_funcs = dict()
+        prototypes = list()
+        includes = list()
+        for id in self.compounds:
+            compound = self.references[id]
+            for m in compound.static_funcs:
+                include = m.include()
+                if include not in includes:
+                    includes.append(include)
+                docstring = m.s_docstring()
+                if len(docstring) == 0:
+                    continue
+                prototype = m.prototypekey()
+                if prototype in static_funcs:
+                    static_funcs[prototype].append((m, docstring))
+                else:
+                    static_funcs[prototype] = [
+                        (m, docstring),
+                    ]
+                    prototypes.append(prototype)
+
+        self.output.out(
+            "".join(
+                [
+                    template_include_extern.format(filename=filename)
+                    for filename in includes
+                ]
+            )
+        )
+
+        self.output.out(template_open_namespace.format(namespace="doxygen"))
+
+        for prototype in prototypes:
+            member_and_docstring_s = static_funcs[prototype]
+            body = "".join(
+                [
+                    template_static_func_doc_body.format(
+                        namespace=member.parent.innerNamespace(),
+                        membername=member.s_name(),
+                        docstring=docstring,
+                        rettype=member.s_rettype(),
+                        argsstring=member.s_prototypeArgs(),
+                    )
+                    for member, docstring in member_and_docstring_s
+                ]
+            )
+
+            member = member_and_docstring_s[0][0]
+            # TODO fix case of static method in templated class.
+            tplargs = ", ".join(
+                [
+                    d["type"] + " " + d["name"]
+                    for d in member.parent.template_params + member.template_params
+                ]
+            )
+            self.output.out(
+                template_static_func_doc.format(
+                    template=(
+                        "template <{}>\n".format(tplargs) if len(tplargs) > 0 else ""
+                    ),
+                    rettype=member.s_rettype(),
+                    argsstring=member.s_prototypeArgs(),
+                    body=body,
+                )
+            )
+
+        self.output.out(template_close_namespace.format(namespace="doxygen"))
+        self.output.close()
+
+    def registerReference(self, obj, overwrite=True):
+        if obj.id in self.references:
+            if obj.name != self.references[obj.id].name:
+                self.output.warn(
+                    "!!!! Compounds " + obj.id + " already exists.",
+                    obj.name,
+                    self.references[obj.id].name,
+                )
+            else:
+                self.output.warn("Reference " + obj.id + " already exists.", obj.name)
+            if not overwrite:
+                return
+        self.references[obj.id] = obj
+
+    def hasref(self, id):
+        return id in self.references
+
+    def getref(self, id):
+        return self.references[id]
+
+
+class OutputStreams(object):
+    def __init__(self, output_dir, warn, error, errorPrefix=""):
+        self.output_dir = output_dir
+        self._out = None
+        self._warn = warn
+        self._err = error
+        self.errorPrefix = errorPrefix
+
+        self._created_files = dict()
+
+    def open(self, name):
+        assert self._out is None, "You did not close the previous file"
+        import os
+
+        fullname = os.path.join(self.output_dir, name)
+        dirname = os.path.dirname(fullname)
+        if not os.path.isdir(dirname):
+            os.makedirs(dirname)
+
+        if name in self._created_files:
+            self._out = self._created_files[name]
+        else:
+            import codecs
+
+            if sys.version_info >= (3,):
+                encoding = "utf-8"
+            else:
+                encoding = "latin1"
+            self._out = codecs.open(fullname, mode="w", encoding=encoding)
+            self._created_files[name] = self._out
+
+            # Header
+            self.out(
+                template_file_header.format(
+                    path=os.path.dirname(os.path.abspath(__file__)),
+                    header_guard=makeHeaderGuard(name),
+                )
+            )
+
+    def close(self):
+        self._out = None
+
+    def writeFooterAndCloseFiles(self):
+        for n, f in self._created_files.items():
+            # Footer
+            self._out = f
+            self.out(
+                template_file_footer.format(
+                    header_guard=makeHeaderGuard(n),
+                )
+            )
+            f.close()
+        self._created_files.clear()
+        self._out = None
+
+    def out(self, *args):
+        if sys.version_info >= (3,):
+            print(*args, file=self._out)
+        else:
+            print(" ".join(str(arg) for arg in args).decode("latin1"), file=self._out)
+
+    def warn(self, *args):
+        print(self.errorPrefix, *args, file=self._warn)
+
+    def err(self, *args):
+        print(self.errorPrefix, *args, file=self._err)
+
+
+if __name__ == "__main__":
+    import argparse
+
+    parser = argparse.ArgumentParser(
+        description="Process Doxygen XML documentation and generate C++ code."
+    )
+    parser.add_argument("doxygen_index_xml", type=str, help="the Doxygen XML index.")
+    parser.add_argument("output_directory", type=str, help="the output directory.")
+    args = parser.parse_args()
+
+    index = Index(
+        input=sys.argv[1],
+        output=OutputStreams(args.output_directory, sys.stdout, sys.stderr),
+    )
+    index.parseCompound()
+    index.write()
+    index.output.writeFooterAndCloseFiles()
+    assert index.output._out is None

doc/python/xml_docstring.py

+class XmlDocString(object):
+    def __init__(self, index):
+        self.index = index
+        self.tags = {
+            "para": self.para,
+            "ref": self.ref,
+            "briefdescription": self.otherTags,
+            "detaileddescription": self.otherTags,
+            "parameterlist": self.parameterlist,
+            "parameterdescription": self.otherTags,
+            "emphasis": self.emphasis,
+            "simplesect": self.simplesect,
+            "formula": self.formula,
+            "itemizedlist": self.itemizedlist,
+            "listitem": self.listitem,
+        }
+        self.unkwownTags = set()
+        self.unkwownReferences = dict()
+        self._linesep = '\\n"\n"'
+
+        try:
+            from pylatexenc.latex2text import LatexNodes2Text
+
+            self.latex = LatexNodes2Text()
+        except ImportError:
+            self.latex = None
+
+    def clear(self):
+        self.lines = []
+        self.unkwownTags.clear()
+        self.unkwownReferences.clear()
+
+    def writeErrors(self, output):
+        ret = False
+        for t in self.unkwownTags:
+            output.warn("Unknown tag: ", t)
+            ret = True
+        for ref, node in self.unkwownReferences.items():
+            output.warn("Unknown reference: ", ref, node.text)
+            ret = True
+        return ret
+
+    def _write(self, str):
+        nlines = str.split("\n")
+        if len(self.lines) == 0:
+            self.lines += nlines
+        else:
+            self.lines[-1] += nlines[0]
+            self.lines += nlines[1:]
+            # self.lines += nlines[1:]
+
+    def _newline(self, n=1):
+        self.lines.extend(
+            [
+                "",
+            ]
+            * n
+        )
+
+    def _clean(self):
+        s = 0
+        for line in self.lines:
+            if len(line.strip()) == 0:
+                s += 1
+            else:
+                break
+        e = len(self.lines)
+        for line in reversed(self.lines):
+            if len(line.strip()) == 0:
+                e -= 1
+            else:
+                break
+        self.lines = self.lines[s:e]
+
+    def getDocString(self, brief, detailled, output):
+        self.clear()
+        if brief is not None:
+            self.visit(brief)
+        if detailled is not None and len(detailled.getchildren()) > 0:
+            if brief is not None:
+                self._newline()
+            self.visit(detailled)
+        from sys import version_info
+
+        self.writeErrors(output)
+        self._clean()
+        if version_info[0] == 2:
+            return self._linesep.join(self.lines).encode("utf-8")
+        else:
+            return self._linesep.join(self.lines)
+
+    def visit(self, node):
+        assert isinstance(node.tag, str)
+        tag = node.tag
+        if tag not in self.tags:
+            self.unknownTag(node)
+        else:
+            self.tags[tag](node)
+
+    def unknownTag(self, node):
+        self.unkwownTags.add(node.tag)
+        self.otherTags(node)
+
+    def otherTags(self, node):
+        if node.text:
+            self._write(node.text.strip().replace('"', r"\""))
+        for c in node.iterchildren():
+            self.visit(c)
+            if c.tail:
+                self._write(c.tail.strip().replace('"', r"\""))
+
+    def emphasis(self, node):
+        self._write("*")
+        self.otherTags(node)
+        self._write("*")
+
+    def simplesect(self, node):
+        self._write(node.attrib["kind"].title() + ": ")
+        self.otherTags(node)
+
+    def para(self, node):
+        if node.text:
+            self._write(node.text.replace('"', r"\""))
+        for c in node.iterchildren():
+            self.visit(c)
+            if c.tail:
+                self._write(c.tail.replace('"', r"\""))
+        self._newline()
+
+    def ref(self, node):
+        refid = node.attrib["refid"]
+        if self.index.hasref(refid):
+            self._write(self.index.getref(refid).name)
+        else:
+            self.unkwownReferences[refid] = node
+            self._write(node.text)
+        assert len(node.getchildren()) == 0
+
+    def parameterlist(self, node):
+        self._newline()
+        self._write(node.attrib["kind"].title())
+        self._newline()
+        for item in node.iterchildren("parameteritem"):
+            self.parameteritem(item)
+
+    def parameteritem(self, node):
+        indent = "  "
+        self._write(indent + "- ")
+        # should contain two children
+        assert len(node.getchildren()) == 2
+        namelist = node.find("parameternamelist")
+        desc = node.find("parameterdescription")
+        sep = ""
+        for name in namelist.iterchildren("parametername"):
+            self._write(sep + name.text)
+            sep = ", "
+        self._write(" ")
+        self.visit(desc)
+
+    def itemizedlist(self, node):
+        self._newline()
+        self.otherTags(node)
+
+    def listitem(self, node):
+        self._write("- ")
+        self.otherTags(node)
+
+    def formula(self, node):
+        if node.text:
+            if self.latex is None:
+                self._write(node.text.strip())
+            else:
+                self._write(self.latex.latex_to_text(node.text))

doc/shape-mesh-collision-call.dot

+digraph CD  {
+
+	rankdir = BT
+	compound=true
+        size = 11.7
+
+        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box]
+        "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box]
+        "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" [shape = box]
+        "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" [shape = box]
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box]
+        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box]
+        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box]
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box]
+        "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box]
+        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box]
+        "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box]
+
+        "BVHShapeCollider<OBBRSS, Shape, NarrowPhaseSolver>::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp"
+        "details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp"
+        "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp"
+        "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
+        "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp"
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h"
+        "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h"
+        "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red]
+        "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red]
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp"
+        "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp"
+        "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293"
+        "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156"
+}

doc/shape-shape-collision-call.dot

+digraph CD  {
+
+	rankdir = BT
+	compound=true
+        size = 11.7
+
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box]
+        "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box]
+        "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box]
+        "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box]
+        "template<typename S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box]
+
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)"
+        "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)"
+        "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" -> "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)"
+        "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" -> "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h"
+        "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template<typename S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h"
+}

fcl.pc.in

-# This file was generated by CMake for @PROJECT_NAME@
-prefix=@CMAKE_INSTALL_PREFIX@
-exec_prefix=${prefix}
-libdir=${prefix}/lib
-includedir=${prefix}/include
-
-Name: @PROJECT_NAME@
-Description: @PKG_DESC@
-Version: @FCL_VERSION@
-Requires: @PKG_EXTERNAL_DEPS@
-Libs: -L${libdir} -lfcl
-Cflags: -I${includedir}

flake.lock

+{
+  "nodes": {
+    "flake-parts": {
+      "inputs": {
+        "nixpkgs-lib": "nixpkgs-lib"
+      },
+      "locked": {
+        "lastModified": 1738453229,
+        "narHash": "sha256-7H9XgNiGLKN1G1CgRh0vUL4AheZSYzPm+zmZ7vxbJdo=",
+        "owner": "hercules-ci",
+        "repo": "flake-parts",
+        "rev": "32ea77a06711b758da0ad9bd6a844c5740a87abd",
+        "type": "github"
+      },
+      "original": {
+        "owner": "hercules-ci",
+        "repo": "flake-parts",
+        "type": "github"
+      }
+    },
+    "nixpkgs": {
+      "locked": {
+        "lastModified": 1734122394,
+        "narHash": "sha256-TmVqB5V9ZIn66jlyPcp4yzsC6uF46YQLH00MSBio42c=",
+        "owner": "NixOS",
+        "repo": "nixpkgs",
+        "rev": "eb1b38d147a53360c11f0d033196f94d844bd86c",
+        "type": "github"
+      },
+      "original": {
+        "owner": "NixOS",
+        "ref": "refs/pull/357705/head",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
+    "nixpkgs-lib": {
+      "locked": {
+        "lastModified": 1738452942,
+        "narHash": "sha256-vJzFZGaCpnmo7I6i416HaBLpC+hvcURh/BQwROcGIp8=",
+        "type": "tarball",
+        "url": "https://github.com/NixOS/nixpkgs/archive/072a6db25e947df2f31aab9eccd0ab75d5b2da11.tar.gz"
+      },
+      "original": {
+        "type": "tarball",
+        "url": "https://github.com/NixOS/nixpkgs/archive/072a6db25e947df2f31aab9eccd0ab75d5b2da11.tar.gz"
+      }
+    },
+    "root": {
+      "inputs": {
+        "flake-parts": "flake-parts",
+        "nixpkgs": "nixpkgs"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}

flake.nix

+{
+  description = "An extension of the Flexible Collision Library";
+
+  inputs = {
+    flake-parts.url = "github:hercules-ci/flake-parts";
+    # TODO: switch back to nixos-unstable after
+    # https://github.com/NixOS/nixpkgs/pull/357705
+    nixpkgs.url = "github:NixOS/nixpkgs/refs/pull/357705/head";
+  };
+
+  outputs =
+    inputs:
+    inputs.flake-parts.lib.mkFlake { inherit inputs; } {
+      systems = inputs.nixpkgs.lib.systems.flakeExposed;
+      perSystem =
+        { pkgs, self', ... }:
+        {
+          apps.default = {
+            type = "app";
+            program = pkgs.python3.withPackages (_: [ self'.packages.default ]);
+          };
+          devShells.default = pkgs.mkShell { inputsFrom = [ self'.packages.default ]; };
+          packages = {
+            default = self'.packages.coal;
+            coal = pkgs.python3Packages.coal.overrideAttrs (_: {
+              src = pkgs.lib.fileset.toSource {
+                root = ./.;
+                fileset = pkgs.lib.fileset.unions [
+                  ./CMakeLists.txt
+                  ./doc
+                  ./hpp-fclConfig.cmake
+                  ./include
+                  ./package.xml
+                  ./python
+                  ./src
+                  ./test
+                ];
+              };
+            });
+          };
+        };
+    };
+}

hpp-fclConfig.cmake

+# This file provide bacward compatiblity for `find_package(hpp-fcl)`.
+
+message(WARNING "Please update your CMake from 'hpp-fcl' to 'coal'")
+
+find_package(coal REQUIRED)
+
+if(NOT TARGET hpp-fcl::hpp-fcl)
+  add_library(hpp-fcl::hpp-fcl INTERFACE IMPORTED)
+
+  # Compute the installation prefix relative to this file.
+  # This code is taken from generated cmake xxxTargets.cmake.
+  get_filename_component(_IMPORT_PREFIX "${CMAKE_CURRENT_LIST_FILE}" PATH)
+  get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+  get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+  get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+  if(_IMPORT_PREFIX STREQUAL "/")
+    set(_IMPORT_PREFIX "")
+  endif()
+
+  set_target_properties(
+    hpp-fcl::hpp-fcl
+    PROPERTIES INTERFACE_COMPILE_DEFINITIONS
+               "COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL"
+               INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include"
+               INTERFACE_LINK_LIBRARIES "coal::coal")
+endif()