Enable interior nodes for interpolated flex shell mode.

Previously shell mode required cellcount=1 along at least one axis. This CL adds support for cellcount > 1 in all three axes by pinning interior grid nodes to the parent body and reconstructing their positions from boundary nodes via Transfinite Interpolation (TFI). PiperOrigin-RevId: 924314800 Change-Id: I8c2438f4866dd4133feed65f535a1ab69f0c9188
2026-05-31 10:35:16 -07:00
parent 4548e81e4d
commit 91c92279d2
14 changed files with 681 additions and 39 deletions
@@ -1550,5 +1550,167 @@ TEST_F(FaceStateTest, RotationConsistencyWith3D) {
  }
 }

+// ------------------------------ Shell TFI Interpolation ----------------------
+
+using ShellTFITest = MujocoTest;
+
+// helper: set up a regular nx*ny*nz grid with positions at grid indices
+static void MakeRegularGrid(mjtNum* nodexpos, int nx, int ny, int nz) {
+  for (int i = 0; i < nx; i++) {
+    for (int j = 0; j < ny; j++) {
+      for (int k = 0; k < nz; k++) {
+        int idx = i*ny*nz + j*nz + k;
+        nodexpos[3*idx+0] = (mjtNum)i;
+        nodexpos[3*idx+1] = (mjtNum)j;
+        nodexpos[3*idx+2] = (mjtNum)k;
+      }
+    }
+  }
+}
+
+TEST_F(ShellTFITest, IdentityGrid) {
+  // 3x3x3 grid: 1 interior node at (1,1,1)
+  constexpr int nx = 3, ny = 3, nz = 3;
+  mjtNum nodexpos[3*nx*ny*nz];
+  MakeRegularGrid(nodexpos, nx, ny, nz);
+
+  // save expected interior position
+  mjtNum expected[3] = {1.0, 1.0, 1.0};
+
+  // run TFI
+  mju_shellTrackInterior(nodexpos, nx, ny, nz);
+
+  // interior node at (1,1,1) should match
+  int idx = 1*ny*nz + 1*nz + 1;
+  EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-5));
+  EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-5));
+  EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-5));
+}
+
+TEST_F(ShellTFITest, UniformScaling) {
+  // 3x3x3: scale all boundary nodes by 2x, interior should follow
+  constexpr int nx = 3, ny = 3, nz = 3;
+  mjtNum nodexpos[3*nx*ny*nz];
+  MakeRegularGrid(nodexpos, nx, ny, nz);
+
+  // scale all nodes
+  for (int i = 0; i < 3*nx*ny*nz; i++) {
+    nodexpos[i] *= 2.0;
+  }
+
+  // run TFI — interior should be reconstructed to 2*original
+  mju_shellTrackInterior(nodexpos, nx, ny, nz);
+
+  int idx = 1*ny*nz + 1*nz + 1;
+  EXPECT_NEAR(nodexpos[3*idx+0], 2.0, MjTol(1e-12, 1e-5));
+  EXPECT_NEAR(nodexpos[3*idx+1], 2.0, MjTol(1e-12, 1e-5));
+  EXPECT_NEAR(nodexpos[3*idx+2], 2.0, MjTol(1e-12, 1e-5));
+}
+
+TEST_F(ShellTFITest, AffineDeformation) {
+  // 4x4x4 grid with 8 interior nodes. Apply affine transform to boundary,
+  // then verify TFI reproduces the same affine transform on interior nodes.
+  constexpr int nx = 4, ny = 4, nz = 4;
+  mjtNum nodexpos[3*nx*ny*nz];
+  MakeRegularGrid(nodexpos, nx, ny, nz);
+
+  // affine: F(x,y,z) = A*[x,y,z]^T + b
+  // A = [[2, 0.5, 0], [0.3, 1.5, 0], [0, 0, 1]], b = [10, 20, 30]
+  auto affine = [](mjtNum x, mjtNum y, mjtNum z, mjtNum out[3]) {
+    out[0] = 2.0*x + 0.5*y + 10.0;
+    out[1] = 0.3*x + 1.5*y + 20.0;
+    out[2] = z + 30.0;
+  };
+
+  // apply affine to all nodes
+  for (int i = 0; i < nx; i++) {
+    for (int j = 0; j < ny; j++) {
+      for (int k = 0; k < nz; k++) {
+        int idx = i*ny*nz + j*nz + k;
+        affine((mjtNum)i, (mjtNum)j, (mjtNum)k, nodexpos + 3*idx);
+      }
+    }
+  }
+
+  // corrupt interior nodes to verify TFI actually reconstructs them
+  for (int i = 1; i < nx-1; i++) {
+    for (int j = 1; j < ny-1; j++) {
+      for (int k = 1; k < nz-1; k++) {
+        int idx = i*ny*nz + j*nz + k;
+        nodexpos[3*idx+0] = -999;
+        nodexpos[3*idx+1] = -999;
+        nodexpos[3*idx+2] = -999;
+      }
+    }
+  }
+
+  // run TFI
+  mju_shellTrackInterior(nodexpos, nx, ny, nz);
+
+  // check all interior nodes match affine
+  for (int i = 1; i < nx-1; i++) {
+    for (int j = 1; j < ny-1; j++) {
+      for (int k = 1; k < nz-1; k++) {
+        int idx = i*ny*nz + j*nz + k;
+        mjtNum expected[3];
+        affine((mjtNum)i, (mjtNum)j, (mjtNum)k, expected);
+        EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-4))
+            << "i=" << i << " j=" << j << " k=" << k;
+        EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-4))
+            << "i=" << i << " j=" << j << " k=" << k;
+        EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-4))
+            << "i=" << i << " j=" << j << " k=" << k;
+      }
+    }
+  }
+}
+
+TEST_F(ShellTFITest, BoundaryUnmodified) {
+  // verify that boundary nodes are not modified by TFI
+  constexpr int nx = 4, ny = 4, nz = 4;
+  mjtNum nodexpos[3*nx*ny*nz];
+  MakeRegularGrid(nodexpos, nx, ny, nz);
+
+  // save boundary node values
+  mjtNum saved[3*nx*ny*nz];
+  mju_copy(saved, nodexpos, 3*nx*ny*nz);
+
+  mju_shellTrackInterior(nodexpos, nx, ny, nz);
+
+  // check all boundary nodes unchanged
+  for (int i = 0; i < nx; i++) {
+    for (int j = 0; j < ny; j++) {
+      for (int k = 0; k < nz; k++) {
+        bool is_boundary = (i == 0 || i == nx-1 ||
+                            j == 0 || j == ny-1 ||
+                            k == 0 || k == nz-1);
+        if (is_boundary) {
+          int idx = i*ny*nz + j*nz + k;
+          EXPECT_EQ(nodexpos[3*idx+0], saved[3*idx+0]);
+          EXPECT_EQ(nodexpos[3*idx+1], saved[3*idx+1]);
+          EXPECT_EQ(nodexpos[3*idx+2], saved[3*idx+2]);
+        }
+      }
+    }
+  }
+}
+
+TEST_F(ShellTFITest, NoInteriorSmallGrid) {
+  // 2x2x2 and 2x3x2: no interior nodes, TFI should be a no-op
+  constexpr int nx = 2, ny = 3, nz = 2;
+  mjtNum nodexpos[3*nx*ny*nz];
+  MakeRegularGrid(nodexpos, nx, ny, nz);
+
+  mjtNum saved[3*nx*ny*nz];
+  mju_copy(saved, nodexpos, 3*nx*ny*nz);
+
+  mju_shellTrackInterior(nodexpos, nx, ny, nz);
+
+  // all nodes unchanged
+  for (int i = 0; i < 3*nx*ny*nz; i++) {
+    EXPECT_EQ(nodexpos[i], saved[i]);
+  }
+}
+
 }  // namespace
 }  // namespace mujoco