diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 69296b00..60d18b22 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -910,6 +910,215 @@ size_t mju_decodeBase64(uint8_t* buf, const char* s) { } +//------------------------------ delay buffers ----------------------------------------------------- + +// convert logical index (0=oldest, n-1=newest) to physical index +// cursor points to the newest element (logical index n-1) +static inline int delayPhysicalIndex(int cursor, int n, int logical) { + return (cursor + 1 + logical) % n; +} + + +// find logical index i such that times[i-1] < t <= times[i], using circular binary search +// returns 0 if t <= times[oldest], n if t > times[newest] +// cursor points to the newest element (logical index n-1) +static int delayFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) { + // get oldest and newest timestamps + int oldest_phys = delayPhysicalIndex(cursor, n, 0); + int newest_phys = delayPhysicalIndex(cursor, n, n-1); + mjtNum t_oldest = times[oldest_phys]; + mjtNum t_newest = times[newest_phys]; + + // before or at first element + if (t <= t_oldest) { + return 0; + } + + // after last element + if (t > t_newest) { + return n; + } + + // circular binary search: find smallest logical i such that times[phys(i)] >= t + int lo = 0; + int hi = n - 1; + while (hi - lo > 1) { + int mid = (lo + hi) / 2; + int mid_phys = delayPhysicalIndex(cursor, n, mid); + if (times[mid_phys] < t) { + lo = mid; + } else { + hi = mid; + } + } + + return hi; +} + + +// initialize delay buffer with given times and values; times must be strictly increasing +// buffer layout: [user(1), cursor(1), times(n), values(n*dim)] +void mju_delayInit(mjtNum* buf, int n, int dim, const mjtNum* times, const mjtNum* values, + mjtNum user) { + // check strict monotonicity of times + for (int i = 0; i < n-1; i++) { + if (times[i+1] - times[i] < mjMINVAL) { + mjERROR("times must be strictly increasing, got times[%d]=%g >= times[%d]=%g", + i, times[i], i+1, times[i+1]); + } + } + + // buf layout: [user(1), cursor(1), times(n), values(n*dim)] + buf[0] = user; // user value + buf[1] = (mjtNum)(n-1); // cursor points to newest (logical index n-1 = physical index n-1) + + mjtNum* buf_times = buf + 2; + mjtNum* buf_values = buf + 2 + n; + + if (times != buf_times) mju_copy(buf_times, times, n); + if (values) mju_copy(buf_values, values, n*dim); +} + + +// find insertion slot for time t, maintaining sorted order +// if t matches an existing timestamp, returns pointer to that slot +// if a new sample is inserted, the oldest sample is dropped +// returns pointer to value slot where caller should write dim values +mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t) { + int cursor = (int)buf[1]; + mjtNum* times = buf + 2; + mjtNum* values = buf + 2 + n; + + // find logical insertion index: times[i-1] < t <= times[i] + int i = delayFindIndex(times, n, cursor, t); + + // exact match at logical i: return pointer to existing slot + if (i < n) { + int phys_i = delayPhysicalIndex(cursor, n, i); + if (mju_abs(t - times[phys_i]) < mjMINVAL) { + return values + phys_i*dim; + } + } + + // logical i == 0: new sample is older than oldest, replace oldest slot + if (i == 0) { + int oldest_phys = delayPhysicalIndex(cursor, n, 0); + times[oldest_phys] = t; + return values + oldest_phys*dim; + } + + // logical i == n: new sample is newer than newest, advance cursor and write + if (i == n) { + cursor = (cursor + 1) % n; + buf[1] = (mjtNum)cursor; + + // cursor now points to the new newest slot (which was the old oldest) + times[cursor] = t; + return values + cursor*dim; + } + + // 0 < i < n: out-of-order insertion, shift [1, i-1] left (dropping 0), insert at i-1 + for (int j = 0; j < i-1; j++) { + int src_phys = delayPhysicalIndex(cursor, n, j+1); + int dst_phys = delayPhysicalIndex(cursor, n, j); + times[dst_phys] = times[src_phys]; + mju_copy(values + dst_phys*dim, values + src_phys*dim, dim); + } + int insert_phys = delayPhysicalIndex(cursor, n, i-1); + times[insert_phys] = t; + return values + insert_phys*dim; +} + + +// read vector value at time t; interp: 0=zero-order-hold, 1=linear, 2=cubic spline +// returns pointer to sample in buffer on exact match or ZOH (res untouched) +// returns NULL and writes interpolated result to res on interpolation +const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtNum t, int interp) { + int cursor = (int)buf[1]; + const mjtNum* times = buf + 2; + const mjtNum* values = buf + 2 + n; + + int oldest_phys = delayPhysicalIndex(cursor, n, 0); + int newest_phys = delayPhysicalIndex(cursor, n, n-1); + mjtNum t_oldest = times[oldest_phys]; + mjtNum t_newest = times[newest_phys]; + + // extrapolate before oldest: return pointer to oldest value + if (t <= t_oldest + mjMINVAL) { + return values + oldest_phys*dim; + } + + // extrapolate after newest: return pointer to newest value + if (t >= t_newest - mjMINVAL) { + return values + newest_phys*dim; + } + + // find bracketing logical index: times[i-1] < t <= times[i] + int i = delayFindIndex(times, n, cursor, t); + int phys_i = delayPhysicalIndex(cursor, n, i); + + // check for exact match at i + if (mju_abs(t - times[phys_i]) < mjMINVAL) { + return values + phys_i*dim; + } + + // lo = i-1, hi = i (we know i > 0 because t > t_oldest) + int phys_lo = delayPhysicalIndex(cursor, n, i-1); + int phys_hi = phys_i; + + // zero-order hold: return pointer to lo (most recent sample <= t) + if (interp == 0) { + return values + phys_lo*dim; + } + + mjtNum dt = times[phys_hi] - times[phys_lo]; + mjtNum alpha = (t - times[phys_lo]) / dt; + + // piecewise linear interpolation + if (interp == 1) { + for (int d = 0; d < dim; d++) { + res[d] = values[phys_lo*dim+d] + alpha * (values[phys_hi*dim+d] - values[phys_lo*dim+d]); + } + } + + // cubic spline interpolation + else { + // Hermite basis functions + mjtNum alpha2 = alpha * alpha; + mjtNum alpha3 = alpha2 * alpha; + mjtNum h00 = 2*alpha3 - 3*alpha2 + 1; + mjtNum h10 = alpha3 - 2*alpha2 + alpha; + mjtNum h01 = -2*alpha3 + 3*alpha2; + mjtNum h11 = alpha3 - alpha2; + + for (int d = 0; d < dim; d++) { + // finite differenced catmull-rom slopes, 0 at endpoints (constant extrapolation) + + mjtNum m_lo = 0; + if (i > 1) { + int phys_lo_prev = delayPhysicalIndex(cursor, n, i-2); + mjtNum dt_lo = times[phys_hi] - times[phys_lo_prev]; + m_lo = (values[phys_hi*dim+d] - values[phys_lo_prev*dim+d]) / dt_lo; + } + + mjtNum m_hi = 0; + if (i < n - 1) { + int phys_hi_next = delayPhysicalIndex(cursor, n, i+1); + mjtNum dt_hi = times[phys_hi_next] - times[phys_lo]; + m_hi = (values[phys_hi_next*dim+d] - values[phys_lo*dim+d]) / dt_hi; + } + + res[d] = h00 * values[phys_lo*dim+d] + + h10 * dt * m_lo + + h01 * values[phys_hi*dim+d] + + h11 * dt * m_hi; + } + } + + return NULL; +} + + //------------------------------ miscellaneous ----------------------------------------------------- // convert contact force to pyramid representation diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 798657cf..28e726e2 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -75,7 +75,7 @@ MJAPI mjtNum mju_evalBasis(const mjtNum x[3], int i, int order); // interpolate a function at x with given interpolation coefficients and order n MJAPI void mju_interpolate3D(mjtNum res[3], const mjtNum x[3], const mjtNum* coeff, int order); -// ----------------------------- Base64 ----------------------------------------------------------- +// ----------------------------- Base64 ------------------------------------------------------------ // encode data as Base64 into buf (including padding and null char) // returns number of chars written in buf: 4 * [(ndata + 2) / 3] + 1 @@ -89,7 +89,31 @@ MJAPI size_t mju_isValidBase64(const char* s); // returns number of bytes decoded (upper limit of 3 * (strlen(s) / 4)) MJAPI size_t mju_decodeBase64(uint8_t* buf, const char* s); -//------------------------------ miscellaneous ---------------------------------------------------- +//------------------------------ delay buffers ----------------------------------------------------- + +// buffer layout: [user(1), cursor(1), times(n), values(n*dim)] +// - user: 1 mjtNum reserved for user data (ignored by these functions) +// - cursor: 1 mjtNum for circular buffer index (integer stored as mjtNum) +// - times: n timestamps, contiguous at buf[2..n+1] +// - values: n*dim values, contiguous at buf[n+2..n+2+n*dim-1] +// total buffer size: 2 + n*(1 + dim) + +// initialize delay buffer with given times and values; times must be strictly increasing +// values is size n x dim +MJAPI void mju_delayInit(mjtNum* buf, int n, int dim, const mjtNum* times, + const mjtNum* values, mjtNum user); + +// find insertion slot for sample at time t, maintaining sorted order +// returns pointer to value slot (size dim) where caller should write +MJAPI mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t); + +// read vector value at time t; interp: 0=zero-order-hold, 1=linear, 2=cubic spline +// returns pointer to sample in buffer on exact match (res untouched) +// returns NULL and writes interpolated result to res otherwise +MJAPI const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, + mjtNum* res, mjtNum t, int interp); + +//------------------------------ miscellaneous ----------------------------------------------------- // convert contact force to pyramid representation MJAPI void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 6e9012fc..16e3ed89 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/test/engine/engine_util_misc_test.cc @@ -15,6 +15,7 @@ // Tests for engine/engine_util_solve.c. #include +#include #include #include #include @@ -22,6 +23,7 @@ #include #include +#include #include #include #include "src/engine/engine_util_misc.h" @@ -722,5 +724,328 @@ TEST_F(Base64Test, decodeAndEncode) { EXPECT_THAT(buffer2.data(), StrEq(s)); } +// --------------------------------- Delay Buffers ---------------------------- + +using DelayTest = MujocoTest; + +// buffer layout: [user(1), cursor(1), times(n), values(n*dim)] +// cursor points to newest element (logical index n-1) +// after init, cursor=n-1, so physical indices equal logical indices + +TEST_F(DelayTest, Init) { + constexpr int n = 4; + constexpr int dim = 1; + mjtNum buf[2 + n + n*dim]; + + std::vector times = {4, 6, 8, 10}; + std::vector values = {99, 99, 99, 99}; + mju_delayInit(buf, n, dim, times.data(), values.data(), 0.0); + + // check header + EXPECT_EQ(buf[0], 0.0); // user + EXPECT_EQ(buf[1], static_cast(n-1)); // cursor = n-1 + + // timestamps: [4, 6, 8, 10] (t=10 is newest) + // values: [99, 99, 99, 99] + // verify via read function (logical order) + mjtNum res; + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 99.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 99.0); +} + +TEST_F(DelayTest, Init_Vector) { + constexpr int n = 3; + constexpr int dim = 2; + mjtNum buf[2 + n + n*dim]; + + std::vector times = {-2, -1, 0}; + std::vector values = {1.0, 2.0, 1.0, 2.0, 1.0, 2.0}; + mju_delayInit(buf, n, dim, times.data(), values.data(), 0.0); + + EXPECT_EQ(buf[1], static_cast(n-1)); // cursor = n-1 + + // verify via read function + mjtNum res[dim]; + const mjtNum* ptr = mju_delayRead(buf, n, dim, res, -2.0, 0); + ASSERT_NE(ptr, nullptr); + EXPECT_EQ(ptr[0], 1.0); + EXPECT_EQ(ptr[1], 2.0); +} + +TEST_F(DelayTest, Append) { + constexpr int n = 4; + constexpr int dim = 1; + // Initialize buffer properly, then insert + mjtNum buf[2 + 2*n]; + buf[0] = 0.0; + buf[1] = n - 1; + // timestamps: [4, 6, 8, 10] + mjtNum times[] = {4, 6, 8, 10}; + mju_copy(buf + 2, times, n); + // values: [0, 0, 0, 0] + mju_zero(buf + 2 + n, n); + + // overwrite with specific values + *mju_delayInsert(buf, n, dim, 4.0) = 1.0; + *mju_delayInsert(buf, n, dim, 6.0) = 2.0; + *mju_delayInsert(buf, n, dim, 8.0) = 3.0; + *mju_delayInsert(buf, n, dim, 10.0) = 4.0; + + // now append at t=12 + *mju_delayInsert(buf, n, dim, 12.0) = 99.0; + + // verify logical order: [6, 8, 10, 12] -> [2, 3, 4, 99] + mjtNum res; + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 12.0, 0), 99.0); + + // oldest should now be t=6 + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 2.0); +} + +TEST_F(DelayTest, Append_Multiple) { + constexpr int n = 3; + constexpr int dim = 1; + mjtNum buf[2 + 2*n]; + buf[0] = 0.0; + buf[1] = n - 1; + mjtNum times[] = {-2, -1, 0}; + mju_copy(buf + 2, times, n); + mju_zero(buf + 2 + n, n); + + for (int i = 1; i <= 4; i++) { + mjtNum i_real = static_cast(i); + *mju_delayInsert(buf, n, dim, i_real) = i_real; + } + // Final: logical timestamps [2, 3, 4], values [2, 3, 4] + mjtNum res; + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 2.0, 0), 2.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 3.0, 0), 3.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 4.0); +} + +TEST_F(DelayTest, ReadVector_ExactMatch) { + constexpr int n = 3; + constexpr int dim = 2; + mjtNum buf[2 + n + n*dim]; + buf[0] = 0.0; + buf[1] = n - 1; + mjtNum times[] = {0, 1, 2}; + mju_copy(buf + 2, times, n); + mju_zero(buf + 2 + n, n*dim); + + // set values: t=0->(1,2), t=1->(3,4), t=2->(5,6) + mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0); + slot0[0] = 1.0; slot0[1] = 2.0; + mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0); + slot1[0] = 3.0; slot1[1] = 4.0; + mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0); + slot2[0] = 5.0; slot2[1] = 6.0; + + mjtNum res[dim]; + const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 1.0, 0); + ASSERT_NE(ptr, nullptr); + EXPECT_EQ(ptr[0], 3.0); + EXPECT_EQ(ptr[1], 4.0); +} + +TEST_F(DelayTest, ReadVector_ZOH) { + constexpr int n = 3; + constexpr int dim = 2; + mjtNum buf[2 + n + n*dim]; + buf[0] = 0.0; + buf[1] = n - 1; + mjtNum times[] = {0, 1, 2}; + mju_copy(buf + 2, times, n); + mju_zero(buf + 2 + n, n*dim); + + mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0); + slot0[0] = 1.0; slot0[1] = 2.0; + mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0); + slot1[0] = 3.0; slot1[1] = 4.0; + mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0); + slot2[0] = 5.0; slot2[1] = 6.0; + + mjtNum res[dim]; + const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 0.5, 0); + ASSERT_NE(ptr, nullptr); + EXPECT_EQ(ptr[0], 1.0); + EXPECT_EQ(ptr[1], 2.0); +} + +TEST_F(DelayTest, ReadVector_Linear) { + constexpr int n = 3; + constexpr int dim = 2; + mjtNum buf[2 + n + n*dim]; + buf[0] = 0.0; + buf[1] = n - 1; + mjtNum times[] = {0, 1, 2}; + mju_copy(buf + 2, times, n); + mju_zero(buf + 2 + n, n*dim); + + mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0); + slot0[0] = 1.0; slot0[1] = 2.0; + mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0); + slot1[0] = 3.0; slot1[1] = 4.0; + mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0); + slot2[0] = 5.0; slot2[1] = 6.0; + + mjtNum res[dim]; + const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 0.5, 1); + EXPECT_EQ(ptr, nullptr); + EXPECT_THAT(res[0], DoubleNear(2.0, 1e-10)); // (1+3)/2 + EXPECT_THAT(res[1], DoubleNear(3.0, 1e-10)); // (2+4)/2 +} + +TEST_F(DelayTest, InsertOutOfOrder) { + constexpr int n = 4; + constexpr int dim = 1; + mjtNum buf[2 + 2*n]; + mjtNum res; + + auto reset = [&]() { + buf[0] = 0.0; + buf[1] = n - 1; + mjtNum times[] = {4, 6, 8, 10}; + mju_copy(buf + 2, times, n); + mju_zero(buf + 2 + n, n); + + *mju_delayInsert(buf, n, dim, 4.0) = 1.0; + *mju_delayInsert(buf, n, dim, 6.0) = 2.0; + *mju_delayInsert(buf, n, dim, 8.0) = 3.0; + *mju_delayInsert(buf, n, dim, 10.0) = 4.0; + }; + + // insert in middle (between t=8 and t=10) + reset(); + *mju_delayInsert(buf, n, dim, 9.0) = 99.0; + // logical: [6, 8, 9, 10] -> [2, 3, 99, 4] + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 9.0, 0), 99.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0); + + // insert near start (between t=4 and t=6) + reset(); + *mju_delayInsert(buf, n, dim, 5.0) = 99.0; + // logical: [5, 6, 8, 10] -> [99, 2, 3, 4] + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 5.0, 0), 99.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0); + + // insert before oldest (t=3 < t=4): replaces oldest + reset(); + *mju_delayInsert(buf, n, dim, 3.0) = 99.0; + // logical: [3, 6, 8, 10] -> [99, 2, 3, 4] + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 3.0, 0), 99.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0); +} + +TEST_F(DelayTest, InsertReplaceOnCollision) { + constexpr int n = 4; + constexpr int dim = 1; + mjtNum buf[2 + 2*n]; + mjtNum res; + + auto reset = [&]() { + // timestamps: [4, 6, 8, 10], values initialized to 0 + buf[0] = 0.0; + buf[1] = n - 1; + mjtNum times[] = {4, 6, 8, 10}; + mju_copy(buf + 2, times, n); + mju_zero(buf + 2 + n, n); + + *mju_delayInsert(buf, n, dim, 4.0) = 1.0; + *mju_delayInsert(buf, n, dim, 6.0) = 2.0; + *mju_delayInsert(buf, n, dim, 8.0) = 3.0; + *mju_delayInsert(buf, n, dim, 10.0) = 4.0; + }; + + // collision in middle (t=8) + reset(); + *mju_delayInsert(buf, n, dim, 8.0) = 99.0; + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 1.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 99.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0); + + // collision at newest (t=10) + reset(); + *mju_delayInsert(buf, n, dim, 10.0) = 99.0; + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 1.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 99.0); + + // collision at oldest (t=4) + reset(); + *mju_delayInsert(buf, n, dim, 4.0) = 99.0; + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 99.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0); +} + +void TriggerDelayInitNonMonotonic() { + mjtNum buf[10]; + mjtNum times[4] = {1, 2, 2, 4}; // not strictly increasing + mjtNum values[4] = {0}; + mju_delayInit(buf, 4, 1, times, values, 0.0); +} + +TEST_F(DelayTest, Init_NonMonotonic) { + EXPECT_FATAL_FAILURE(TriggerDelayInitNonMonotonic(), + "mju_delayInit: times must be strictly increasing"); +} + +TEST_F(DelayTest, CubicInterpolation) { + int n = 2; + int dim = 2; + mjtNum buf[100]; // 2 + 2 + 2*2 = 8 + buf[0] = 0.0; + buf[1] = n - 1; + mjtNum times[] = {-1, 0}; + mju_copy(buf + 2, times, n); + mju_zero(buf + 2 + n, n*dim); + + // Insert (0, 0, 1) and (1, 1, 0). + // Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3 + // Dim 1: 1 -> 0. Spline: p(x) = 1 - 3x^2 + 2x^3 + mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0); + slot0[0] = 0.0; slot0[1] = 1.0; + mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0); + slot1[0] = 1.0; slot1[1] = 0.0; + + mjtNum res[2]; + + // Test midpoint x=0.5 + // Dim 0: 0.5 + // Dim 1: 1 - 0.5 = 0.5 + mju_delayRead(buf, n, dim, res, 0.5, 2); + EXPECT_NEAR(res[0], 0.5, 1e-9); + EXPECT_NEAR(res[1], 0.5, 1e-9); + + // Test x=0.25 + // Dim 0: 3*0.25^2 - 2*0.25^3 + // Dim 1: 1 - (3*0.25^2 - 2*0.25^3) + mju_delayRead(buf, n, dim, res, 0.25, 2); + mjtNum expected_0_25 = 3*0.25*0.25 - 2*0.25*0.25*0.25; + EXPECT_NEAR(res[0], expected_0_25, 1e-9); + EXPECT_NEAR(res[1], 1.0 - expected_0_25, 1e-9); + + // Test x=0.8 + // Dim 0: 3*0.8^2 - 2*0.8^3 + // Dim 1: 1 - (3*0.8^2 - 2*0.8^3) + mju_delayRead(buf, n, dim, res, 0.8, 2); + mjtNum expected_0_8 = 3*0.8*0.8 - 2*0.8*0.8*0.8; + EXPECT_NEAR(res[0], expected_0_8, 1e-9); + EXPECT_NEAR(res[1], 1.0 - expected_0_8, 1e-9); +} + } // namespace } // namespace mujoco