diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 7d0f2736..7d767a13 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -869,7 +869,7 @@ static void mj_advance(const mjModel* m, mjData* d, // get history buffer pointer and insert ctrl at current time mjtNum* buf = d->history + m->actuator_historyadr[i]; - *mju_delayInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[i]; + *mju_historyInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[i]; } // advance sensor history buffers @@ -888,7 +888,7 @@ static void mj_advance(const mjModel* m, mjData* d, mjtNum time_prev = buf[0]; // first slot stores previous sensor tick if (time_prev + interval <= d->time) { buf[0] += interval; // advance by exact interval (continuous time) - mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time); + mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time); if (delay > 0) { // have delay, compute sensor mj_computeSensor(m, d, i, slot); @@ -899,11 +899,11 @@ static void mj_advance(const mjModel* m, mjData* d, } } else if (delay > 0) { // delay-only mode: always compute and insert - mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time); + mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time); mj_computeSensor(m, d, i, slot); } else { // history-only mode: copy from sensordata (already computed) - mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time); + mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time); mju_copy(slot, d->sensordata + m->sensor_adr[i], dim); } } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a75bb405..2ce96372 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -831,7 +831,7 @@ mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int i mjtNum delay = m->actuator_delay[id]; const mjtNum* buf = d->history + m->actuator_historyadr[id]; mjtNum res; - const mjtNum* ptr = mju_delayRead(buf, nsample, /*dim=*/1, &res, time - delay, interp); + const mjtNum* ptr = mju_historyRead(buf, nsample, /*dim=*/1, &res, time - delay, interp); return ptr ? *ptr : res; } @@ -858,7 +858,7 @@ const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum ti int dim = m->sensor_dim[id]; mjtNum delay = m->sensor_delay[id]; const mjtNum* buf = d->history + m->sensor_historyadr[id]; - return mju_delayRead(buf, nsample, dim, result, time - delay, interp); + return mju_historyRead(buf, nsample, dim, result, time - delay, interp); } @@ -888,7 +888,7 @@ void mj_initCtrlHistory(const mjModel* m, mjData* d, int id, mjtNum user = buf[0]; // initialize history buffer - mju_delayInit(buf, nsample, 1, buf_times, values, user); + mju_historyInit(buf, nsample, 1, buf_times, values, user); } @@ -916,5 +916,5 @@ void mj_initSensorHistory(const mjModel* m, mjData* d, int id, const mjtNum* buf_times = times ? times : buf + 2; // initialize history buffer with provided phase - mju_delayInit(buf, nsample, dim, buf_times, values, phase); + mju_historyInit(buf, nsample, dim, buf_times, values, phase); } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 60d18b22..89abeed1 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -910,11 +910,11 @@ size_t mju_decodeBase64(uint8_t* buf, const char* s) { } -//------------------------------ delay buffers ----------------------------------------------------- +//------------------------------ history 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) { +static inline int historyPhysicalIndex(int cursor, int n, int logical) { return (cursor + 1 + logical) % n; } @@ -922,10 +922,10 @@ static inline int delayPhysicalIndex(int cursor, int n, int logical) { // 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) { +static int historyFindIndex(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); + int oldest_phys = historyPhysicalIndex(cursor, n, 0); + int newest_phys = historyPhysicalIndex(cursor, n, n-1); mjtNum t_oldest = times[oldest_phys]; mjtNum t_newest = times[newest_phys]; @@ -944,7 +944,7 @@ static int delayFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) { int hi = n - 1; while (hi - lo > 1) { int mid = (lo + hi) / 2; - int mid_phys = delayPhysicalIndex(cursor, n, mid); + int mid_phys = historyPhysicalIndex(cursor, n, mid); if (times[mid_phys] < t) { lo = mid; } else { @@ -956,10 +956,10 @@ static int delayFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) { } -// initialize delay buffer with given times and values; times must be strictly increasing +// initialize history 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) { +void mju_historyInit(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) { @@ -984,17 +984,17 @@ void mju_delayInit(mjtNum* buf, int n, int dim, const mjtNum* times, const mjtNu // 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) { +mjtNum* mju_historyInsert(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); + int i = historyFindIndex(times, n, cursor, t); // exact match at logical i: return pointer to existing slot if (i < n) { - int phys_i = delayPhysicalIndex(cursor, n, i); + int phys_i = historyPhysicalIndex(cursor, n, i); if (mju_abs(t - times[phys_i]) < mjMINVAL) { return values + phys_i*dim; } @@ -1002,7 +1002,7 @@ mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t) { // logical i == 0: new sample is older than oldest, replace oldest slot if (i == 0) { - int oldest_phys = delayPhysicalIndex(cursor, n, 0); + int oldest_phys = historyPhysicalIndex(cursor, n, 0); times[oldest_phys] = t; return values + oldest_phys*dim; } @@ -1019,12 +1019,12 @@ mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t) { // 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); + int src_phys = historyPhysicalIndex(cursor, n, j+1); + int dst_phys = historyPhysicalIndex(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); + int insert_phys = historyPhysicalIndex(cursor, n, i-1); times[insert_phys] = t; return values + insert_phys*dim; } @@ -1033,13 +1033,13 @@ 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 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) { +const mjtNum* mju_historyRead(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); + int oldest_phys = historyPhysicalIndex(cursor, n, 0); + int newest_phys = historyPhysicalIndex(cursor, n, n-1); mjtNum t_oldest = times[oldest_phys]; mjtNum t_newest = times[newest_phys]; @@ -1054,8 +1054,8 @@ const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtN } // find bracketing logical index: times[i-1] < t <= times[i] - int i = delayFindIndex(times, n, cursor, t); - int phys_i = delayPhysicalIndex(cursor, n, i); + int i = historyFindIndex(times, n, cursor, t); + int phys_i = historyPhysicalIndex(cursor, n, i); // check for exact match at i if (mju_abs(t - times[phys_i]) < mjMINVAL) { @@ -1063,7 +1063,7 @@ const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtN } // lo = i-1, hi = i (we know i > 0 because t > t_oldest) - int phys_lo = delayPhysicalIndex(cursor, n, i-1); + int phys_lo = historyPhysicalIndex(cursor, n, i-1); int phys_hi = phys_i; // zero-order hold: return pointer to lo (most recent sample <= t) @@ -1096,14 +1096,14 @@ const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtN mjtNum m_lo = 0; if (i > 1) { - int phys_lo_prev = delayPhysicalIndex(cursor, n, i-2); + int phys_lo_prev = historyPhysicalIndex(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); + int phys_hi_next = historyPhysicalIndex(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; } diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 28e726e2..651d1524 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -89,7 +89,7 @@ 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); -//------------------------------ delay buffers ----------------------------------------------------- +//------------------------------ history buffers --------------------------------------------------- // buffer layout: [user(1), cursor(1), times(n), values(n*dim)] // - user: 1 mjtNum reserved for user data (ignored by these functions) @@ -98,20 +98,20 @@ MJAPI size_t mju_decodeBase64(uint8_t* buf, const char* s); // - 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 +// initialize history 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); +MJAPI void mju_historyInit(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); +MJAPI mjtNum* mju_historyInsert(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); +MJAPI const mjtNum* mju_historyRead(const mjtNum* buf, int n, int dim, + mjtNum* res, mjtNum t, int interp); //------------------------------ miscellaneous ----------------------------------------------------- diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 16e3ed89..8610328b 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/test/engine/engine_util_misc_test.cc @@ -724,22 +724,22 @@ TEST_F(Base64Test, decodeAndEncode) { EXPECT_THAT(buffer2.data(), StrEq(s)); } -// --------------------------------- Delay Buffers ---------------------------- +// --------------------------------- History Buffers --------------------------- -using DelayTest = MujocoTest; +using HistoryTest = 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) { +TEST_F(HistoryTest, 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); + mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0); // check header EXPECT_EQ(buf[0], 0.0); // user @@ -749,30 +749,30 @@ TEST_F(DelayTest, Init) { // 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); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 99.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 99.0); } -TEST_F(DelayTest, Init_Vector) { +TEST_F(HistoryTest, 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); + mju_historyInit(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); + const mjtNum* ptr = mju_historyRead(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) { +TEST_F(HistoryTest, Append) { constexpr int n = 4; constexpr int dim = 1; // Initialize buffer properly, then insert @@ -786,26 +786,26 @@ TEST_F(DelayTest, Append) { 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; + *mju_historyInsert(buf, n, dim, 4.0) = 1.0; + *mju_historyInsert(buf, n, dim, 6.0) = 2.0; + *mju_historyInsert(buf, n, dim, 8.0) = 3.0; + *mju_historyInsert(buf, n, dim, 10.0) = 4.0; // now append at t=12 - *mju_delayInsert(buf, n, dim, 12.0) = 99.0; + *mju_historyInsert(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); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); + EXPECT_EQ(*mju_historyRead(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); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 2.0); } -TEST_F(DelayTest, Append_Multiple) { +TEST_F(HistoryTest, Append_Multiple) { constexpr int n = 3; constexpr int dim = 1; mjtNum buf[2 + 2*n]; @@ -817,16 +817,16 @@ TEST_F(DelayTest, Append_Multiple) { for (int i = 1; i <= 4; i++) { mjtNum i_real = static_cast(i); - *mju_delayInsert(buf, n, dim, i_real) = i_real; + *mju_historyInsert(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); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 2.0, 0), 2.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 3.0, 0), 3.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 4.0); } -TEST_F(DelayTest, ReadVector_ExactMatch) { +TEST_F(HistoryTest, ReadVector_ExactMatch) { constexpr int n = 3; constexpr int dim = 2; mjtNum buf[2 + n + n*dim]; @@ -837,21 +837,21 @@ TEST_F(DelayTest, ReadVector_ExactMatch) { 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); + mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); slot0[0] = 1.0; slot0[1] = 2.0; - mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0); + mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); slot1[0] = 3.0; slot1[1] = 4.0; - mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0); + mjtNum* slot2 = mju_historyInsert(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); + const mjtNum* ptr = mju_historyRead(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) { +TEST_F(HistoryTest, ReadVector_ZOH) { constexpr int n = 3; constexpr int dim = 2; mjtNum buf[2 + n + n*dim]; @@ -861,21 +861,21 @@ TEST_F(DelayTest, ReadVector_ZOH) { mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n*dim); - mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0); + mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); slot0[0] = 1.0; slot0[1] = 2.0; - mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0); + mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); slot1[0] = 3.0; slot1[1] = 4.0; - mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0); + mjtNum* slot2 = mju_historyInsert(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); + const mjtNum* ptr = mju_historyRead(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) { +TEST_F(HistoryTest, ReadVector_Linear) { constexpr int n = 3; constexpr int dim = 2; mjtNum buf[2 + n + n*dim]; @@ -885,21 +885,21 @@ TEST_F(DelayTest, ReadVector_Linear) { mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n*dim); - mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0); + mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); slot0[0] = 1.0; slot0[1] = 2.0; - mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0); + mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); slot1[0] = 3.0; slot1[1] = 4.0; - mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0); + mjtNum* slot2 = mju_historyInsert(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); + const mjtNum* ptr = mju_historyRead(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) { +TEST_F(HistoryTest, InsertOutOfOrder) { constexpr int n = 4; constexpr int dim = 1; mjtNum buf[2 + 2*n]; @@ -912,41 +912,41 @@ TEST_F(DelayTest, InsertOutOfOrder) { 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; + *mju_historyInsert(buf, n, dim, 4.0) = 1.0; + *mju_historyInsert(buf, n, dim, 6.0) = 2.0; + *mju_historyInsert(buf, n, dim, 8.0) = 3.0; + *mju_historyInsert(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; + *mju_historyInsert(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); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 9.0, 0), 99.0); + EXPECT_EQ(*mju_historyRead(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; + *mju_historyInsert(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); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 5.0, 0), 99.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_historyRead(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; + *mju_historyInsert(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); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 3.0, 0), 99.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); } -TEST_F(DelayTest, InsertReplaceOnCollision) { +TEST_F(HistoryTest, InsertReplaceOnCollision) { constexpr int n = 4; constexpr int dim = 1; mjtNum buf[2 + 2*n]; @@ -960,50 +960,50 @@ TEST_F(DelayTest, InsertReplaceOnCollision) { 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; + *mju_historyInsert(buf, n, dim, 4.0) = 1.0; + *mju_historyInsert(buf, n, dim, 6.0) = 2.0; + *mju_historyInsert(buf, n, dim, 8.0) = 3.0; + *mju_historyInsert(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); + *mju_historyInsert(buf, n, dim, 8.0) = 99.0; + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 1.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 99.0); + EXPECT_EQ(*mju_historyRead(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); + *mju_historyInsert(buf, n, dim, 10.0) = 99.0; + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 1.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_historyRead(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); + *mju_historyInsert(buf, n, dim, 4.0) = 99.0; + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 99.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); + EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); } -void TriggerDelayInitNonMonotonic() { +void TriggerHistoryInitNonMonotonic() { 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); + mju_historyInit(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(HistoryTest, Init_NonMonotonic) { + EXPECT_FATAL_FAILURE(TriggerHistoryInitNonMonotonic(), + "mju_historyInit: times must be strictly increasing"); } -TEST_F(DelayTest, CubicInterpolation) { +TEST_F(HistoryTest, CubicInterpolation) { int n = 2; int dim = 2; mjtNum buf[100]; // 2 + 2 + 2*2 = 8 @@ -1016,9 +1016,9 @@ TEST_F(DelayTest, CubicInterpolation) { // 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); + mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); slot0[0] = 0.0; slot0[1] = 1.0; - mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0); + mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); slot1[0] = 1.0; slot1[1] = 0.0; mjtNum res[2]; @@ -1026,14 +1026,14 @@ TEST_F(DelayTest, CubicInterpolation) { // 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); + mju_historyRead(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); + mju_historyRead(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); @@ -1041,7 +1041,7 @@ TEST_F(DelayTest, CubicInterpolation) { // 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); + mju_historyRead(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);