Fix bug in replaceSimplex3 in nativeccd.

PiperOrigin-RevId: 746014344
Change-Id: I93b32ab5f67ee48ab72238de53f7cda07638cb79

src/engine/engine_collision_gjk.c

@@ -834,22 +834,16 @@ static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) {
 
 // replace a 3-simplex with one of its faces
 static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, int v2, int v3) {
+  // reset status simplex
   status->nsimplex = 3;
-  Vertex* v = pt->verts;
+  status->simplex[0] = pt->verts[v1];
-  copy3(status->simplex[0].vert1, v[v1].vert1);
+  status->simplex[1] = pt->verts[v2];
-  copy3(status->simplex[1].vert1, v[v2].vert1);
+  status->simplex[2] = pt->verts[v3];
-  copy3(status->simplex[2].vert1, v[v3].vert1);
-
-  copy3(status->simplex[0].vert2, v[v1].vert2);
-  copy3(status->simplex[1].vert2, v[v2].vert2);
-  copy3(status->simplex[2].vert2, v[v3].vert2);
-
-  copy3(status->simplex[0].vert, v[v1].vert);
-  copy3(status->simplex[1].vert, v[v2].vert);
-  copy3(status->simplex[2].vert, v[v3].vert);
 
+  // reset polytope
   pt->nfaces = 0;
   pt->nverts = 0;
+  pt->nmap = 0;
 }
 
 

test/engine/engine_collision_gjk_test.cc

@@ -1152,6 +1152,74 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="geom1" type="box" pos="0 0 0" size="0.02 0.02 0.02"/>
+      <geom name="geom2" type="box" pos="0 0 0" size="0.02 0.02 0.02"/>
+    </worldbody>
+  </mujoco>)";
+
+  std::array<char, 1000> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  mjtNum* xpos = data->geom_xpos;
+  mjtNum* xmat = data->geom_xmat;
+
+  xmat[0] = 1.0000000000000000000000000000000000000000;
+  xmat[1] = -0.0000000000000000000000000000001366192847;
+  xmat[2] = -0.0000000000000002451235402041944571528177;
+  xmat[3] = 0.0000000000000000000000000000001366707863;
+  xmat[4] = 1.0000000000000000000000000000000000000000;
+  xmat[5] = 0.0000000000000000002101047324941978855126;
+  xmat[6] = 0.0000000000000002451235402041944571528177;
+  xmat[7] = -0.0000000000000000002101047324941978855126;
+  xmat[8] = 1.0000000000000000000000000000000000000000;
+
+  xpos[0] = -0.1000000000000000055511151231257827021182;
+  xpos[1] = -0.2000000000000000111022302462515654042363;
+  xpos[2] = -0.0809921810760001470441693527391180396080;
+
+  xpos = data->geom_xpos + 3;
+  xmat = data->geom_xmat + 9;
+
+  xmat[0] = 1.0000000000000000000000000000000000000000;
+  xmat[1] = -0.0000000000000000000000000000000740327228;
+  xmat[2] = -0.0000000000000002557259745463766308177658;
+  xmat[3] = 0.0000000000000000000000000000000775428823;
+  xmat[4] = 1.0000000000000000000000000000000000000000;
+  xmat[5] = 0.0000000000000000137262533997081760161613;
+  xmat[6] = 0.0000000000000002557259745463766308177658;
+  xmat[7] = -0.0000000000000000137262533997081760161613;
+  xmat[8] = 1.0000000000000000000000000000000000000000;
+
+  xpos[0] = -0.1000000000000000055511151231257827021182;
+  xpos[1] = -0.2000000000000000111022302462515654042363;
+  xpos[2] = -0.0418396695286432432348000531874276930466;
+
+  int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+  int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+
+  mjCCDStatus status;
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
+
+  EXPECT_EQ(ncons, 4);
+
+  EXPECT_NEAR(dir[0], 0, kTolerance);
+  EXPECT_NEAR(dir[1], 0, kTolerance);
+  EXPECT_NEAR(dir[2], 1, kTolerance);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 TEST_F(MjGjkTest, SmallBoxMesh) {
   static constexpr char xml[] = R"(
   <mujoco>

test/engine/testdata/collision_convex/dice.xml

@@ -25,7 +25,7 @@
   <worldbody>
     <light pos="0 0 1" dir="0 0 -1" castshadow="false"/>
      <body name="container" pos="0 0 -0.11">
-      <geom class="floor" size="0.27 0.27 0.01"/>>
+      <geom class="floor" size="0.27 0.27 0.01"/>
       <geom class="wall"  size="0.26 0.01 0.10" pos="0.01 0.26 0.11"/>
       <geom class="wall"  size="0.26 0.01 0.10" pos="-0.01 -0.26 0.11"/>
       <geom class="wall"  size="0.01 0.26 0.10" pos="0.26 -0.01 0.11"/>
@@ -37,7 +37,7 @@
         <replicate count="4" offset="0 0 .1">
           <body pos="-.2 -.2 0.15">
             <freejoint/>
-            <geom size=".02 .02 .02" type="mesh" mesh="die"/>
+            <geom type="mesh" mesh="die"/>
           </body>
         </replicate>
       </replicate>

test/engine/testdata/collision_convex/dice_boxmesh.xml

@@ -0,0 +1,47 @@
+<mujoco>
+  <option>
+    <flag multiccd="enable"/>
+  </option>
+
+  <visual>
+    <scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
+    <map force="0.1"/>
+  </visual>
+
+  <asset>
+    <mesh name="boxmesh" scale=".02 .02 .02"
+          vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"/>
+  </asset>
+
+  <default>
+    <geom condim="1" type="mesh" solref="2e-5 1"/>
+    <default class="wall">
+      <geom type="box" rgba="0.1 0.2 1 .2"/>
+    </default>
+    <default class="floor">
+      <geom type="box" rgba="0.1 0.2 1 1"/>
+    </default>
+  </default>
+
+  <worldbody>
+    <light pos="0 0 1" dir="0 0 -1" castshadow="false"/>
+     <body name="container" pos="0 0 -0.11">
+      <geom class="floor" size="0.27 0.27 0.01"/>
+      <geom class="wall"  size="0.26 0.01 0.10" pos="0.01 0.26 0.11"/>
+      <geom class="wall"  size="0.26 0.01 0.10" pos="-0.01 -0.26 0.11"/>
+      <geom class="wall"  size="0.01 0.26 0.10" pos="0.26 -0.01 0.11"/>
+      <geom class="wall"  size="0.01 0.26 0.10" pos="-0.26 0.01 0.11"/>
+    </body>
+
+    <replicate count="5" offset=".1 0 0">
+      <replicate count="5" offset="0 .1 0">
+        <replicate count="4" offset="0 0 .1">
+          <body pos="-.2 -.2 0.15">
+            <freejoint/>
+            <geom type="mesh" mesh="boxmesh"/>
+          </body>
+        </replicate>
+      </replicate>
+    </replicate>
+  </worldbody>
+</mujoco>