doc/gjk.py

Go to the documentation of this file.

#!/usr/bin/env python3
import pdb
import sys

# ABC = AB^AC
# (ABC^AJ).a = (j.c - j.b) a.a + (j.a - j.c) b.a + (j.b - j.a) c.a, for j = b or c

segment_fmt = "{j}a_aa"
plane_fmt = ""
edge_fmt = "{j}a * {b}a_{c}a + {j}{b} * {c}a_aa - {j}{c} * {b}a_aa"

# These checks must be negative and not positive, as in the cheat sheet.
# They are the same as in the cheat sheet, except that we consider (...).dot(A) instead of (...).dot(-A)
plane_tests = ["C.dot (a_cross_b)", "D.dot(a_cross_c)", "-D.dot(a_cross_b)"]
checks = (
    plane_tests
    + [edge_fmt.format(**{"j": j, "b": "b", "c": "c"}) for j in ["b", "c"]]
    + [edge_fmt.format(**{"j": j, "b": "c", "c": "d"}) for j in ["c", "d"]]
    + [edge_fmt.format(**{"j": j, "b": "d", "c": "b"}) for j in ["d", "b"]]
    + [segment_fmt.format(**{"j": j}) for j in ["b", "c", "d"]]
)
checks_hr = (
    ["ABC.AO >= 0", "ACD.AO >= 0", "ADB.AO >= 0"]
    + ["(ABC ^ {}).AO >= 0".format(n) for n in ["AB", "AC"]]
    + ["(ACD ^ {}).AO >= 0".format(n) for n in ["AC", "AD"]]
    + ["(ADB ^ {}).AO >= 0".format(n) for n in ["AD", "AB"]]
    + ["AB.AO >= 0", "AC.AO >= 0", "AD.AO >= 0"]
)

# weights of the checks.
weights = (
    [
        2,
    ]
    * 3
    + [
        3,
    ]
    * 6
    + [
        1,
    ]
    * 3
)

# Segment tests first, because they have lower weight.
# tests = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, ]
tests = [
    9,
    10,
    11,
    0,
    1,
    2,
    3,
    4,
    5,
    6,
    7,
    8,
]
assert len(tests) == len(checks)
assert sorted(tests) == list(range(len(tests)))

regions = [
    "ABC",
    "ACD",
    "ADB",
    "AB",
    "AC",
    "AD",
    "A",
    "Inside",
]
cases = list(range(len(regions)))

# The following 3 lists refer to table doc/GJK_tetrahedra_boolean_table.ods

# A check ID is (+/- (index+1)) where a minus sign encodes a NOT operation
# and index refers to an index in list checks.

# definitions is a list of list of check IDs to be ANDed.
# For instance, a0.a3.!a4 -> [ 1, 4, -5]
definitions = [
    [1, 4, -5],
    [2, 6, -7],
    [3, 8, -9],
    [-4, 9, 10],
    [-6, 5, 11],
    [-8, 7, 12],
    [-10, -11, -12],
    [-1, -2, -3],
]
# conditions is a list of (list of (list of check IDs to be ANDed) to be ORed).
conditions = [
    [],
    [],
    [],
    [],
    [],
    [],
    [],
    [],  # [ [10, 11, 12], ], # I don't think this is always true...
]
# rejections is a list of (list of (list of check IDs to be ANDed) to be ORed).
rejections = [
    [
        [2, 6, 7],
        [3, -8, -9],
    ],
    [
        [3, 8, 9],
        [1, -4, -5],
    ],
    [
        [1, 4, 5],
        [2, -6, -7],
    ],
    [
        [-1, -3],
    ],
    [
        [-2, -1],
    ],
    [
        [-3, -2],
    ],
    [
        [4, -5],
        [6, -7],
        [8, -9],
    ],
    [],
]

