gjk.py 13.8 KB
Newer Older
1
2
#!/usr/bin/env python3
import pdb
3
import sys
4

5
6
# 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
7

8
9
10
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"
11

12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
# 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)))
34
35
36
37

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

38
39
40
41
42
43
44
# 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]
45
46
47
48
49
50
51
52
53
54
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 ],
        ]
55
# conditions is a list of (list of (list of check IDs to be ANDed) to be ORed).
56
57
58
59
60
61
62
63
conditions = [
        [],
        [],
        [],
        [],
        [],
        [],
        [],
64
        [ ], #[ [10, 11, 12], ], # I don't think this is always true...
65
        ]
66
# rejections is a list of (list of (list of check IDs to be ANDed) to be ORed).
67
rejections = [
68
69
70
        [ [  2,  6,  7], [  3,-  8,-  9], ],
        [ [  3,  8,  9], [  1,-  4,-  5], ],
        [ [  1,  4,  5], [  2,-  6,-  7], ],
71
72
73
74
75
76
77
        [ [- 1,- 3], ],
        [ [- 2,- 1], ],
        [ [- 3,- 2], ],
        [ [  4,- 5], [  6,- 7], [  8,- 9], ],
        [],
        ]

78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
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] ],

        [ [- 4,  9,- 10,], [- 11,- 12] ],
        [ [- 6,  5,- 11,], [- 12,- 10] ],
        [ [- 8,  7,- 12,], [- 10,- 11] ],
        ]

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

131
132
133
134
135
136
137
138
139
140
141
142
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

143
    # Check all cases.
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
    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:
176
        return prevScore, { 'case': None, 'comments': [ "applied " + str(test_values), "to " + ", ".join([regions[c] for c in cases ]) ] }
177
178
179
180
181
182
183
184
185
186

    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]
187
        #currentScore = prevScore + weights[test]
188
189
190
        if currentScore > currentBestScore: # Cannot do better -> stop
            continue

191
192
193
194
        try:
            remaining_tests, next_test_values = set_test_values (current_tests, test_values, test, True)
        except ValueError:
            remaining_tests = None
195

196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
        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
214
215
216
217
218
219
220
221
222
223
224
225
226

        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

227
228
229
230
231
232
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):
233
    if start:
234
235
236
237
        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 int a = 3, b = 2, c = 1, d = 0;", file=file)
238
        for l in "abcd":
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
            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)
262
263

    if 'case' in order:
264
265
266
267
268
269
270
271
272
273
        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']
274
        if region == "Inside":
275
            print (indent + "REGION_INSIDE()", file=file)
276
277
            toFree = []
        elif region == 'A':
278
            print (indent + "originToPoint (current, a, A, next, ray);", file=file)
279
280
        elif len(region)==2:
            a = region[0]
281
282
283
284
            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())
285
        elif len(region)==3:
286
287
288
289
290
291
292
293
294
            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())
295
296
297
        else:
            assert False, "Unknown region " + region
        for pt in toFree:
298
            print (indent + "free_v[nfree++] = current.vertex[{}];".format(pt), file=file)
299
300
    else:
        assert "test" in order and 'true' in order and 'false' in order
301
302
303
304
305
306
307
308
309
310
311
312
        check    = checks[order['test']]
        check_hr = checks_hr[order['test']]
        printComments (order, indent, file)
        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), file=file)
                printOrder (order['false'], indent=indent, start=False, file=file)
        elif order['false'] is None:
            print (indent + "assert({} <= 0); // {}".format(check, check_hr), file=file)
            printOrder (order['true'], indent=indent, start=False, file=file)
313
        else:
314
315
316
317
318
            print (indent + "if ({} <= 0) {{ // if {}".format(check, check_hr), file=file)
            printOrder (order['true'], indent=indent+"  ", start=False, file=file)
            print (indent + "}} else {{ // not {}".format(check_hr), file=file)
            printOrder (order['false'], indent=indent+"  ", start=False, file=file)
            print (indent + "}} // end of {}".format(check_hr), file=file)
319
320

    if start:
321
322
323
324
325
326
327
328
329
330
331
332
333
334
        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()
335
336
337
338
339
340
341
342
343

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.