From c5bf1bf6f8e26aa8115af2a22ed290665485202d Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Sun, 1 Sep 2019 19:10:22 +0200
Subject: [PATCH] [GJK] Update GJK::projectTetrahedraOrigin
---
doc/gjk.py | 290 +++++++++----
src/narrowphase/gjk.cpp | 902 +++++++++++++++-------------------------
2 files changed, 547 insertions(+), 645 deletions(-)
diff --git a/doc/gjk.py b/doc/gjk.py
index b72b3e7d..a7f172af 100644
--- a/doc/gjk.py
+++ b/doc/gjk.py
@@ -1,19 +1,47 @@
#!/usr/bin/env python3
import pdb
+import sys
-checks = [ "AB", "AC", "AD" ] \
- + [ "ABC", "ACD", "ADB" ] \
- + [ "ABC.cross({})".format(n) for n in [ "AB", "AC" ] ] \
- + [ "ACD.cross({})".format(n) for n in [ "AC", "AD" ] ] \
- + [ "ADB.cross({})".format(n) for n in [ "AD", "AB" ] ]
+#Â 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
-tests = list(range(len(checks)))
+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"
-weights = [ 1, ] * 3 + [ 2, ] * 3 + [ 3, ] * 6
+# 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 ],
@@ -24,6 +52,7 @@ definitions = [
[-10,-11,-12 ],
[- 1,- 2,- 3 ],
]
+# conditions is a list of (list of (list of check IDs to be ANDed) to be ORed).
conditions = [
[],
[],
@@ -32,12 +61,13 @@ conditions = [
[],
[],
[],
- [ [10, 11, 12], ],
+ [ ], #[ [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], ],
@@ -45,6 +75,59 @@ rejections = [
[],
]
+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
+
def apply_test_values (cases, test_values):
def canSatisfy (values, indices):
for k in indices:
@@ -57,7 +140,7 @@ def apply_test_values (cases, test_values):
if k < 0 and values[-k-1] != False: return False
return True
- # Check whether all cases are
+ # Check all cases.
left_cases = []
for case in cases:
defi = definitions[case]
@@ -90,33 +173,44 @@ def max_number_of_tests (current_tests, cases, test_values = [None,]*len(tests),
if len(left_cases) == 1:
return prevScore, { 'case': left_cases[0], }
elif len(left_cases) == 0:
- return prevScore, { }
+ 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"
- remaining_tests = current_tests[1:]
currentBestScore = prevBestScore
bestScore = float('inf')
bestOrder = [None, None]
for i, test in enumerate(current_tests):
assert bestScore >= currentBestScore
- if i > 0:
- remaining_tests[i-1] = current_tests[i-1]
-
currentScore = prevScore + len(left_cases) * weights[test]
+ #currentScore = prevScore + weights[test]
if currentScore > currentBestScore: # Cannot do better -> stop
continue
- test_values[test] = True
- score_if_t, order_if_t = max_number_of_tests (remaining_tests, left_cases, test_values, currentBestScore, currentScore)
- if score_if_t >= currentBestScore: # True didn't do better -> stop
- test_values[test] = None
- continue
+ try:
+ remaining_tests, next_test_values = set_test_values (current_tests, test_values, test, True)
+ except ValueError:
+ remaining_tests = None
- test_values[test] = False
- score_if_f, order_if_f = max_number_of_tests (remaining_tests, left_cases, test_values, currentBestScore, currentScore)
- test_values[test] = 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:
@@ -130,80 +224,114 @@ def max_number_of_tests (current_tests, cases, test_values = [None,]*len(tests),
return bestScore, bestOrder
-def printOrder (order, indent = "", start=True):
+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):
if start:
- print (indent+"// The code of this function was generated using doc/gjk.py")
- print (indent+"const int a = 3, b = 2, c = 1, d = 0;")
+ 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)
for l in "abcd":
- print (indent+"const Vec3f& {} (current.vertex[{}]->w);".format(l.upper(),l))
- for l in "BCD":
- print (indent+"const Vec3f A{0} ({0}-A);".format(l))
- print (indent+"const FCL_REAL A{0}_dot_AO = A{0}.dot(-A);".format(l))
- for l1,l2 in zip("BCD","CDB"):
- print (indent+"const Vec3f A{0}{1} (A{0}.cross(A{1}));".format(l1,l2))
- print (indent+"const FCL_REAL A{0}{1}_dot_AO = A{0}{1}.dot(-A);".format(l1,l2))
- print (indent+"Vec3f cross;")
- print()
- print( "#define REGION_INSIDE() "+indent+"\\")
- print(indent+" ray.setZero(); \\")
- print(indent+" next.vertex[0] = current.vertex[d]; \\")
- print(indent+" next.vertex[1] = current.vertex[c]; \\")
- print(indent+" next.vertex[2] = current.vertex[b]; \\")
- print(indent+" next.vertex[3] = current.vertex[a]; \\")
- print(indent+" next.rank=4; \\")
- print(indent+" return 0;")
- print()
+ 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:
- region = regions[order['case']]
- print (indent + "// Region " + region)
- toFree = ['a', 'b', 'c', 'd']
+ 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()")
+ print (indent + "REGION_INSIDE()", file=file)
toFree = []
elif region == 'A':
- print (indent + "originToPoint (current, a, A, next, ray);")
- toFree.remove('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, {}, {}, {}, {}, {}, {}_dot_AO, next, ray);".format(
- a.lower(), b.lower(), a, b, region, region))
- toFree.remove(a.lower())
- toFree.remove(b.lower())
+ 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:
- a = region[0]
- b = region[1]
- c = region[2]
- print (indent + "originToTriangle (current, {}, {}, {}, {}, {}_dot_AO next, ray);".format(
- a.lower(), b.lower(), c.lower(), region, region))
- toFree.remove(a.lower())
- toFree.remove(b.lower())
- toFree.remove(c.lower())
+ 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))
+ 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']]
- if 'cross' in check:
- print (indent + "cross.noalias() = {};".format(checks[order['test']]))
- print (indent + "if (cross.dot(-A) >= 0) {")
- elif len(check) == 2 or len(check) == 3:
- print (indent + "if ({}_dot_AO >= 0) {{".format(checks[order['test']]))
+ 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)
else:
- assert False, "Unknown check " + check
- #print (indent + "if ({}.dot(-A) >= 0) {{".format(checks[order['test']]))
- printOrder (order['true'], indent=indent+" ", start=False)
- print (indent + "} else {")
- printOrder (order['false'], indent=indent+" ", start=False)
- print (indent + "}")
+ 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)
if start:
- print()
- print("#undef REGION_INSIDE")
- print(indent+"return ray.squaredNorm();")
+ 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)
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index d83803e7..149df2d5 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -688,44 +688,33 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)
{
+ // The code of this function was generated using doc/gjk.py
const int a = 3, b = 2, c = 1, d = 0;
const Vec3f& A (current.vertex[a]->w);
const Vec3f& B (current.vertex[b]->w);
const Vec3f& C (current.vertex[c]->w);
const Vec3f& D (current.vertex[d]->w);
- const Vec3f AB (B-A);
- const FCL_REAL AB_dot_AO = AB.dot(-A);
- const Vec3f AC (C-A);
- const FCL_REAL AC_dot_AO = AC.dot(-A);
- const Vec3f AD (D-A);
- const FCL_REAL AD_dot_AO = AD.dot(-A);
- const Vec3f ABC (AB.cross(AC));
- const FCL_REAL ABC_dot_AO = ABC.dot(-A);
- const Vec3f ACD (AC.cross(AD));
- const FCL_REAL ACD_dot_AO = ACD.dot(-A);
- const Vec3f ADB (AD.cross(AB));
- const FCL_REAL ADB_dot_AO = ADB.dot(-A);
- Vec3f cross;
-
-
-#ifndef NDEBUG
- Vec3f BC (C-B),
- CD (D-C),
- BD (D-B),
- BCD ((C-B).cross(D-B));
- FCL_REAL t = BCD.dot(-AB);
- assert (t >= 0);
- t = BCD.dot(-B);
- assert (t >= 0);
- t = BCD.dot(-ray);
- assert (t >= 0);
-
- // We necessarily come from the triangle case with the closest point lying on
- // the triangle. Hence the assertions below.
- assert (BCD.cross(-BC).dot (B) >= 0);
- assert (BCD.cross(-CD).dot (C) >= 0);
- assert (BCD.cross( BD).dot (D) >= 0);
-#endif
+ const FCL_REAL aa = A.squaredNorm();
+ const FCL_REAL da = D.dot(A);
+ const FCL_REAL db = D.dot(B);
+ const FCL_REAL dc = D.dot(C);
+ const FCL_REAL dd = D.dot(D);
+ const FCL_REAL da_aa = da - aa;
+ const FCL_REAL ca = C.dot(A);
+ const FCL_REAL cb = C.dot(B);
+ const FCL_REAL cc = C.dot(C);
+ const FCL_REAL& cd = dc;
+ const FCL_REAL ca_aa = ca - aa;
+ const FCL_REAL ba = B.dot(A);
+ const FCL_REAL bb = B.dot(B);
+ const FCL_REAL& bc = cb;
+ const FCL_REAL& bd = db;
+ const FCL_REAL ba_aa = ba - aa;
+ const FCL_REAL ba_ca = ba - ca;
+ const FCL_REAL ca_da = ca - da;
+ const FCL_REAL da_ba = da - ba;
+ const Vec3f a_cross_b = A.cross(B);
+ const Vec3f a_cross_c = A.cross(C);
#define REGION_INSIDE() \
ray.setZero(); \
@@ -736,599 +725,384 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)
next.rank=4; \
return true;
- if (AB_dot_AO >= 0) {
- cross.noalias() = ABC.cross(AB);
- if (cross.dot(-A) >= 0) {
- cross.noalias() = ABC.cross(AC);
- if (cross.dot(-A) >= 0) {
- cross.noalias() = ACD.cross(AD);
- if (cross.dot(-A) >= 0) {
- if (AC_dot_AO >= 0) {
- cross.noalias() = ACD.cross(AC);
- if (cross.dot(-A) >= 0) {
- if (AD_dot_AO >= 0) {
- // Region Inside
- REGION_INSIDE()
- } else {
- if (ADB_dot_AO >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region Inside
- REGION_INSIDE()
- }
- }
- } else {
- // Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[d];
- }
- } else {
- if (ADB_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (ACD_dot_AO >= 0) {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region Inside
- REGION_INSIDE()
- }
- }
- }
- } else {
- if (ADB_dot_AO >= 0) {
- cross.noalias() = ACD.cross(AC);
- if (cross.dot(-A) >= 0) {
- if (ACD_dot_AO >= 0) {
+ if (ba_aa <= 0) { // if AB.AO >= 0
+ if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0
+ if (ba * da_ba + bd * ba_aa - bb * da_aa <= 0) { // if (ADB ^ AB).AO >= 0
+ if (da_aa <= 0) { // if AD.AO >= 0
+ assert(da * da_ba + dd * ba_aa - db * da_aa <= 0); // (ADB ^ AD).AO >= 0
+ if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0
+ // Region ABC
+ originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray);
+ free_v[nfree++] = current.vertex[d];
+ } else { // not (ABC ^ AB).AO >= 0
+ // Region AB
+ originToSegment (current, a, b, A, B, B-A, -ba_aa, next, ray);
+ free_v[nfree++] = current.vertex[c];
+ free_v[nfree++] = current.vertex[d];
+ } // end of (ABC ^ AB).AO >= 0
+ } else { // not AD.AO >= 0
+ if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0
+ if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0
+ if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0
// Region ACD
- originToTriangle (current, a, c, d, ACD, ACD_dot_AO, next, ray);
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
free_v[nfree++] = current.vertex[b];
- } else {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (AC_dot_AO >= 0) {
- // Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[d];
- } else {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- }
- }
- } else {
- if (ACD_dot_AO >= 0) {
- cross.noalias() = ACD.cross(AC);
- if (cross.dot(-A) >= 0) {
- // Region ACD
- originToTriangle (current, a, c, d, ACD, ACD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- } else {
- // Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[d];
- }
- } else {
- if (ABC_dot_AO >= 0) {
+ } else { // not (ACD ^ AC).AO >= 0
// Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[d];
- } else {
- // Region Inside
- REGION_INSIDE()
- }
- }
- }
- }
- } else {
- if (ABC_dot_AO >= 0) {
- // Region ABC
- originToTriangle (current, a, b, c, ABC, ABC_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[d];
- } else {
- if (ACD_dot_AO >= 0) {
- cross.noalias() = ACD.cross(AD);
- if (cross.dot(-A) >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- cross.noalias() = ACD.cross(AC);
- if (cross.dot(-A) >= 0) {
- // Region ACD
- originToTriangle (current, a, c, d, ACD, ACD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- } else {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- }
- }
- } else {
- if (ADB_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
+ originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray);
free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- // Region Inside
- REGION_INSIDE()
- }
- }
- }
- }
- } else {
- cross.noalias() = ACD.cross(AC);
- if (cross.dot(-A) >= 0) {
- if (ACD_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- if (ADB_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AB);
- if (cross.dot(-A) >= 0) {
- // Region AB
- originToSegment (current, a, b, A, B, AB, AB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
free_v[nfree++] = current.vertex[d];
- } else {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- cross.noalias() = ACD.cross(AD);
- if (cross.dot(-A) >= 0) {
- // Region AB
- originToSegment (current, a, b, A, B, AB, AB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
+ } // end of (ACD ^ AC).AO >= 0
+ } else { // not (ABC ^ AC).AO >= 0
+ if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0
+ // Region ABC
+ originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray);
free_v[nfree++] = current.vertex[d];
- } else {
+ } else { // not ABC.AO >= 0
// Region ACD
- originToTriangle (current, a, c, d, ACD, ACD_dot_AO, next, ray);
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
free_v[nfree++] = current.vertex[b];
- }
- }
- } else {
- cross.noalias() = ACD.cross(AD);
- if (cross.dot(-A) >= 0) {
- if (AD_dot_AO >= 0) {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region AB
- originToSegment (current, a, b, A, B, AB, AB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- } else {
+ } // end of ABC.AO >= 0
+ } // end of (ABC ^ AC).AO >= 0
+ } else { // not (ABC ^ AB).AO >= 0
+ // Region AB
+ originToSegment (current, a, b, A, B, B-A, -ba_aa, next, ray);
+ free_v[nfree++] = current.vertex[c];
+ free_v[nfree++] = current.vertex[d];
+ } // end of (ABC ^ AB).AO >= 0
+ } // end of AD.AO >= 0
+ } else { // not (ADB ^ AB).AO >= 0
+ if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0
+ // Region ADB
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
+ free_v[nfree++] = current.vertex[c];
+ } else { // not (ADB ^ AD).AO >= 0
+ if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0
+ if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[c];
+ } else { // not (ACD ^ AD).AO >= 0
// Region ACD
- originToTriangle (current, a, c, d, ACD, ACD_dot_AO, next, ray);
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
free_v[nfree++] = current.vertex[b];
- }
- }
- } else {
- if (ADB_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- cross.noalias() = ADB.cross(AB);
- if (cross.dot(-A) >= 0) {
+ } // end of (ACD ^ AD).AO >= 0
+ } else { // not (ACD ^ AC).AO >= 0
+ if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[c];
+ } else { // not (ACD ^ AD).AO >= 0
+ // Region AC
+ originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[d];
+ } // end of (ACD ^ AD).AO >= 0
+ } // end of (ACD ^ AC).AO >= 0
+ } // end of (ADB ^ AD).AO >= 0
+ } // end of (ADB ^ AB).AO >= 0
+ } else { // not ADB.AO >= 0
+ if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0
+ if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0
+ if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0
+ if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0
+ // Region ACD
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
+ free_v[nfree++] = current.vertex[b];
+ } else { // not (ACD ^ AC).AO >= 0
+ // Region AC
+ originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[d];
+ } // end of (ACD ^ AC).AO >= 0
+ } else { // not (ABC ^ AC).AO >= 0
+ // Region ABC
+ originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray);
+ free_v[nfree++] = current.vertex[d];
+ } // end of (ABC ^ AC).AO >= 0
+ } else { // not (ABC ^ AB).AO >= 0
+ if (ca_aa <= 0) { // if AC.AO >= 0
+ assert(!(ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0)); // Not (ABC ^ AC).AO >= 0
+ if (ba * da_ba + bd * ba_aa - bb * da_aa <= 0) { // if (ADB ^ AB).AO >= 0
+ // Region AB
+ originToSegment (current, a, b, A, B, B-A, -ba_aa, next, ray);
+ free_v[nfree++] = current.vertex[c];
+ free_v[nfree++] = current.vertex[d];
+ } else { // not (ADB ^ AB).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[c];
+ } // end of (ADB ^ AB).AO >= 0
+ } else { // not AC.AO >= 0
+ if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0
+ if (ba * da_ba + bd * ba_aa - bb * da_aa <= 0) { // if (ADB ^ AB).AO >= 0
// Region AB
- originToSegment (current, a, b, A, B, AB, AB_dot_AO, next, ray);
+ originToSegment (current, a, b, A, B, B-A, -ba_aa, next, ray);
free_v[nfree++] = current.vertex[c];
free_v[nfree++] = current.vertex[d];
- } else {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
+ } else { // not (ADB ^ AB).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[c];
- }
- } else {
- cross.noalias() = ADB.cross(AB);
- if (cross.dot(-A) >= 0) {
+ } // end of (ADB ^ AB).AO >= 0
+ } else { // not (ACD ^ AD).AO >= 0
+ if (ba * da_ba + bd * ba_aa - bb * da_aa <= 0) { // if (ADB ^ AB).AO >= 0
// Region AB
- originToSegment (current, a, b, A, B, AB, AB_dot_AO, next, ray);
+ originToSegment (current, a, b, A, B, B-A, -ba_aa, next, ray);
free_v[nfree++] = current.vertex[c];
free_v[nfree++] = current.vertex[d];
- } else {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
+ } else { // not (ADB ^ AB).AO >= 0
+ // Region ACD
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- }
- }
- } else {
- if (ABC_dot_AO >= 0) {
- // Region AB
- originToSegment (current, a, b, A, B, AB, AB_dot_AO, next, ray);
+ } // end of (ADB ^ AB).AO >= 0
+ } // end of (ACD ^ AD).AO >= 0
+ } // end of AC.AO >= 0
+ } // end of (ABC ^ AB).AO >= 0
+ } else { // not ABC.AO >= 0
+ if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0
+ if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0
+ if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- } else {
- // Region Inside
- REGION_INSIDE()
- }
- }
- }
- } else {
- cross.noalias() = ADB.cross(AB);
- if (cross.dot(-A) >= 0) {
- // Region AB
- originToSegment (current, a, b, A, B, AB, AB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- } else {
- if (AC_dot_AO >= 0) {
- cross.noalias() = ABC.cross(AC);
- if (cross.dot(-A) >= 0) {
- // Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
+ } else { // not (ACD ^ AD).AO >= 0
+ // Region ACD
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[d];
- } else {
- if (AD_dot_AO >= 0) {
- // Region Inside
- REGION_INSIDE()
- } else {
- if (ADB_dot_AO >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region Inside
- REGION_INSIDE()
- }
- }
- }
- } else {
- if (ADB_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
+ } // end of (ACD ^ AD).AO >= 0
+ } else { // not (ACD ^ AC).AO >= 0
+ if (ca_aa <= 0) { // if AC.AO >= 0
+ if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0
+ // Region AC
+ originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray);
free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (ACD_dot_AO >= 0) {
+ free_v[nfree++] = current.vertex[d];
+ } else { // not (ABC ^ AC).AO >= 0
// Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[c];
- } else {
- // Region Inside
- REGION_INSIDE()
- }
- }
- }
- }
- }
- }
- } else {
- if (ABC_dot_AO >= 0) {
- cross.noalias() = ABC.cross(AC);
- if (cross.dot(-A) >= 0) {
- cross.noalias() = ACD.cross(AC);
- if (cross.dot(-A) >= 0) {
- cross.noalias() = ACD.cross(AD);
- if (cross.dot(-A) >= 0) {
- if (AD_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
+ } // end of (ABC ^ AC).AO >= 0
+ } else { // not AC.AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[c];
+ } // end of AC.AO >= 0
+ } // end of (ACD ^ AC).AO >= 0
+ } else { // not ACD.AO >= 0
+ // Region Inside
+ REGION_INSIDE()
+ } // end of ACD.AO >= 0
+ } // end of ABC.AO >= 0
+ } // end of ADB.AO >= 0
+ } else { // not AB.AO >= 0
+ if (ca_aa <= 0) { // if AC.AO >= 0
+ if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0
+ if (da_aa <= 0) { // if AD.AO >= 0
+ if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0
+ if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0
+ if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0
// Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
free_v[nfree++] = current.vertex[c];
- } else {
+ } else { // not (ADB ^ AD).AO >= 0
// Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (AC_dot_AO >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- } else {
- if (ACD_dot_AO >= 0) {
+ } // end of (ADB ^ AD).AO >= 0
+ } else { // not (ACD ^ AD).AO >= 0
// Region ACD
- originToTriangle (current, a, c, d, ACD, ACD_dot_AO, next, ray);
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
free_v[nfree++] = current.vertex[b];
- } else {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- }
- }
- } else {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- if (AC_dot_AO >= 0) {
+ } // end of (ACD ^ AD).AO >= 0
+ } else { // not (ACD ^ AC).AO >= 0
+ assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= 0)); // Not (ACD ^ AD).AO >= 0
+ if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0
// Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
+ originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray);
free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[d];
- } else {
- if (AD_dot_AO >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- } else {
- if (AC_dot_AO >= 0) {
+ } else { // not (ABC ^ AC).AO >= 0
+ // Region ABC
+ originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray);
+ free_v[nfree++] = current.vertex[d];
+ } // end of (ABC ^ AC).AO >= 0
+ } // end of (ACD ^ AC).AO >= 0
+ } else { // not AD.AO >= 0
+ if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0
+ if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0
+ assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= 0)); // Not (ACD ^ AD).AO >= 0
+ // Region ACD
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
+ free_v[nfree++] = current.vertex[b];
+ } else { // not (ACD ^ AC).AO >= 0
// Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
+ originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray);
free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[d];
- } else {
- if (AD_dot_AO >= 0) {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- }
- }
- } else {
- cross.noalias() = ABC.cross(AB);
- if (cross.dot(-A) >= 0) {
- // Region ABC
- originToTriangle (current, a, b, c, ABC, ABC_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[d];
- } else {
- cross.noalias() = ACD.cross(AD);
- if (cross.dot(-A) >= 0) {
- if (AD_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (AC_dot_AO >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- } else {
- cross.noalias() = ACD.cross(AC);
- if (cross.dot(-A) >= 0) {
- if (ACD_dot_AO >= 0) {
+ } // end of (ACD ^ AC).AO >= 0
+ } else { // not (ABC ^ AC).AO >= 0
+ if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0
+ assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0); // (ABC ^ AB).AO >= 0
+ // Region ABC
+ originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray);
+ free_v[nfree++] = current.vertex[d];
+ } else { // not ABC.AO >= 0
+ if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0
+ assert(!(da * ca_da + dc * da_aa - dd * ca_aa <= 0)); // Not (ACD ^ AD).AO >= 0
// Region ACD
- originToTriangle (current, a, c, d, ACD, ACD_dot_AO, next, ray);
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
free_v[nfree++] = current.vertex[b];
- } else {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (AC_dot_AO >= 0) {
+ } else { // not (ACD ^ AC).AO >= 0
// Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
free_v[nfree++] = current.vertex[c];
- } else {
- if (AD_dot_AO >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- }
- }
- }
- }
- } else {
- if (ACD_dot_AO >= 0) {
- cross.noalias() = ACD.cross(AC);
- if (cross.dot(-A) >= 0) {
- cross.noalias() = ACD.cross(AD);
- if (cross.dot(-A) >= 0) {
- if (AD_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
+ } // end of (ACD ^ AC).AO >= 0
+ } // end of ABC.AO >= 0
+ } // end of (ABC ^ AC).AO >= 0
+ } // end of AD.AO >= 0
+ } else { // not ACD.AO >= 0
+ if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0
+ if (ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0) { // if (ABC ^ AC).AO >= 0
+ if (ca * ca_da + cc * da_aa - cd * ca_aa <= 0) { // if (ACD ^ AC).AO >= 0
+ if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0
// Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
free_v[nfree++] = current.vertex[c];
- } else {
+ } else { // not (ADB ^ AD).AO >= 0
// Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (AC_dot_AO >= 0) {
+ } // end of (ADB ^ AD).AO >= 0
+ } else { // not (ACD ^ AC).AO >= 0
+ // Region AC
+ originToSegment (current, a, c, A, C, C-A, -ca_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[d];
+ } // end of (ACD ^ AC).AO >= 0
+ } else { // not (ABC ^ AC).AO >= 0
+ assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0); // (ABC ^ AB).AO >= 0
+ // Region ABC
+ originToTriangle (current, a, b, c, (B-A).cross(C-A), -C.dot (a_cross_b), next, ray);
+ free_v[nfree++] = current.vertex[d];
+ } // end of (ABC ^ AC).AO >= 0
+ } else { // not ABC.AO >= 0
+ if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0
+ if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0
+ // Region ADB
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
+ free_v[nfree++] = current.vertex[c];
+ } else { // not (ADB ^ AD).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[c];
+ } // end of (ADB ^ AD).AO >= 0
+ } else { // not ADB.AO >= 0
+ // Region Inside
+ REGION_INSIDE()
+ } // end of ADB.AO >= 0
+ } // end of ABC.AO >= 0
+ } // end of ACD.AO >= 0
+ } else { // not AC.AO >= 0
+ if (da_aa <= 0) { // if AD.AO >= 0
+ if (C.dot (a_cross_b) <= 0) { // if ABC.AO >= 0
+ if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0
+ if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0
+ assert(ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0); // (ABC ^ AC).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[c];
+ } else { // not (ABC ^ AB).AO >= 0
+ if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0
// Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
+ } else { // not (ADB ^ AD).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- } else {
- // Region ACD
- originToTriangle (current, a, c, d, ACD, ACD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- }
- } else {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- if (AC_dot_AO >= 0) {
- cross.noalias() = ABC.cross(AC);
- if (cross.dot(-A) >= 0) {
- // Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[d];
- } else {
+ } // end of (ADB ^ AD).AO >= 0
+ } // end of (ABC ^ AB).AO >= 0
+ } else { // not (ACD ^ AD).AO >= 0
+ if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0
+ assert(ca * ca_da + cc * da_aa - cd * ca_aa <= 0); // (ACD ^ AC).AO >= 0
+ // Region ACD
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
+ free_v[nfree++] = current.vertex[b];
+ } else { // not ACD.AO >= 0
+ if (ba * ba_ca + bb * ca_aa - bc * ba_aa <= 0) { // if (ABC ^ AB).AO >= 0
+ assert(ca * ba_ca + cb * ca_aa - cc * ba_aa <= 0); // (ABC ^ AC).AO >= 0
+ // There are no case corresponding to this set of tests.
+ // applied [True, False, None, True, True, None, False, None, None, False, False, True]
+ // to ABC, ADB
+ assert(false);
+ } else { // not (ABC ^ AB).AO >= 0
// Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (AD_dot_AO >= 0) {
+ } // end of (ABC ^ AB).AO >= 0
+ } // end of ACD.AO >= 0
+ } // end of (ACD ^ AD).AO >= 0
+ } else { // not ABC.AO >= 0
+ if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0
+ if (da * ca_da + dc * da_aa - dd * ca_aa <= 0) { // if (ACD ^ AD).AO >= 0
+ if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0
// Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
- free_v[nfree++] = current.vertex[b];
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- } else {
- if (AC_dot_AO >= 0) {
- cross.noalias() = ABC.cross(AC);
- if (cross.dot(-A) >= 0) {
- // Region AC
- originToSegment (current, a, c, A, C, AC, AC_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[d];
- } else {
+ } else { // not (ADB ^ AD).AO >= 0
// Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (AD_dot_AO >= 0) {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
+ } // end of (ADB ^ AD).AO >= 0
+ } else { // not (ACD ^ AD).AO >= 0
+ assert(ca * ca_da + cc * da_aa - cd * ca_aa <= 0); // (ACD ^ AC).AO >= 0
+ // Region ACD
+ originToTriangle (current, a, c, d, (C-A).cross(D-A), -D.dot(a_cross_c), next, ray);
+ free_v[nfree++] = current.vertex[b];
+ } // end of (ACD ^ AD).AO >= 0
+ } else { // not ACD.AO >= 0
+ if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0
+ if (da * da_ba + dd * ba_aa - db * da_aa <= 0) { // if (ADB ^ AD).AO >= 0
+ // Region ADB
+ originToTriangle (current, a, d, b, (D-A).cross(B-A), D.dot(a_cross_b), next, ray);
free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
+ } else { // not (ADB ^ AD).AO >= 0
+ // Region AD
+ originToSegment (current, a, d, A, D, D-A, -da_aa, next, ray);
free_v[nfree++] = current.vertex[b];
free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- }
- }
- } else {
- if (ADB_dot_AO >= 0) {
- if (AD_dot_AO >= 0) {
- cross.noalias() = ADB.cross(AD);
- if (cross.dot(-A) >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region AD
- originToSegment (current, a, d, A, D, AD, AD_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- }
- } else {
- if (AC_dot_AO >= 0) {
- // Region ADB
- originToTriangle (current, a, d, b, ADB, ADB_dot_AO, next, ray);
- free_v[nfree++] = current.vertex[c];
- } else {
- // Region A
- originToPoint (current, a, A, next, ray);
- free_v[nfree++] = current.vertex[b];
- free_v[nfree++] = current.vertex[c];
- free_v[nfree++] = current.vertex[d];
- }
- }
- } else {
- // Region Inside
- REGION_INSIDE()
- }
- }
- }
- }
+ } // end of (ADB ^ AD).AO >= 0
+ } else { // not ADB.AO >= 0
+ // Region Inside
+ REGION_INSIDE()
+ } // end of ADB.AO >= 0
+ } // end of ACD.AO >= 0
+ } // end of ABC.AO >= 0
+ } else { // not AD.AO >= 0
+ // Region A
+ originToPoint (current, a, A, next, ray);
+ free_v[nfree++] = current.vertex[b];
+ free_v[nfree++] = current.vertex[c];
+ free_v[nfree++] = current.vertex[d];
+ } // end of AD.AO >= 0
+ } // end of AC.AO >= 0
+ } // end of AB.AO >= 0
#undef REGION_INSIDE
-
return false;
}
--
GitLab