implications = [
    [
        [
            4,
            5,
            10,
        ],
        [11],
    ],
    [
        [
            6,
            7,
            11,
        ],
        [12],
    ],
    [
        [
            8,
            9,
            12,
        ],
        [10],
    ],
    [
        [
            -4,
            -5,
            11,
        ],
        [10],
    ],
    [
        [
            -6,
            -7,
            12,
        ],
        [11],
    ],
    [
        [
            -8,
            -9,
            10,
        ],
        [12],
    ],
    [[1, 4, 5, 6], [-7]],
    [[2, 6, 9, 8], [-9]],
    [[3, 8, 9, 4], [-5]],
    [
        [
            -4,
            5,
            10,
        ],
        [-11],
    ],
    [
        [
            4,
            -5,
            -10,
        ],
        [11],
    ],
    [
        [
            -6,
            7,
            11,
        ],
        [-12],
    ],
    [
        [
            6,
            -7,
            -11,
        ],
        [12],
    ],
    [
        [
            -8,
            9,
            12,
        ],
        [-10],
    ],
    [
        [
            8,
            -9,
            -12,
        ],
        [10],
    ],
    [[10, 3, 9, -12, 4, -5], [1]],
    [[10, -3, 1, -4], [9]],
    [[10, -3, -1, 2, -6, 11], [5]],
    [[-10, 11, 2, -12, -5, -1], [6]],
    [[-10, 11, -2, 1, 5], [-6]],
    [[-10, -11, 12, 1, -7, -2, 4], [-5]],
    [[-10, -11, 12, -3, 2, 7], [-8]],
    [[-10, -11, 12, -3, -2], [-1]],
]


def set_test_values(current_tests, test_values, itest, value):
    def satisfies(values, indices):
        for k in indices:
            if k > 0 and values[k - 1] != True:
                return False
            if k < 0 and values[-k - 1] != False:
                return False
        return True

    remaining_tests = list(current_tests)
    next_test_values = list(test_values)

    remaining_tests.remove(itest)
    next_test_values[itest] = value
    rerun = True
    while rerun:
        rerun = False
        for impl in implications:
            if satisfies(next_test_values, impl[0]):
                for id in impl[1]:
                    k = (id - 1) if id > 0 else (-id - 1)
                    if k in remaining_tests:
                        next_test_values[k] = id > 0
                        remaining_tests.remove(k)
                        rerun = True
                    else:
                        if next_test_values[k] != (id > 0):
                            raise ValueError("Absurd case")
    return remaining_tests, next_test_values


def set_tests_values(current_tests, test_values, itests, values):
    for itest, value in zip(itests, values):
        current_tests, test_values = set_test_values(
            current_tests, test_values, itest, value
        )
    return current_tests, test_values


def apply_test_values(cases, test_values):
    def canSatisfy(values, indices):
        for k in indices:
            if k > 0 and values[k - 1] == False:
                return False
            if k < 0 and values[-k - 1] == True:
                return False
        return True

    def satisfies(values, indices):
        for k in indices:
            if k > 0 and values[k - 1] != True:
                return False
            if k < 0 and values[-k - 1] != False:
                return False
        return True

    # Check all cases.
    left_cases = []
    for case in cases:
        defi = definitions[case]
        conds = conditions[case]
        rejs = rejections[case]
        if satisfies(test_values, defi):
            # A definition is True, stop recursion
            return [case]
        if not canSatisfy(test_values, defi):
            continue
        for cond in conds:
            if satisfies(test_values, cond):
                # A condition is True, stop recursion
                return [case]
        append = True
        for rej in rejs:
            if satisfies(test_values, rej):
                # A rejection is True, discard this case
                append = False
                break
        if append:
            left_cases.append(case)
    return left_cases


