doc/gjk.py
Go to the documentation of this file.
1 #!/usr/bin/env python3
2 import pdb
3 import sys
4 
5 # ABC = AB^AC
6 # (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 segment_fmt = "{j}a_aa"
9 plane_fmt = ""
10 edge_fmt = "{j}a * {b}a_{c}a + {j}{b} * {c}a_aa - {j}{c} * {b}a_aa"
11 
12 # These checks must be negative and not positive, as in the cheat sheet.
13 # They are the same as in the cheat sheet, except that we consider (...).dot(A)
14 # instead of (...).dot(-A)
15 plane_tests = ["C.dot (a_cross_b)", "D.dot(a_cross_c)", "-D.dot(a_cross_b)"]
16 checks = (
17  plane_tests
18  + [edge_fmt.format(**{"j": j, "b": "b", "c": "c"}) for j in ["b", "c"]]
19  + [edge_fmt.format(**{"j": j, "b": "c", "c": "d"}) for j in ["c", "d"]]
20  + [edge_fmt.format(**{"j": j, "b": "d", "c": "b"}) for j in ["d", "b"]]
21  + [segment_fmt.format(**{"j": j}) for j in ["b", "c", "d"]]
22 )
23 checks_hr = (
24  ["ABC.AO >= 0", "ACD.AO >= 0", "ADB.AO >= 0"]
25  + ["(ABC ^ {}).AO >= 0".format(n) for n in ["AB", "AC"]]
26  + ["(ACD ^ {}).AO >= 0".format(n) for n in ["AC", "AD"]]
27  + ["(ADB ^ {}).AO >= 0".format(n) for n in ["AD", "AB"]]
28  + ["AB.AO >= 0", "AC.AO >= 0", "AD.AO >= 0"]
29 )
30 
31 # weights of the checks.
32 weights = (
33  [
34  2,
35  ]
36  * 3
37  + [
38  3,
39  ]
40  * 6
41  + [
42  1,
43  ]
44  * 3
45 )
46 
47 # Segment tests first, because they have lower weight.
48 # tests = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, ]
49 tests = [
50  9,
51  10,
52  11,
53  0,
54  1,
55  2,
56  3,
57  4,
58  5,
59  6,
60  7,
61  8,
62 ]
63 assert len(tests) == len(checks)
64 assert sorted(tests) == list(range(len(tests)))
65 
66 regions = [
67  "ABC",
68  "ACD",
69  "ADB",
70  "AB",
71  "AC",
72  "AD",
73  "A",
74  "Inside",
75 ]
76 cases = list(range(len(regions)))
77 
78 # The following 3 lists refer to table doc/GJK_tetrahedra_boolean_table.ods
79 
80 # A check ID is (+/- (index+1)) where a minus sign encodes a NOT operation
81 # and index refers to an index in list checks.
82 
83 # definitions is a list of list of check IDs to be ANDed.
84 # For instance, a0.a3.!a4 -> [ 1, 4, -5]
85 definitions = [
86  [1, 4, -5],
87  [2, 6, -7],
88  [3, 8, -9],
89  [-4, 9, 10],
90  [-6, 5, 11],
91  [-8, 7, 12],
92  [-10, -11, -12],
93  [-1, -2, -3],
94 ]
95 # conditions is a list of (list of (list of check IDs to be ANDed) to be ORed).
96 conditions = [
97  [],
98  [],
99  [],
100  [],
101  [],
102  [],
103  [],
104  [], # [ [10, 11, 12], ], # I don't think this is always true...
105 ]
106 # rejections is a list of (list of (list of check IDs to be ANDed) to be ORed).
107 rejections = [
108  [
109  [2, 6, 7],
110  [3, -8, -9],
111  ],
112  [
113  [3, 8, 9],
114  [1, -4, -5],
115  ],
116  [
117  [1, 4, 5],
118  [2, -6, -7],
119  ],
120  [
121  [-1, -3],
122  ],
123  [
124  [-2, -1],
125  ],
126  [
127  [-3, -2],
128  ],
129  [
130  [4, -5],
131  [6, -7],
132  [8, -9],
133  ],
134  [],
135 ]
136 
137 implications = [
138  [
139  [
140  4,
141  5,
142  10,
143  ],
144  [11],
145  ],
146  [
147  [
148  6,
149  7,
150  11,
151  ],
152  [12],
153  ],
154  [
155  [
156  8,
157  9,
158  12,
159  ],
160  [10],
161  ],
162  [
163  [
164  -4,
165  -5,
166  11,
167  ],
168  [10],
169  ],
170  [
171  [
172  -6,
173  -7,
174  12,
175  ],
176  [11],
177  ],
178  [
179  [
180  -8,
181  -9,
182  10,
183  ],
184  [12],
185  ],
186  [[1, 4, 5, 6], [-7]],
187  [[2, 6, 9, 8], [-9]],
188  [[3, 8, 9, 4], [-5]],
189  [
190  [
191  -4,
192  5,
193  10,
194  ],
195  [-11],
196  ],
197  [
198  [
199  4,
200  -5,
201  -10,
202  ],
203  [11],
204  ],
205  [
206  [
207  -6,
208  7,
209  11,
210  ],
211  [-12],
212  ],
213  [
214  [
215  6,
216  -7,
217  -11,
218  ],
219  [12],
220  ],
221  [
222  [
223  -8,
224  9,
225  12,
226  ],
227  [-10],
228  ],
229  [
230  [
231  8,
232  -9,
233  -12,
234  ],
235  [10],
236  ],
237  [[10, 3, 9, -12, 4, -5], [1]],
238  [[10, -3, 1, -4], [9]],
239  [[10, -3, -1, 2, -6, 11], [5]],
240  [[-10, 11, 2, -12, -5, -1], [6]],
241  [[-10, 11, -2, 1, 5], [-6]],
242  [[-10, -11, 12, 1, -7, -2, 4], [-5]],
243  [[-10, -11, 12, -3, 2, 7], [-8]],
244  [[-10, -11, 12, -3, -2], [-1]],
245 ]
246 
247 
248 def set_test_values(current_tests, test_values, itest, value):
249  def satisfies(values, indices):
250  for k in indices:
251  if k > 0 and values[k - 1] is not True:
252  return False
253  if k < 0 and values[-k - 1] is not False:
254  return False
255  return True
256 
257  remaining_tests = list(current_tests)
258  next_test_values = list(test_values)
259 
260  remaining_tests.remove(itest)
261  next_test_values[itest] = value
262  rerun = True
263  while rerun:
264  rerun = False
265  for impl in implications:
266  if satisfies(next_test_values, impl[0]):
267  for id in impl[1]:
268  k = (id - 1) if id > 0 else (-id - 1)
269  if k in remaining_tests:
270  next_test_values[k] = id > 0
271  remaining_tests.remove(k)
272  rerun = True
273  else:
274  if next_test_values[k] != (id > 0):
275  raise ValueError("Absurd case")
276  return remaining_tests, next_test_values
277 
278 
279 def set_tests_values(current_tests, test_values, itests, values):
280  for itest, value in zip(itests, values):
281  current_tests, test_values = set_test_values(
282  current_tests, test_values, itest, value
283  )
284  return current_tests, test_values
285 
286 
287 def apply_test_values(cases, test_values):
288  def canSatisfy(values, indices):
289  for k in indices:
290  if k > 0 and values[k - 1] is False:
291  return False
292  if k < 0 and values[-k - 1] is True:
293  return False
294  return True
295 
296  def satisfies(values, indices):
297  for k in indices:
298  if k > 0 and values[k - 1] is not True:
299  return False
300  if k < 0 and values[-k - 1] is not False:
301  return False
302  return True
303 
304  # Check all cases.
305  left_cases = []
306  for case in cases:
307  defi = definitions[case]
308  conds = conditions[case]
309  rejs = rejections[case]
310  if satisfies(test_values, defi):
311  # A definition is True, stop recursion
312  return [case]
313  if not canSatisfy(test_values, defi):
314  continue
315  for cond in conds:
316  if satisfies(test_values, cond):
317  # A condition is True, stop recursion
318  return [case]
319  append = True
320  for rej in rejs:
321  if satisfies(test_values, rej):
322  # A rejection is True, discard this case
323  append = False
324  break
325  if append:
326  left_cases.append(case)
327  return left_cases
328 
329 
330 def max_number_of_tests(
331  current_tests,
332  cases,
333  test_values=[
334  None,
335  ]
336  * len(tests),
337  prevBestScore=float("inf"),
338  prevScore=0,
339 ):
340  for test in current_tests:
341  assert test_values[test] is None, "Test " + str(test) + " already performed"
342 
343  left_cases = apply_test_values(cases, test_values)
344 
345  if len(left_cases) == 1:
346  return prevScore, {
347  "case": left_cases[0],
348  }
349  elif len(left_cases) == 0:
350  return prevScore, {
351  "case": None,
352  "comments": [
353  "applied " + str(test_values),
354  "to " + ", ".join([regions[c] for c in cases]),
355  ],
356  }
357 
358  assert len(current_tests) > 0, "No more test but " + str(left_cases) + " remains"
359 
360  currentBestScore = prevBestScore
361  bestScore = float("inf")
362  bestOrder = [None, None]
363  for i, test in enumerate(current_tests):
364  assert bestScore >= currentBestScore
365 
366  currentScore = prevScore + len(left_cases) * weights[test]
367  # currentScore = prevScore + weights[test]
368  if currentScore > currentBestScore: # Cannot do better -> stop
369  continue
370 
371  try:
372  remaining_tests, next_test_values = set_test_values(
373  current_tests, test_values, test, True
374  )
375  except ValueError:
376  remaining_tests = None
377 
378  if remaining_tests is not None:
379  # Do not put this in try catch as I do not want other ValueError to be
380  # understood as an infeasible branch.
381  score_if_t, order_if_t = max_number_of_tests(
382  remaining_tests,
383  left_cases,
384  next_test_values,
385  currentBestScore,
386  currentScore,
387  )
388  if score_if_t >= currentBestScore: # True didn't do better -> stop
389  continue
390  else:
391  score_if_t, order_if_t = prevScore, None
392 
393  try:
394  remaining_tests, next_test_values = set_test_values(
395  current_tests, test_values, test, False
396  )
397  except ValueError:
398  remaining_tests = None
399 
400  if remaining_tests is not None:
401  # Do not put this in try catch as I do not want other ValueError to be
402  # understood as an infeasible branch.
403  score_if_f, order_if_f = max_number_of_tests(
404  remaining_tests,
405  left_cases,
406  next_test_values,
407  currentBestScore,
408  currentScore,
409  )
410  else:
411  score_if_f, order_if_f = prevScore, None
412 
413  currentScore = max(score_if_t, score_if_f)
414  if currentScore < bestScore:
415  if currentScore < currentBestScore:
416  bestScore = currentScore
417  bestOrder = {"test": test, "true": order_if_t, "false": order_if_f}
418  # pdb.set_trace()
419  currentBestScore = currentScore
420  if len(tests) == len(current_tests):
421  print("New best score: {}".format(currentBestScore))
422 
423  return bestScore, bestOrder
424 
425 
426 def printComments(order, indent, file):
427  if "comments" in order:
428  for comment in order["comments"]:
429  print(indent + "// " + comment, file=file)
430 
431 
432 def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]):
433  if start:
434  print(
435  "bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)",
436  file=file,
437  )
438  print("{", file=file)
439  print(
440  indent + "// The code of this function was generated using doc/gjk.py",
441  file=file,
442  )
443  print(indent + "const vertex_id_t a = 3, b = 2, c = 1, d = 0;", file=file)
444  for v in "abcd":
445  print(
446  indent
447  + "const Vec3f& {} (current.vertex[{}]->w);".format(v.upper(), v),
448  file=file,
449  )
450  print(indent + "const FCL_REAL aa = A.squaredNorm();".format(), file=file)
451  for v in "dcb":
452  for m in "abcd":
453  if m <= v:
454  print(
455  indent
456  + "const FCL_REAL {0}{1} = {2}.dot({3});".format(
457  v, m, v.upper(), m.upper()
458  ),
459  file=file,
460  )
461  else:
462  print(
463  indent + "const FCL_REAL& {0}{1} = {1}{0};".format(v, m),
464  file=file,
465  )
466  print(indent + "const FCL_REAL {0}a_aa = {0}a - aa;".format(v), file=file)
467  for l0, l1 in zip("bcd", "cdb"):
468  print(
469  indent + "const FCL_REAL {0}a_{1}a = {0}a - {1}a;".format(l0, l1),
470  file=file,
471  )
472  for v in "bc":
473  print(
474  indent + "const Vec3f a_cross_{0} = A.cross({1});".format(v, v.upper()),
475  file=file,
476  )
477  print("", file=file)
478  print("#define REGION_INSIDE() " + indent + "\\", file=file)
479  print(indent + " ray.setZero(); \\", file=file)
480  print(indent + " next.vertex[0] = current.vertex[d]; \\", file=file)
481  print(indent + " next.vertex[1] = current.vertex[c]; \\", file=file)
482  print(indent + " next.vertex[2] = current.vertex[b]; \\", file=file)
483  print(indent + " next.vertex[3] = current.vertex[a]; \\", file=file)
484  print(indent + " next.rank=4; \\", file=file)
485  print(indent + " return true;", file=file)
486  print("", file=file)
487 
488  if "case" in order:
489  case = order["case"]
490  if case is None:
491  print(
492  indent + "// There are no case corresponding to this set of tests.",
493  file=file,
494  )
495  printComments(order, indent, file)
496  print(indent + "assert(false);", file=file)
497  return
498  region = regions[case]
499  print(indent + "// Region " + region, file=file)
500  printComments(order, indent, file)
501  toFree = ["b", "c", "d"]
502  if region == "Inside":
503  print(indent + "REGION_INSIDE()", file=file)
504  toFree = []
505  elif region == "A":
506  print(indent + "originToPoint (current, a, A, next, ray);", file=file)
507  elif len(region) == 2:
508  region[0]
509  B = region[1]
510  print(
511  indent + "originToSegment "
512  "(current, a, {b}, A, {B}, {B}-A, -{b}a_aa, next, ray);".format(
513  **{"b": B.lower(), "B": B}
514  ),
515  file=file,
516  )
517  toFree.remove(B.lower())
518  elif len(region) == 3:
519  B = region[1]
520  C = region[2]
521  test = plane_tests[["ABC", "ACD", "ADB"].index(region)]
522  if test.startswith("-"):
523  test = test[1:]
524  else:
525  test = "-" + test
526  print(
527  indent + "originToTriangle "
528  "(current, a, {b}, {c}, ({B}-A).cross({C}-A), {t}, next, ray);".format(
529  **{"b": B.lower(), "c": C.lower(), "B": B, "C": C, "t": test}
530  ),
531  file=file,
532  )
533  toFree.remove(B.lower())
534  toFree.remove(C.lower())
535  else:
536  assert False, "Unknown region " + region
537  for pt in toFree:
538  print(
539  indent + "free_v[nfree++] = current.vertex[{}];".format(pt), file=file
540  )
541  else:
542  assert "test" in order and "true" in order and "false" in order
543  check = checks[order["test"]]
544  check_hr = checks_hr[order["test"]]
545  printComments(order, indent, file)
546  nextTests_t = curTests + [
547  "a" + str(order["test"] + 1),
548  ]
549  nextTests_f = curTests + [
550  "!a" + str(order["test"] + 1),
551  ]
552  if order["true"] is None:
553  if order["false"] is None:
554  print(
555  indent
556  + """assert(false && "Case {} should never happen.");""".format(
557  check_hr
558  )
559  )
560  else:
561  print(
562  indent
563  + "assert(!({} <= 0)); // Not {} / {}".format(
564  check, check_hr, ".".join(nextTests_f)
565  ),
566  file=file,
567  )
568  printOrder(
569  order["false"],
570  indent=indent,
571  start=False,
572  file=file,
573  curTests=nextTests_f,
574  )
575  elif order["false"] is None:
576  print(
577  indent
578  + "assert({} <= 0); // {} / {}".format(
579  check, check_hr, ".".join(nextTests_t)
580  ),
581  file=file,
582  )
583  printOrder(
584  order["true"],
585  indent=indent,
586  start=False,
587  file=file,
588  curTests=nextTests_t,
589  )
590  else:
591  print(
592  indent
593  + "if ({} <= 0) {{ // if {} / {}".format(
594  check, check_hr, ".".join(nextTests_t)
595  ),
596  file=file,
597  )
598  printOrder(
599  order["true"],
600  indent=indent + " ",
601  start=False,
602  file=file,
603  curTests=nextTests_t,
604  )
605  print(
606  indent
607  + "}} else {{ // not {} / {}".format(check_hr, ".".join(nextTests_f)),
608  file=file,
609  )
610  printOrder(
611  order["false"],
612  indent=indent + " ",
613  start=False,
614  file=file,
615  curTests=nextTests_f,
616  )
617  print(indent + "}} // end of {}".format(check_hr), file=file)
618 
619  if start:
620  print("", file=file)
621  print("#undef REGION_INSIDE", file=file)
622  print(indent + "return false;", file=file)
623  print("}", file=file)
624 
625 
626 def unit_tests():
627  # a4, a5, a10, a11, a12
628  cases = list(range(len(regions)))
629  pdb.set_trace()
630  left_cases = apply_test_values(
631  cases,
632  test_values=[
633  None,
634  None,
635  None,
636  True,
637  True,
638  None,
639  None,
640  None,
641  None,
642  True,
643  True,
644  True,
645  ],
646  )
647  assert len(left_cases) > 1
648 
649 
650 # unit_tests()
651 
652 score, order = max_number_of_tests(tests, cases)
653 
654 print(score)
655 printOrder(order, indent=" ")
656 
657 # TODO add weights such that:
658 # - it is preferred to have all the use of one check in one branch.
659 # idea: ponderate by the number of remaining tests.
doxygen_xml_parser.index
index
Definition: doxygen_xml_parser.py:886
str
const char * str()
Definition: doxygen_xml_parser.py:882


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:13