Add mju_interpolate3D.

This is a more general API in preparation of a higher interpolation order

PiperOrigin-RevId: 820174741
Change-Id: I81bd52ab44e0485d72579e21d1c8a48a1b35ec3f

src/engine/engine_core_constraint.c

@@ -186,9 +186,7 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int v,
   int nb = 0;
 
   for (int i = nstart; i < nend; i++) {
-    mjtNum w = ((i-nstart)&1 ? coord[2] : 1-coord[2]) *
-               ((i-nstart)&2 ? coord[1] : 1-coord[1]) *
-               ((i-nstart)&4 ? coord[0] : 1-coord[0]);
+    mjtNum w = mju_evalBasis(coord, i-nstart, m->flex_interp[f]);
     if (w < 1e-5) {
       continue;
     }