def max_number_of_tests(
    current_tests,
    cases,
    test_values=[
        None,
    ]
    * len(tests),
    prevBestScore=float("inf"),
    prevScore=0,
):
    for test in current_tests:
        assert test_values[test] == None, "Test " + str(test) + " already performed"

    left_cases = apply_test_values(cases, test_values)

    if len(left_cases) == 1:
        return prevScore, {
            "case": left_cases[0],
        }
    elif len(left_cases) == 0:
        return prevScore, {
            "case": None,
            "comments": [
                "applied " + str(test_values),
                "to " + ", ".join([regions[c] for c in cases]),
            ],
        }

    assert len(current_tests) > 0, "No more test but " + str(left_cases) + " remains"

    currentBestScore = prevBestScore
    bestScore = float("inf")
    bestOrder = [None, None]
    for i, test in enumerate(current_tests):
        assert bestScore >= currentBestScore

        currentScore = prevScore + len(left_cases) * weights[test]
        # currentScore = prevScore + weights[test]
        if currentScore > currentBestScore:  # Cannot do better -> stop
            continue

        try:
            remaining_tests, next_test_values = set_test_values(
                current_tests, test_values, test, True
            )
        except ValueError:
            remaining_tests = None

        if remaining_tests is not None:
            # Do not put this in try catch as I do not want other ValueError to be understood as an infeasible branch.
            score_if_t, order_if_t = max_number_of_tests(
                remaining_tests,
                left_cases,
                next_test_values,
                currentBestScore,
                currentScore,
            )
            if score_if_t >= currentBestScore:  # True didn't do better -> stop
                continue
        else:
            score_if_t, order_if_t = prevScore, None

        try:
            remaining_tests, next_test_values = set_test_values(
                current_tests, test_values, test, False
            )
        except ValueError:
            remaining_tests = None

        if remaining_tests is not None:
            # Do not put this in try catch as I do not want other ValueError to be understood as an infeasible branch.
            score_if_f, order_if_f = max_number_of_tests(
                remaining_tests,
                left_cases,
                next_test_values,
                currentBestScore,
                currentScore,
            )
        else:
            score_if_f, order_if_f = prevScore, None

        currentScore = max(score_if_t, score_if_f)
        if currentScore < bestScore:
            if currentScore < currentBestScore:
                bestScore = currentScore
                bestOrder = {"test": test, "true": order_if_t, "false": order_if_f}
                # pdb.set_trace()
                currentBestScore = currentScore
                if len(tests) == len(current_tests):
                    print("New best score: {}".format(currentBestScore))

    return bestScore, bestOrder


def printComments(order, indent, file):
    if "comments" in order:
        for comment in order["comments"]:
            print(indent + "// " + comment, file=file)


def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]):
    if start:
        print(
            "bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)",
            file=file,
        )
        print("{", file=file)
        print(
            indent + "// The code of this function was generated using doc/gjk.py",
            file=file,
        )
        print(indent + "const vertex_id_t a = 3, b = 2, c = 1, d = 0;", file=file)
        for l in "abcd":
            print(
                indent
                + "const Vec3f& {} (current.vertex[{}]->w);".format(l.upper(), l),
                file=file,
            )
        print(indent + "const FCL_REAL aa = A.squaredNorm();".format(l), file=file)
        for l in "dcb":
            for m in "abcd":
                if m <= l:
                    print(
                        indent
                        + "const FCL_REAL {0}{1}    = {2}.dot({3});".format(
                            l, m, l.upper(), m.upper()
                        ),
                        file=file,
                    )
                else:
                    print(
                        indent + "const FCL_REAL& {0}{1}    = {1}{0};".format(l, m),
                        file=file,
                    )
            print(indent + "const FCL_REAL {0}a_aa = {0}a - aa;".format(l), file=file)
        for l0, l1 in zip("bcd", "cdb"):
            print(
                indent + "const FCL_REAL {0}a_{1}a = {0}a - {1}a;".format(l0, l1),
                file=file,
            )
        for l in "bc":
            print(
                indent + "const Vec3f a_cross_{0} = A.cross({1});".format(l, l.upper()),
                file=file,
            )
        print("", file=file)
        print("#define REGION_INSIDE()               " + indent + "\\", file=file)
        print(indent + "  ray.setZero();                      \\", file=file)
        print(indent + "  next.vertex[0] = current.vertex[d]; \\", file=file)
        print(indent + "  next.vertex[1] = current.vertex[c]; \\", file=file)
        print(indent + "  next.vertex[2] = current.vertex[b]; \\", file=file)
        print(indent + "  next.vertex[3] = current.vertex[a]; \\", file=file)
        print(indent + "  next.rank=4;                        \\", file=file)
        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:
            a = region[0]
            B = region[1]
            print(
                indent
                + "originToSegment (current, a, {b}, A, {B}, {B}-A, -{b}a_aa, next, ray);".format(
                    **{
                        "b": B.lower(),
                        "B": B,
                    }
                ),
                file=file,
            )
            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.