Add box-triangle projection tests to octree construction.

Number of octree nodes for cow example:
Before change 45217
After change 28921

PiperOrigin-RevId: 783748606
Change-Id: I278ac7bc969dc1463d6dd0f7cdc8b267b7ce6c92

Author: quagla

src/user/user_objects.cc

@@ -597,35 +597,59 @@ void mjCOctree::CreateOctree(const double aamm[6]) {
 }
 
 
-static bool boxTriangle(const Triangle& element, const double aamm[6]) {
+static double dot2(const double* a, const double* b) {
+  return a[0] * b[0] + a[1] * b[1];
+}
+
+
+// From M. Schwarz and H.-P. Seidel, "Fast Parallel Surface and Solid Voxelization on GPUs".
+static bool boxTriangle(const Triangle& v, const double aamm[6]) {
   // bounding box tests
   for (int i = 0; i < 3; i++) {
-    if (element[0][i] < aamm[i] && element[1][i] < aamm[i] && element[2][i] < aamm[i]) {
+    if (v[0][i] < aamm[i] && v[1][i] < aamm[i] && v[2][i] < aamm[i]) {
       return false;
     }
     int j = i + 3;
-    if (element[0][i] > aamm[j] && element[1][i] > aamm[j] && element[2][i] > aamm[j]) {
+    if (v[0][i] > aamm[j] && v[1][i] > aamm[j] && v[2][i] > aamm[j]) {
       return false;
     }
   }
 
   // test for triangle plane and box overlap
   double n[3];
-  double v0[3] = {element[0][0], element[0][1], element[0][2]};
-  double e1[3] = {element[1][0] - v0[0], element[1][1] - v0[1], element[1][2] - v0[2]};
-  double e2[3] = {element[2][0] - v0[0], element[2][1] - v0[1], element[2][2] - v0[2]};
+  double e[3][3];
+  for (int i = 0; i < 3; i++) {
+    for (int j = 0; j < 3; j++) {
+      e[i][j] = v[(i+1)%3][j] - v[i][j];
+    }
+  }
 
-  mjuu_crossvec(n, e1, e2);
+  mjuu_crossvec(n, e[0], e[1]);
   double size[3] = {aamm[3] - aamm[0], aamm[4] - aamm[1], aamm[5] - aamm[2]};
   double c[3] = {n[0] > 0 ? size[0] : 0, n[1] > 0 ? size[1] : 0, n[2] > 0 ? size[2] : 0};
-  double c1[3] = {c[0] - v0[0], c[1] - v0[1], c[2] - v0[2]};
-  double c2[3] = {size[0] - c[0] - v0[0], size[1] - c[1] - v0[1], size[2] - c[2] - v0[2]};
+  double c1[3] = {c[0] - v[0][0], c[1] - v[0][1], c[2] - v[0][2]};
+  double c2[3] = {size[0] - c[0] - v[0][0], size[1] - c[1] - v[0][1], size[2] - c[2] - v[0][2]};
 
   if ((mjuu_dot3(n, aamm) + mjuu_dot3(n, c1)) * (mjuu_dot3(n, aamm) + mjuu_dot3(n, c2)) > 0) {
     return false;
   }
 
-  // TODO: add additionally separating axis tests
+  // test projection overlap
+  for (int a = 0; a < 3; a++) {
+    int b = (a + 1) % 3;
+    int c = (a + 2) % 3;
+    for (int i = 0; i < 3; i++) {
+      double sign = n[a] >= 0 ? 1 : -1;
+      double ne[2] = {-e[i][c] * sign, -e[i][b] * sign};
+      double vi[2] = {v[i][b], v[i][c]};
+      double d = -dot2(ne, vi) + mju_max(0, size[b]*ne[0]) + mju_max(0, size[c]*ne[1]);
+      double p[2] = {aamm[b], aamm[c]};
+      if (dot2(ne, p) + d < 0) {
+        return false;
+      }
+    }
+  }
+
   return true;
 }