chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add quadratic interpolation to mju_interpolate3D.

Browse Source 
PiperOrigin-RevId: 820252349
Change-Id: I983175eb51d7e1a81da06c0911326fcb8f5fe764
This commit is contained in:
Alessio Quaglino
2025-10-16 08:47:14 -07:00
committed by Copybara-Service
parent 87adb7e5d6
commit ff7c94de8a
2 changed files with 86 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_util_misc_test.cc
+42
View File
@@ -393,6 +393,48 @@ TEST_F(UtilMiscTest, MjuIsZeroByte) {
using InterpolationTest = MujocoTest;
TEST_F(InterpolationTest, mju_interpolate3D) {
// quadratic functions should be interpolated exactly if order = 2
auto quadratic_function_1 = [](mjtNum x, mjtNum y, mjtNum z) {
return x*x + y*y + z*z;
};
auto quadratic_function_2 = [](mjtNum x, mjtNum y, mjtNum z) {
return x*y*z + y*z*z + x*z*z;
};
auto quadratic_function_3 = [](mjtNum x, mjtNum y, mjtNum z) {
return x*y*z + y*z*z + x*z*z + y*y*z + x*x*z + x + y + z;
};
static constexpr int order = 2;
mjtNum coeff[3*(order+1)*(order+1)*(order+1)];
int index = 0;
for (int i = 0; i <= order; ++i) {
for (int j = 0; j <= order; ++j) {
for (int k = 0; k <= order; ++k) {
coeff[3*index+0] = quadratic_function_1(.5*i, .5*j, .5*k);
coeff[3*index+1] = quadratic_function_2(.5*i, .5*j, .5*k);
coeff[3*index+2] = quadratic_function_3(.5*i, .5*j, .5*k);
index++;
}
}
}
static constexpr int nsample = 5;
for (int i = 0; i < nsample; ++i) {
mjtNum sample[3];
mjtNum expected[3];
mjtNum res[3] = {0};
sample[0] = mju_Halton(i, 2);
sample[1] = mju_Halton(i, 3);
sample[2] = mju_Halton(i, 5);
expected[0] = quadratic_function_1(sample[0], sample[1], sample[2]);
expected[1] = quadratic_function_2(sample[0], sample[1], sample[2]);
expected[2] = quadratic_function_3(sample[0], sample[1], sample[2]);
mju_interpolate3D(res, sample, coeff, order);
EXPECT_NEAR(res[0], expected[0], 1e-10);
EXPECT_NEAR(res[1], expected[1], 1e-10);
EXPECT_NEAR(res[2], expected[2], 1e-10);
}
}
TEST_F(InterpolationTest, mju_defGradient) {
int order = 1;
mjtNum mat[9];